[med-svn] [opensurgsim] 01/14: New upstream version 0.7.0
Paul Novotny
paulnovo-guest at moszumanska.debian.org
Sun Oct 16 21:01:29 UTC 2016
This is an automated email from the git hooks/post-receive script.
paulnovo-guest pushed a commit to branch master
in repository opensurgsim.
commit 179b713810751038d35a5f041a095343d8de8659
Author: Paul Novotny <paul at paulnovo.us>
Date: Fri Oct 14 10:03:10 2016 -0400
New upstream version 0.7.0
---
.gitignore | 1 -
CMake/External_yamlcpp.cmake | 47 +-
CMake/FindLabJack.cmake | 2 +-
CMake/FindLeapSdk.cmake | 52 +
CMake/FindMathJax.cmake | 38 +
CMake/FindNovintHdalSdk.cmake | 51 +-
CMake/FindOculusSdk.cmake | 132 +
CMake/FindOpenNI2.cmake | 117 +
CMake/FindOptiTrack.cmake | 2 +-
CMake/FindSixenseSdk.cmake | 111 +-
CMake/Library.h.in | 23 +
CMake/OpenSurgSimConfig.cmake.in | 2 +-
CMake/SurgSimBuildFlags.cmake | 17 +-
CMake/SurgSimUtilities.cmake | 88 +-
CMakeLists.txt | 49 +-
Data/Fonts/COPYRIGHT.TXT | 57 +
Data/Fonts/Vera.ttf | Bin 0 -> 65932 bytes
Data/Shaders/bilateral_blur.frag | 68 +
Data/Shaders/depth_map.frag | 10 +-
Data/Shaders/dns_mapping_material.frag | 78 +
Data/Shaders/dns_mapping_material.vert | 87 +
Data/Shaders/dns_mapping_material_twosided.frag | 87 +
Data/Shaders/dns_mapping_multitexture.frag | 81 +
Data/Shaders/ds_mapping_material.frag | 17 +-
Data/Shaders/ds_mapping_material.vert | 4 +-
Data/Shaders/ds_mapping_material_twosided.frag | 85 +
Data/Shaders/ds_mapping_multitexture.frag | 61 +
Data/Shaders/gauss_blur.frag | 52 +
Data/Shaders/gauss_blur_horizontal.vert | 43 +
Data/Shaders/gauss_blur_vertical.vert | 44 +
Data/Shaders/implicit_surface/depth.frag | 58 +
Data/Shaders/implicit_surface/depth.vert | 51 +
Data/Shaders/implicit_surface/normal.frag | 82 +
Data/Shaders/implicit_surface/normal.vert | 28 +
Data/Shaders/implicit_surface/shading.frag | 130 +
Data/Shaders/implicit_surface/shading.vert | 34 +
Data/Shaders/material.frag | 19 +-
Data/Shaders/material.vert | 37 +-
Data/Shaders/material_curve.vert | 89 +
Data/Shaders/material_multitexture.frag | 55 +
Data/Shaders/s_mapping_material.frag | 60 +
...pping_material.vert => s_mapping_material.vert} | 0
Data/Shaders/s_mapping_metal.frag | 84 +
Data/Shaders/s_mapping_metal.vert | 64 +
Data/Shaders/shadow_map.frag | 24 +-
Data/Shaders/shadow_map.vert | 19 +-
Data/Shaders/skinning.vert | 76 +
Data/Shaders/unlit_text.frag | 29 +
Data/Shaders/unlit_texture_rectangle.frag | 19 +
Data/devices.yaml | 8 +
Documentation/CMakeLists.txt | 33 +-
Documentation/Doxyfile.in | 11 +-
Documentation/mainPage.dox | 50 -
Examples/AddSphereFromInput/AddSphereBehavior.h | 8 +-
Examples/AddSphereFromInput/AddSphereFromInput.cpp | 44 +-
Examples/AddSphereFromInput/CMakeLists.txt | 4 +-
Examples/CMakeLists.txt | 13 +-
Examples/DroppingBalls/AddRandomSphereBehavior.h | 6 +-
Examples/DroppingBalls/CMakeLists.txt | 4 +-
Examples/DroppingBalls/DroppingBalls.cpp | 45 +-
Examples/GraphicsScene/CMakeLists.txt | 6 +-
Examples/GraphicsScene/Data/StereoView.yaml | 35 +
Examples/GraphicsScene/GraphicsScene.cpp | 306 +-
Examples/GraphicsScene/config.txt.in | 2 +-
Examples/InputVtc/CMakeLists.txt | 24 +-
Examples/InputVtc/Data/Device.yaml | 13 +
Examples/InputVtc/DeviceFactory.cpp | 73 -
Examples/InputVtc/DeviceFactory.h | 45 -
Examples/InputVtc/InputVtc.cpp | 113 +-
Examples/InputVtc/config.txt.in | 2 +
Examples/ShowScenery/CMakeLists.txt | 50 +
Examples/ShowScenery/ShowScenery.cpp | 141 +
Examples/ShowScenery/config.txt.in | 1 +
Examples/Stapling/CMakeLists.txt | 29 +-
Examples/Stapling/Data/StaplingDemo.yaml | 527 +-
Examples/Stapling/SerializedStapling.cpp | 60 +-
Examples/Stapling/StapleElement.cpp | 3 +-
Examples/Stapling/StapleElement.h | 2 +-
Examples/Stapling/StaplerBehavior.cpp | 128 +-
Examples/Stapling/StaplerBehavior.h | 22 +-
Examples/Stapling/Stapling.cpp | 127 +-
Modules/CMakeLists.txt | 30 +
Modules/README | 6 +
NOTICE | 22 +-
README | 143 -
README.md | 154 +
SurgSim/Blocks/CMakeLists.txt | 32 +-
SurgSim/Blocks/CompoundShapeToGraphics.cpp | 151 +
SurgSim/Blocks/CompoundShapeToGraphics.h | 115 +
SurgSim/Blocks/DebugDumpBehavior.cpp | 166 +
SurgSim/Blocks/DebugDumpBehavior.h | 80 +
SurgSim/Blocks/DriveElementFromInputBehavior.cpp | 13 +-
SurgSim/Blocks/FunctionBehavior.cpp | 76 +
SurgSim/Blocks/FunctionBehavior.h | 107 +
SurgSim/Blocks/GraphicsUtilities.cpp | 246 +
SurgSim/Blocks/GraphicsUtilities.h | 126 +
SurgSim/Blocks/ImplicitSurface.cpp | 358 +
SurgSim/Blocks/ImplicitSurface.h | 73 +
SurgSim/Blocks/KeyBehavior.cpp | 141 +
SurgSim/Blocks/KeyBehavior.h | 96 +
SurgSim/Blocks/KeyboardCallbackBehavior.cpp | 101 +
SurgSim/Blocks/KeyboardCallbackBehavior.h | 92 +
.../Blocks/KeyboardTogglesComponentBehavior.cpp | 17 +-
SurgSim/Blocks/KeyboardTogglesComponentBehavior.h | 10 +-
SurgSim/Blocks/MassSpring1DRepresentation.cpp | 61 +-
SurgSim/Blocks/MassSpring1DRepresentation.h | 3 -
SurgSim/Blocks/PoseInterpolator.cpp | 15 +-
SurgSim/Blocks/PoseInterpolator.h | 19 +-
SurgSim/Blocks/ShadowMapping.cpp | 280 +
SurgSim/Blocks/ShadowMapping.h | 81 +
SurgSim/Blocks/SingleKeyBehavior.cpp | 88 +
SurgSim/Blocks/SingleKeyBehavior.h | 74 +
SurgSim/Blocks/SphereElement.cpp | 36 +-
SurgSim/Blocks/SphereElement.h | 18 +-
.../TransferParticlesToPointCloudBehavior.cpp | 100 +
.../Blocks/TransferParticlesToPointCloudBehavior.h | 86 +
.../TransferPhysicsToGraphicsMeshBehavior.cpp | 211 +-
.../Blocks/TransferPhysicsToGraphicsMeshBehavior.h | 65 +-
.../Blocks/TransferPhysicsToPointCloudBehavior.cpp | 20 +-
.../Blocks/TransferPhysicsToPointCloudBehavior.h | 6 +-
.../Blocks/TransferPhysicsToVerticesBehavior.cpp | 90 +
SurgSim/Blocks/TransferPhysicsToVerticesBehavior.h | 85 +
SurgSim/Blocks/UnitTests/CMakeLists.txt | 16 +-
.../UnitTests/CompoundShapeToGraphicsTests.cpp | 199 +
.../UnitTests/Data/Geometry/wound_deformable.ply | 2389 ----
.../TransferPhysicsToGraphicsMeshBehavior/data.ply | 14 +
.../data_more.ply | 27 +
.../DriveElementFromInputBehaviorTests.cpp | 130 +
SurgSim/Blocks/UnitTests/FunctionBehaviorTests.cpp | 106 +
SurgSim/Blocks/UnitTests/KeyBehaviorTests.cpp | 152 +
.../UnitTests/KeyboardCallbackBehaviorTests.cpp | 79 +
.../KeyboardTogglesComponentBehaviorTests.cpp | 19 +-
SurgSim/Blocks/UnitTests/PoseInterpolatorTests.cpp | 81 +-
.../Blocks/UnitTests/SingleKeyBehaviorTests.cpp | 105 +
.../TransferParticlesToPointCloudBehaviorTests.cpp | 200 +
.../TransferPhysicsToGraphicsMeshBehaviorTests.cpp | 115 +-
.../TransferPhysicsToPointCloudBehaviorTests.cpp | 8 +-
.../Blocks/UnitTests/VisualizeConstraintsTest.cpp | 79 +
.../UnitTests/VisualizeContactsBehaviorTests.cpp | 43 +-
SurgSim/Blocks/UnitTests/config.txt.in | 4 +-
SurgSim/Blocks/VisualizeConstraints.cpp | 211 +
SurgSim/Blocks/VisualizeConstraints.h | 103 +
SurgSim/Blocks/VisualizeContactsBehavior.cpp | 70 +-
SurgSim/Blocks/VisualizeContactsBehavior.h | 32 +-
SurgSim/CMakeLists.txt | 3 +
SurgSim/Collision/BoxCapsuleContact.cpp | 189 +
SurgSim/Collision/BoxCapsuleContact.h | 49 +
SurgSim/Collision/BoxCapsuleDcdContact.cpp | 194 -
SurgSim/Collision/BoxCapsuleDcdContact.h | 53 -
SurgSim/Collision/BoxDoubleSidedPlaneContact.cpp | 152 +
SurgSim/Collision/BoxDoubleSidedPlaneContact.h | 51 +
.../Collision/BoxDoubleSidedPlaneDcdContact.cpp | 166 -
SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.h | 52 -
SurgSim/Collision/BoxPlaneContact.cpp | 76 +
SurgSim/Collision/BoxPlaneContact.h | 52 +
SurgSim/Collision/BoxPlaneDcdContact.cpp | 88 -
SurgSim/Collision/BoxPlaneDcdContact.h | 53 -
SurgSim/Collision/BoxSphereContact.cpp | 121 +
SurgSim/Collision/BoxSphereContact.h | 53 +
SurgSim/Collision/BoxSphereDcdContact.cpp | 131 -
SurgSim/Collision/BoxSphereDcdContact.h | 55 -
SurgSim/Collision/CMakeLists.txt | 83 +-
SurgSim/Collision/CapsuleSphereContact.cpp | 76 +
SurgSim/Collision/CapsuleSphereContact.h | 50 +
SurgSim/Collision/CapsuleSphereDcdContact.cpp | 78 -
SurgSim/Collision/CapsuleSphereDcdContact.h | 52 -
SurgSim/Collision/CcdDcdCollision.h | 41 +
SurgSim/Collision/CollisionPair.cpp | 92 +-
SurgSim/Collision/CollisionPair.h | 107 +-
SurgSim/Collision/CompoundShapeContact.cpp | 140 +
SurgSim/Collision/CompoundShapeContact.h | 51 +
SurgSim/Collision/ContactCalculation.cpp | 256 +-
SurgSim/Collision/ContactCalculation.h | 103 +-
SurgSim/Collision/ContactFilter.cpp | 41 +
SurgSim/Collision/ContactFilter.h | 72 +
SurgSim/Collision/DcdCollision.h | 33 -
SurgSim/Collision/DefaultContactCalculation.cpp | 43 +-
SurgSim/Collision/DefaultContactCalculation.h | 19 +-
SurgSim/Collision/ElementContactFilter.cpp | 192 +
SurgSim/Collision/ElementContactFilter.h | 136 +
SurgSim/Collision/OctreeCapsuleContact.cpp | 39 +
SurgSim/Collision/OctreeCapsuleContact.h | 44 +
SurgSim/Collision/OctreeContact.cpp | 115 +
SurgSim/Collision/OctreeContact.h | 90 +
SurgSim/Collision/OctreeDcdContact.cpp | 109 -
SurgSim/Collision/OctreeDcdContact.h | 77 -
.../Collision/OctreeDoubleSidedPlaneContact.cpp | 40 +
SurgSim/Collision/OctreeDoubleSidedPlaneContact.h | 44 +
SurgSim/Collision/OctreePlaneContact.cpp | 39 +
SurgSim/Collision/OctreePlaneContact.h | 44 +
SurgSim/Collision/OctreeSphereContact.cpp | 39 +
SurgSim/Collision/OctreeSphereContact.h | 44 +
SurgSim/Collision/PerformanceTests/CMakeLists.txt | 42 +
...iangleMeshContactCalculationPerformanceTest.cpp | 76 +
SurgSim/Collision/PerformanceTests/config.txt.in | 1 +
SurgSim/Collision/Representation.cpp | 268 +-
SurgSim/Collision/Representation.h | 174 +-
.../Collision/SegmentMeshTriangleMeshContact.cpp | 419 +
SurgSim/Collision/SegmentMeshTriangleMeshContact.h | 56 +
.../Collision/SegmentSegmentCcdIntervalCheck.cpp | 319 +
SurgSim/Collision/SegmentSegmentCcdIntervalCheck.h | 229 +
.../Collision/SegmentSegmentCcdMovingContact.cpp | 542 +
SurgSim/Collision/SegmentSegmentCcdMovingContact.h | 239 +
.../Collision/SegmentSegmentCcdStaticContact.cpp | 436 +
SurgSim/Collision/SegmentSegmentCcdStaticContact.h | 166 +
SurgSim/Collision/SegmentSelfContact.cpp | 567 +
SurgSim/Collision/SegmentSelfContact.h | 211 +
SurgSim/Collision/ShapeCollisionRepresentation.cpp | 12 +-
SurgSim/Collision/ShapeCollisionRepresentation.h | 13 +-
SurgSim/Collision/ShapeShapeContactCalculation.h | 97 +
.../Collision/SphereDoubleSidedPlaneContact.cpp | 79 +
SurgSim/Collision/SphereDoubleSidedPlaneContact.h | 51 +
.../Collision/SphereDoubleSidedPlaneDcdContact.cpp | 85 -
.../Collision/SphereDoubleSidedPlaneDcdContact.h | 52 -
SurgSim/Collision/SpherePlaneContact.cpp | 78 +
SurgSim/Collision/SpherePlaneContact.h | 50 +
SurgSim/Collision/SpherePlaneDcdContact.cpp | 82 -
SurgSim/Collision/SpherePlaneDcdContact.h | 52 -
SurgSim/Collision/SphereSphereContact.cpp | 70 +
SurgSim/Collision/SphereSphereContact.h | 50 +
SurgSim/Collision/SphereSphereDcdContact.cpp | 66 -
SurgSim/Collision/SphereSphereDcdContact.h | 50 -
SurgSim/Collision/TriangleMeshParticlesContact.cpp | 97 +
SurgSim/Collision/TriangleMeshParticlesContact.h | 53 +
SurgSim/Collision/TriangleMeshPlaneContact.cpp | 75 +
SurgSim/Collision/TriangleMeshPlaneContact.h | 48 +
SurgSim/Collision/TriangleMeshPlaneDcdContact.cpp | 91 -
SurgSim/Collision/TriangleMeshPlaneDcdContact.h | 52 -
.../Collision/TriangleMeshSurfaceMeshContact.cpp | 142 +
SurgSim/Collision/TriangleMeshSurfaceMeshContact.h | 53 +
.../Collision/TriangleMeshTriangleMeshContact.cpp | 231 +
.../Collision/TriangleMeshTriangleMeshContact.h | 52 +
.../TriangleMeshTriangleMeshDcdContact.cpp | 229 -
.../Collision/TriangleMeshTriangleMeshDcdContact.h | 50 -
.../BoxCapsuleContactCalculationTests.cpp | 4 +-
.../BoxDoubleSidedPlaneContactCalculationTests.cpp | 12 +-
.../UnitTests/BoxPlaneContactCalculationTests.cpp | 12 +-
.../UnitTests/BoxSphereContactCalculationTests.cpp | 4 +-
SurgSim/Collision/UnitTests/CMakeLists.txt | 11 +-
.../CapsuleSphereContactCalculationTests.cpp | 4 +-
SurgSim/Collision/UnitTests/CollisionPairTests.cpp | 143 +-
.../CompoundShapeContactCalculationTests.cpp | 124 +
.../UnitTests/ContactCalculationTests.cpp | 47 +-
.../UnitTests/ContactCalculationTestsCommon.cpp | 56 +-
.../UnitTests/ContactCalculationTestsCommon.h | 27 +-
.../UnitTests/ElementContactFilterTests.cpp | 289 +
.../UnitTests/OctreeContactCalculationTests.cpp | 326 +-
SurgSim/Collision/UnitTests/RepresentationTest.cpp | 168 +-
...mentMeshTriangleMeshContactCalculationTests.cpp | 905 ++
.../SegmentSegmentCcdIntervalCheckTests.cpp | 558 +
.../SegmentSegmentCcdMovingContactTests.cpp | 581 +
.../SegmentSegmentCcdStaticContactTests.cpp | 884 ++
.../UnitTests/SegmentSelfContactTests.cpp | 589 +
.../UnitTests/ShapeCollisionRepresentationTest.cpp | 53 +-
...hereDoubleSidedPlaneContactCalculationTests.cpp | 6 +-
.../SpherePlaneContactCalculationTests.cpp | 4 +-
.../SphereSphereContactCalculationTests.cpp | 4 +-
...riangleMeshParticlesContactCalculationTests.cpp | 147 +
.../TriangleMeshPlaneContactCalculationTests.cpp | 22 +-
...angleMeshSurfaceMeshContactCalculationTests.cpp | 380 +
...ngleMeshTriangleMeshContactCalculationTests.cpp | 48 +-
SurgSim/Collision/UnitTests/config.txt.in | 2 +-
SurgSim/DataStructures/AabbTree.cpp | 14 +
SurgSim/DataStructures/AabbTree.h | 14 +-
SurgSim/DataStructures/AabbTreeData.cpp | 28 +-
SurgSim/DataStructures/AabbTreeData.h | 11 +-
.../DataStructures/AabbTreeIntersectionVisitor.h | 4 +-
SurgSim/DataStructures/AabbTreeNode.cpp | 74 +-
SurgSim/DataStructures/AabbTreeNode.h | 33 +-
SurgSim/DataStructures/BufferedValue-inl.h | 7 +-
SurgSim/DataStructures/CMakeLists.txt | 26 +-
SurgSim/DataStructures/DataGroup.cpp | 15 +
SurgSim/DataStructures/DataGroup.h | 23 +-
SurgSim/DataStructures/DataGroupBuilder.cpp | 19 +-
SurgSim/DataStructures/DataGroupBuilder.h | 17 +
SurgSim/DataStructures/DataGroupCopier.cpp | 47 +-
SurgSim/DataStructures/DataGroupCopier.h | 19 +-
SurgSim/DataStructures/DataStructuresConvert-inl.h | 40 +-
SurgSim/DataStructures/DataStructuresConvert.h | 10 +-
SurgSim/DataStructures/Grid-inl.h | 301 +
SurgSim/DataStructures/Grid.h | 133 +
SurgSim/DataStructures/Groups-inl.h | 168 +
SurgSim/DataStructures/Groups.h | 117 +
SurgSim/DataStructures/Image-inl.h | 85 +-
SurgSim/DataStructures/Image.h | 57 +-
SurgSim/DataStructures/ImageBase-inl.h | 138 +
SurgSim/DataStructures/ImageBase.h | 125 +
SurgSim/DataStructures/ImageMap-inl.h | 47 +
SurgSim/DataStructures/ImageMap.h | 54 +
SurgSim/DataStructures/IndexedLocalCoordinate.cpp | 7 +
SurgSim/DataStructures/IndexedLocalCoordinate.h | 7 +
SurgSim/DataStructures/Location.h | 164 +-
SurgSim/DataStructures/MeshElement.h | 11 +
SurgSim/DataStructures/NamedData-inl.h | 58 +-
SurgSim/DataStructures/NamedData.h | 25 +
SurgSim/DataStructures/NormalData.h | 51 +
SurgSim/DataStructures/OctreeNode-inl.h | 58 +-
SurgSim/DataStructures/OctreeNode.cpp | 47 +-
SurgSim/DataStructures/OctreeNode.h | 31 +-
.../OctreeNodePlyReaderDelegate-inl.h | 70 +
.../DataStructures/OctreeNodePlyReaderDelegate.cpp | 146 +
.../DataStructures/OctreeNodePlyReaderDelegate.h | 166 +
.../DataStructures/PerformanceTests/CMakeLists.txt | 1 +
.../PerformanceTests/GridPerformanceTest.cpp | 120 +
SurgSim/DataStructures/PlyReader.cpp | 43 +-
SurgSim/DataStructures/PlyReader.h | 21 +-
SurgSim/DataStructures/SegmentEmptyData.h | 41 +
SurgSim/DataStructures/SegmentMesh-inl.h | 243 +
SurgSim/DataStructures/SegmentMesh.cpp | 29 +
SurgSim/DataStructures/SegmentMesh.h | 127 +
SurgSim/DataStructures/TriangleMesh-inl.h | 373 +-
SurgSim/DataStructures/TriangleMesh.cpp | 83 +-
SurgSim/DataStructures/TriangleMesh.h | 283 +-
SurgSim/DataStructures/TriangleMeshBase-inl.h | 263 -
SurgSim/DataStructures/TriangleMeshBase.h | 211 -
.../TriangleMeshPlyReaderDelegate-inl.h | 106 +-
.../DataStructures/TriangleMeshPlyReaderDelegate.h | 30 +-
SurgSim/DataStructures/TriangleMeshUtilities-inl.h | 35 -
SurgSim/DataStructures/TriangleMeshUtilities.cpp | 30 -
SurgSim/DataStructures/TriangleMeshUtilities.h | 43 -
.../DataStructures/UnitTests/AabbTreeNodeTests.cpp | 4 +-
SurgSim/DataStructures/UnitTests/AabbTreeTests.cpp | 64 +-
SurgSim/DataStructures/UnitTests/CMakeLists.txt | 16 +-
.../Data/PlyReaderTests/Cube_with_physics.ply | 52 +
.../UnitTests/Data/SegmentMeshTest/segmentmesh.ply | 18 +
.../DataStructures/UnitTests/DataGroupTests.cpp | 40 +-
.../UnitTests/DataStructuresConvertTests.cpp | 84 +-
SurgSim/DataStructures/UnitTests/Grid1DTests.cpp | 256 +
SurgSim/DataStructures/UnitTests/Grid2DTests.cpp | 258 +
SurgSim/DataStructures/UnitTests/Grid3DTests.cpp | 415 +
SurgSim/DataStructures/UnitTests/GridTests.cpp | 26 +
SurgSim/DataStructures/UnitTests/GridTests.h | 73 +
SurgSim/DataStructures/UnitTests/GroupsTests.cpp | 262 +
SurgSim/DataStructures/UnitTests/ImageMapTest.cpp | 178 +
SurgSim/DataStructures/UnitTests/ImageTest.cpp | 144 +-
.../UnitTests/IndexedLocalCoordinateTest.cpp | 28 +
SurgSim/DataStructures/UnitTests/LocationTests.cpp | 222 +-
SurgSim/DataStructures/UnitTests/MockObjects.h | 173 +-
.../DataStructures/UnitTests/NamedDataTests.cpp | 34 +
.../DataStructures/UnitTests/OctreeNodeTests.cpp | 69 +-
.../UnitTests/OptionalValueTests.cpp | 14 +
.../DataStructures/UnitTests/PlyReaderTests.cpp | 54 +-
.../DataStructures/UnitTests/SegmentMeshTest.cpp | 617 +
.../UnitTests/TetrahedronMeshTest.cpp | 8 +-
.../UnitTests/TriangleMeshBaseTest.cpp | 655 --
.../DataStructures/UnitTests/TriangleMeshTest.cpp | 806 +-
SurgSim/DataStructures/UnitTests/config.txt.in | 2 +-
SurgSim/DataStructures/Vertex.h | 28 +-
SurgSim/DataStructures/Vertices-inl.h | 205 +
SurgSim/DataStructures/Vertices.h | 141 +-
SurgSim/DataStructures/ply.c | 56 +-
SurgSim/Devices/CMakeLists.txt | 49 +-
SurgSim/Devices/DeviceFilters/BoolToScalar.cpp | 206 +
SurgSim/Devices/DeviceFilters/BoolToScalar.h | 125 +
SurgSim/Devices/DeviceFilters/CMakeLists.txt | 26 +-
SurgSim/Devices/DeviceFilters/DeviceFilter.cpp | 82 +
SurgSim/Devices/DeviceFilters/DeviceFilter.h | 84 +
SurgSim/Devices/DeviceFilters/FilteredDevice.cpp | 277 +
SurgSim/Devices/DeviceFilters/FilteredDevice.h | 110 +
SurgSim/Devices/DeviceFilters/ForceScale.cpp | 94 +-
SurgSim/Devices/DeviceFilters/ForceScale.h | 74 +-
SurgSim/Devices/DeviceFilters/PoseIntegrator.cpp | 145 +-
SurgSim/Devices/DeviceFilters/PoseIntegrator.h | 47 +-
SurgSim/Devices/DeviceFilters/PoseTransform.cpp | 263 +-
SurgSim/Devices/DeviceFilters/PoseTransform.h | 104 +-
SurgSim/Devices/DeviceFilters/RecordPose.cpp | 90 +
SurgSim/Devices/DeviceFilters/RecordPose.h | 73 +
.../DeviceFilters/UnitTests/BoolToScalarTest.cpp | 214 +
.../Devices/DeviceFilters/UnitTests/CMakeLists.txt | 19 +-
.../UnitTests/Data/FilteredDevice.yaml | 11 +
.../UnitTests/Data/PoseTransform.yaml | 6 +
.../DeviceFilters/UnitTests/DeviceFilterTest.cpp | 94 +
.../DeviceFilters/UnitTests/FilteredDeviceTest.cpp | 123 +
.../DeviceFilters/UnitTests/ForceScaleTest.cpp | 2 +-
.../DeviceFilters/UnitTests/PoseIntegratorTest.cpp | 2 +-
.../DeviceFilters/UnitTests/PoseTransformTest.cpp | 26 +-
.../DeviceFilters}/UnitTests/config.txt.in | 0
SurgSim/Devices/DeviceUtilities.cpp | 178 +
SurgSim/Devices/DeviceUtilities.h | 65 +
SurgSim/Devices/IdentityPoseDevice/CMakeLists.txt | 3 +-
.../IdentityPoseDevice/IdentityPoseDevice.cpp | 43 +-
.../IdentityPoseDevice/IdentityPoseDevice.h | 22 +-
.../UnitTests/IdentityPoseDeviceTest.cpp | 22 +-
SurgSim/Devices/Keyboard/CMakeLists.txt | 10 +-
SurgSim/Devices/Keyboard/KeyCode.h | 4 +-
SurgSim/Devices/Keyboard/KeyboardDevice.cpp | 33 +-
SurgSim/Devices/Keyboard/KeyboardDevice.h | 22 +-
SurgSim/Devices/Keyboard/KeyboardScaffold.cpp | 52 +-
SurgSim/Devices/Keyboard/KeyboardScaffold.h | 27 +-
SurgSim/Devices/Keyboard/OsgKeyboardHandler.cpp | 43 +-
SurgSim/Devices/Keyboard/OsgKeyboardHandler.h | 6 +-
.../Keyboard/UnitTests/KeyboardDeviceTest.cpp | 27 +-
.../Keyboard/UnitTests/KeyboardScaffoldTest.cpp | 2 +-
.../Devices/Keyboard/VisualTests/CMakeLists.txt | 4 +-
.../Keyboard/VisualTests/KeyboardVisualTests.cpp | 434 +-
SurgSim/Devices/LabJack/CMakeLists.txt | 6 +-
SurgSim/Devices/LabJack/LabJackDevice.cpp | 55 +-
SurgSim/Devices/LabJack/LabJackDevice.h | 43 +-
SurgSim/Devices/LabJack/LabJackScaffold.h | 19 +-
SurgSim/Devices/LabJack/LabJackThread.cpp | 7 +-
SurgSim/Devices/LabJack/LabJackThread.h | 12 +-
.../LabJack/UnitTests/LabJackChecksumsTest.cpp | 34 +-
.../LabJack/UnitTests/LabJackDeviceTest.cpp | 96 +-
.../LabJack/UnitTests/LabJackScaffoldTest.cpp | 4 +-
.../UnitTests/LabJackTypeConvertersTest.cpp | 34 +-
SurgSim/Devices/LabJack/VisualTest/CMakeLists.txt | 23 +-
SurgSim/Devices/LabJack/VisualTest/main.cpp | 110 +-
SurgSim/Devices/LabJack/linux/LabJackChecksums.cpp | 6 +-
SurgSim/Devices/LabJack/linux/LabJackChecksums.h | 6 +-
SurgSim/Devices/LabJack/linux/LabJackConstants.h | 6 +-
SurgSim/Devices/LabJack/linux/LabJackScaffold.cpp | 250 +-
.../LabJack/linux/LabJackTypeConverters.cpp | 4 +-
.../Devices/LabJack/linux/LabJackTypeConverters.h | 6 +-
SurgSim/Devices/LabJack/win32/LabJackScaffold.cpp | 185 +-
SurgSim/Devices/Leap/CMakeLists.txt | 59 +
SurgSim/Devices/Leap/Leap.dox | 16 +
SurgSim/Devices/Leap/LeapDevice.cpp | 128 +
SurgSim/Devices/Leap/LeapDevice.h | 130 +
SurgSim/Devices/Leap/LeapScaffold.cpp | 467 +
SurgSim/Devices/Leap/LeapScaffold.h | 115 +
SurgSim/Devices/Leap/LeapUtilities.cpp | 70 +
SurgSim/Devices/Leap/LeapUtilities.h | 38 +
SurgSim/Devices/Leap/UnitTests/CMakeLists.txt | 33 +
SurgSim/Devices/Leap/UnitTests/LeapDeviceTest.cpp | 232 +
SurgSim/Devices/Leap/VisualTest/CMakeLists.txt | 39 +
SurgSim/Devices/Leap/VisualTest/main.cpp | 132 +
SurgSim/Devices/Mouse/CMakeLists.txt | 12 +-
SurgSim/Devices/Mouse/MouseDevice.cpp | 31 +-
SurgSim/Devices/Mouse/MouseDevice.h | 23 +-
SurgSim/Devices/Mouse/MouseScaffold.cpp | 46 +-
SurgSim/Devices/Mouse/MouseScaffold.h | 26 +-
SurgSim/Devices/Mouse/OsgMouseHandler.cpp | 4 +-
SurgSim/Devices/Mouse/OsgMouseHandler.h | 6 +-
.../Devices/Mouse/UnitTests/MouseDeviceTest.cpp | 26 +-
.../Devices/Mouse/UnitTests/MouseScaffoldTest.cpp | 2 +-
SurgSim/Devices/Mouse/VisualTests/CMakeLists.txt | 4 +-
.../Devices/Mouse/VisualTests/MouseVisualTests.cpp | 6 +-
SurgSim/Devices/MultiAxis/BitSetBuffer.h | 4 +-
SurgSim/Devices/MultiAxis/CMakeLists.txt | 9 +-
.../Devices/MultiAxis/CreateInputDeviceHandle.h | 4 +-
SurgSim/Devices/MultiAxis/GetSystemError.h | 4 +-
SurgSim/Devices/MultiAxis/MultiAxisDevice.cpp | 85 +-
SurgSim/Devices/MultiAxis/MultiAxisDevice.h | 58 +-
SurgSim/Devices/MultiAxis/RawMultiAxisDevice.cpp | 22 +-
SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h | 18 +-
SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.cpp | 101 +-
SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h | 35 +-
SurgSim/Devices/MultiAxis/RawMultiAxisThread.cpp | 4 +-
SurgSim/Devices/MultiAxis/RawMultiAxisThread.h | 14 +-
.../Devices/MultiAxis/SystemInputDeviceHandle.cpp | 4 +-
.../Devices/MultiAxis/SystemInputDeviceHandle.h | 4 +-
.../MultiAxis/UnitTests/MultiAxisDeviceTest.cpp | 11 +-
.../MultiAxis/UnitTests/RawMultiAxisDeviceTest.cpp | 23 +-
.../UnitTests/RawMultiAxisScaffoldTest.cpp | 8 +-
.../Devices/MultiAxis/VisualTest/CMakeLists.txt | 28 +-
SurgSim/Devices/MultiAxis/VisualTest/main.cpp | 4 +-
.../Devices/MultiAxis/VisualTest/raw_test_main.cpp | 4 +-
.../MultiAxis/linux/CreateInputDeviceHandle.cpp | 4 +-
SurgSim/Devices/MultiAxis/linux/FileDescriptor.cpp | 4 +-
SurgSim/Devices/MultiAxis/linux/FileDescriptor.h | 4 +-
SurgSim/Devices/MultiAxis/linux/GetSystemError.cpp | 6 +-
.../Devices/MultiAxis/linux/InputDeviceHandle.cpp | 8 +-
.../Devices/MultiAxis/linux/InputDeviceHandle.h | 12 +-
.../MultiAxis/win32/CreateInputDeviceHandle.cpp | 4 +-
SurgSim/Devices/MultiAxis/win32/FileHandle.cpp | 4 +-
SurgSim/Devices/MultiAxis/win32/FileHandle.h | 4 +-
SurgSim/Devices/MultiAxis/win32/GetSystemError.cpp | 4 +-
.../Devices/MultiAxis/win32/WdkHidDeviceHandle.cpp | 8 +-
.../Devices/MultiAxis/win32/WdkHidDeviceHandle.h | 14 +-
SurgSim/Devices/Nimble/CMakeLists.txt | 62 +
SurgSim/Devices/Nimble/Nimble.dox | 9 +
SurgSim/Devices/Nimble/NimbleDevice.cpp | 81 +
SurgSim/Devices/Nimble/NimbleDevice.h | 95 +
SurgSim/Devices/Nimble/NimbleScaffold.cpp | 496 +
SurgSim/Devices/Nimble/NimbleScaffold.h | 102 +
SurgSim/Devices/Nimble/UnitTests/CMakeLists.txt | 32 +
.../Devices/Nimble/UnitTests/NimbleDeviceTest.cpp | 164 +
.../Nimble/UnitTests/NimbleScaffoldTest.cpp | 181 +
SurgSim/Devices/Nimble/VisualTest/CMakeLists.txt | 36 +
SurgSim/Devices/Nimble/VisualTest/main.cpp | 51 +
SurgSim/Devices/Novint/CMakeLists.txt | 21 +-
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 | 199 +-
SurgSim/Devices/Novint/NovintDevice.h | 163 +-
SurgSim/Devices/Novint/NovintScaffold.cpp | 1314 ++-
SurgSim/Devices/Novint/NovintScaffold.h | 116 +-
SurgSim/Devices/Novint/UnitTests/CMakeLists.txt | 3 +-
.../Novint/UnitTests/Novint7DofDeviceTest.cpp | 219 -
.../Devices/Novint/UnitTests/NovintDeviceTest.cpp | 247 +-
.../Novint/UnitTests/NovintScaffoldTest.cpp | 33 +-
SurgSim/Devices/Novint/VisualTest/CMakeLists.txt | 41 +-
.../Devices/Novint/VisualTest/falcon_7dof_main.cpp | 51 -
SurgSim/Devices/Novint/VisualTest/main.cpp | 33 +-
SurgSim/Devices/Oculus/CMakeLists.txt | 67 +
SurgSim/Devices/Oculus/Oculus.dox | 24 +
SurgSim/Devices/Oculus/OculusDevice.cpp | 99 +
SurgSim/Devices/Oculus/OculusDevice.h | 96 +
SurgSim/Devices/Oculus/OculusDisplaySettings.cpp | 68 +
SurgSim/Devices/Oculus/OculusDisplaySettings.h | 77 +
SurgSim/Devices/Oculus/OculusScaffold.cpp | 322 +
SurgSim/Devices/Oculus/OculusScaffold.h | 106 +
SurgSim/Devices/Oculus/OculusView.cpp | 93 +
SurgSim/Devices/Oculus/OculusView.h | 80 +
SurgSim/Devices/Oculus/SceneTest/CMakeLists.txt | 56 +
.../Devices/Oculus/SceneTest/Data/CameraText.yaml | 22 +
SurgSim/Devices/Oculus/SceneTest/Data/Cube.yaml | 8 +
.../Devices/Oculus/SceneTest/Data/MonoView.yaml | 17 +
.../Devices/Oculus/SceneTest/Data/OculusView.yaml | 36 +
SurgSim/Devices/Oculus/SceneTest/Data/OsgView.yaml | 38 +
.../Devices/Oculus/SceneTest/OculusSceneTest.cpp | 157 +
SurgSim/Devices/Oculus/SceneTest/config.txt.in | 2 +
SurgSim/Devices/Oculus/UnitTests/CMakeLists.txt | 41 +
.../Devices/Oculus/UnitTests/OculusDeviceTest.cpp | 118 +
.../UnitTests/OculusDisplaySettingsTests.cpp | 52 +
.../Devices/Oculus/UnitTests/OculusViewTests.cpp | 59 +
SurgSim/Devices/Oculus/VisualTest/CMakeLists.txt | 35 +
SurgSim/Devices/Oculus/VisualTest/main.cpp | 41 +
SurgSim/Devices/OpenNI/CMakeLists.txt | 55 +
SurgSim/Devices/OpenNI/OpenNI.dox | 18 +
SurgSim/Devices/OpenNI/OpenNIDevice.cpp | 72 +
SurgSim/Devices/OpenNI/OpenNIDevice.h | 68 +
SurgSim/Devices/OpenNI/OpenNIScaffold.cpp | 297 +
SurgSim/Devices/OpenNI/OpenNIScaffold.h | 103 +
SurgSim/Devices/OpenNI/UnitTests/CMakeLists.txt | 31 +
.../Devices/OpenNI/UnitTests/OpenNIDeviceTest.cpp | 133 +
SurgSim/Devices/OpenNI/VisualTest/CMakeLists.txt | 46 +
SurgSim/Devices/OpenNI/VisualTest/main.cpp | 113 +
SurgSim/Devices/Phantom/CMakeLists.txt | 6 +-
SurgSim/Devices/Phantom/Phantom.dox | 1 +
SurgSim/Devices/Phantom/PhantomDevice.cpp | 32 +-
SurgSim/Devices/Phantom/PhantomDevice.h | 25 +-
SurgSim/Devices/Phantom/PhantomScaffold.cpp | 158 +-
SurgSim/Devices/Phantom/PhantomScaffold.h | 43 +-
.../Phantom/UnitTests/PhantomDeviceTest.cpp | 59 +-
.../Phantom/UnitTests/PhantomScaffoldTest.cpp | 19 +-
SurgSim/Devices/Phantom/VisualTest/CMakeLists.txt | 21 +-
SurgSim/Devices/Phantom/VisualTest/main.cpp | 7 +-
SurgSim/Devices/ReplayPoseDevice/CMakeLists.txt | 55 +
.../Devices/ReplayPoseDevice/ReplayPoseDevice.cpp | 97 +
.../Devices/ReplayPoseDevice/ReplayPoseDevice.h | 90 +
.../ReplayPoseDevice/ReplayPoseScaffold.cpp | 281 +
.../Devices/ReplayPoseDevice/ReplayPoseScaffold.h | 91 +
.../ReplayPoseDevice/UnitTests/CMakeLists.txt | 31 +
.../UnitTests/ReplayPoseDeviceTest.cpp | 214 +
SurgSim/Devices/Sixense/CMakeLists.txt | 18 +-
SurgSim/Devices/Sixense/Sixense.dox | 9 +-
SurgSim/Devices/Sixense/SixenseDevice.cpp | 27 +-
SurgSim/Devices/Sixense/SixenseDevice.h | 16 +-
SurgSim/Devices/Sixense/SixenseScaffold.cpp | 29 +-
SurgSim/Devices/Sixense/SixenseScaffold.h | 26 +-
SurgSim/Devices/Sixense/SixenseThread.cpp | 4 +-
SurgSim/Devices/Sixense/SixenseThread.h | 12 +-
SurgSim/Devices/Sixense/UnitTests/CMakeLists.txt | 27 +-
.../Sixense/UnitTests/SixenseDeviceTest.cpp | 4 +-
.../Sixense/UnitTests/SixenseScaffoldTest.cpp | 4 +-
SurgSim/Devices/Sixense/VisualTest/CMakeLists.txt | 37 +-
SurgSim/Devices/Sixense/VisualTest/main.cpp | 4 +-
SurgSim/Devices/TrackIR/CMakeLists.txt | 12 +-
SurgSim/Devices/TrackIR/TrackIRDevice.cpp | 27 +-
SurgSim/Devices/TrackIR/TrackIRDevice.h | 25 +-
SurgSim/Devices/TrackIR/TrackIRScaffold.h | 24 +-
SurgSim/Devices/TrackIR/TrackIRThread.cpp | 5 +-
SurgSim/Devices/TrackIR/TrackIRThread.h | 12 +-
.../TrackIR/UnitTests/TrackIRDeviceTest.cpp | 22 +-
.../TrackIR/UnitTests/TrackIRScaffoldTest.cpp | 20 +-
SurgSim/Devices/TrackIR/VisualTest/CMakeLists.txt | 9 +-
SurgSim/Devices/TrackIR/VisualTest/main.cpp | 4 +-
SurgSim/Devices/TrackIR/linux/TrackIRScaffold.cpp | 38 +-
SurgSim/Devices/TrackIR/win32/TrackIRScaffold.cpp | 49 +-
SurgSim/Devices/UnitTests/CMakeLists.txt | 43 +
SurgSim/Devices/UnitTests/Data/noInitialize.yaml | 2 +
SurgSim/Devices/UnitTests/Data/noName.yaml | 2 +
SurgSim/Devices/UnitTests/Data/notMap.yaml | 1 +
SurgSim/Devices/UnitTests/Data/notRegistered.yaml | 2 +
SurgSim/Devices/UnitTests/Data/notSequence.yaml | 1 +
SurgSim/Devices/UnitTests/Data/success.yaml | 4 +
SurgSim/Devices/UnitTests/DeviceUtilitiesTests.cpp | 92 +
.../{Blocks => Devices}/UnitTests/config.txt.in | 0
SurgSim/Devices/devices.dox | 6 +-
SurgSim/Framework/Accessible.cpp | 68 +-
SurgSim/Framework/Accessible.h | 102 +-
SurgSim/Framework/ApplicationData.cpp | 19 +-
SurgSim/Framework/Asset.cpp | 24 +-
SurgSim/Framework/Asset.h | 23 +-
SurgSim/Framework/BasicSceneElement.h | 2 +-
SurgSim/Framework/BasicThread.cpp | 86 +-
SurgSim/Framework/BasicThread.h | 21 +
SurgSim/Framework/BehaviorManager.cpp | 7 +-
SurgSim/Framework/BehaviorManager.h | 12 +-
SurgSim/Framework/CMakeLists.txt | 9 +-
SurgSim/Framework/Clock.h | 29 +-
SurgSim/Framework/Component-inl.h | 40 +
SurgSim/Framework/Component.cpp | 29 +-
SurgSim/Framework/Component.h | 43 +-
SurgSim/Framework/ComponentManager-inl.h | 25 +-
SurgSim/Framework/ComponentManager.cpp | 53 +-
SurgSim/Framework/ComponentManager.h | 17 +-
SurgSim/Framework/FrameworkConvert-inl.h | 2 +-
SurgSim/Framework/FrameworkConvert.cpp | 164 +-
SurgSim/Framework/FrameworkConvert.h | 42 +-
SurgSim/Framework/LogMessageBase.cpp | 18 +-
SurgSim/Framework/LogOutput.cpp | 23 +-
SurgSim/Framework/LogOutput.h | 11 +-
SurgSim/Framework/LoggerManager.cpp | 9 +
SurgSim/Framework/LoggerManager.h | 10 +-
SurgSim/Framework/Macros.h | 6 +-
SurgSim/Framework/ObjectFactory.h | 50 +-
SurgSim/Framework/PoseComponent.h | 8 +-
SurgSim/Framework/Representation.cpp | 18 +-
SurgSim/Framework/Representation.h | 4 +-
SurgSim/Framework/Runtime.cpp | 190 +-
SurgSim/Framework/Runtime.h | 103 +-
SurgSim/Framework/SamplingMetricBase.cpp | 117 +
SurgSim/Framework/SamplingMetricBase.h | 125 +
SurgSim/Framework/Scene.cpp | 78 +-
SurgSim/Framework/Scene.h | 33 +-
SurgSim/Framework/SceneElement-inl.h | 27 +-
SurgSim/Framework/SceneElement.cpp | 148 +-
SurgSim/Framework/SceneElement.h | 70 +-
SurgSim/Framework/SharedInstance-inl.h | 4 +-
SurgSim/Framework/ThreadPool-inl.h | 74 +
SurgSim/Framework/ThreadPool.cpp | 69 +
SurgSim/Framework/ThreadPool.h | 122 +
SurgSim/Framework/Timer.cpp | 71 +-
SurgSim/Framework/Timer.h | 12 +-
SurgSim/Framework/TransferPropertiesBehavior.cpp | 4 +-
SurgSim/Framework/TransferPropertiesBehavior.h | 19 +-
SurgSim/Framework/UnitTests/AccessibleTests.cpp | 60 +-
SurgSim/Framework/UnitTests/AssetTests.cpp | 2 +
.../Framework/UnitTests/BasicSceneElementTests.cpp | 4 +
SurgSim/Framework/UnitTests/BasicThreadTests.cpp | 62 +-
.../Framework/UnitTests/BehaviorManagerTest.cpp | 5 +-
SurgSim/Framework/UnitTests/CMakeLists.txt | 2 +
SurgSim/Framework/UnitTests/ComponentTest.cpp | 24 +-
.../UnitTests/Data/SceneElementTest/circle-1.yaml | 9 +
.../UnitTests/Data/SceneElementTest/circle-2.yaml | 9 +
.../UnitTests/Data/SceneElementTest/included.yaml | 13 +
.../UnitTests/Data/SceneElementTest/includer.yaml | 10 +
.../UnitTests/Data/SceneElementTest/single.yaml | 6 +
.../UnitTests/Data/SceneTestData/bad.yaml | 10 +
.../UnitTests/Data/SceneTestData/element.yaml | 10 +
.../UnitTests/Data/SceneTestData/elements.yaml | 20 +
.../UnitTests/Data/SceneTestData/scene.yaml | 36 +
.../Framework/UnitTests/LockedContainerTest.cpp | 7 +-
SurgSim/Framework/UnitTests/LoggerManagerTest.cpp | 7 +-
SurgSim/Framework/UnitTests/MockObjects.cpp | 9 +-
SurgSim/Framework/UnitTests/MockObjects.h | 68 +-
SurgSim/Framework/UnitTests/ObjectFactoryTests.cpp | 2 +-
SurgSim/Framework/UnitTests/RuntimeTest.cpp | 110 +-
.../Framework/UnitTests/SamplingMetricBaseTest.cpp | 105 +
SurgSim/Framework/UnitTests/SceneElementTest.cpp | 240 +-
SurgSim/Framework/UnitTests/SceneTest.cpp | 116 +-
SurgSim/Framework/UnitTests/ThreadPoolTest.cpp | 79 +
SurgSim/Framework/UnitTests/TimerTest.cpp | 10 +-
.../UnitTests/TransferPropertiesBehaviorTests.cpp | 30 +-
SurgSim/Graphics/AxesRepresentation.h | 2 +-
SurgSim/Graphics/CMakeLists.txt | 34 +-
SurgSim/Graphics/Camera.cpp | 130 +-
SurgSim/Graphics/Camera.h | 103 +-
SurgSim/Graphics/CurveRepresentation.cpp | 57 +
SurgSim/Graphics/CurveRepresentation.h | 99 +
SurgSim/Graphics/Font.h | 35 +
SurgSim/Graphics/Group.h | 4 +-
SurgSim/Graphics/Light.h | 13 +
SurgSim/Graphics/Manager.cpp | 56 +-
SurgSim/Graphics/Manager.h | 3 +-
SurgSim/Graphics/Material.h | 25 +-
SurgSim/Graphics/Mesh-inl.h | 8 +-
SurgSim/Graphics/Mesh.cpp | 64 +-
SurgSim/Graphics/Mesh.h | 54 +-
SurgSim/Graphics/MeshPlyReaderDelegate.h | 2 +-
SurgSim/Graphics/MeshRepresentation.h | 36 +-
SurgSim/Graphics/MeshUtilities.cpp | 28 -
SurgSim/Graphics/MeshUtilities.h | 39 -
SurgSim/Graphics/Model.h | 34 +
SurgSim/Graphics/OsgAxesRepresentation.cpp | 4 +-
SurgSim/Graphics/OsgAxesRepresentation.h | 8 +-
SurgSim/Graphics/OsgBoxRepresentation.h | 16 +-
SurgSim/Graphics/OsgCamera.cpp | 277 +-
SurgSim/Graphics/OsgCamera.h | 57 +-
SurgSim/Graphics/OsgCapsuleRepresentation.h | 16 +-
SurgSim/Graphics/OsgCurveRepresentation.cpp | 212 +
SurgSim/Graphics/OsgCurveRepresentation.h | 121 +
SurgSim/Graphics/OsgCylinderRepresentation.h | 16 +-
SurgSim/Graphics/OsgFont.cpp | 62 +
SurgSim/Graphics/OsgFont.h | 63 +
SurgSim/Graphics/OsgGroup.h | 16 +-
SurgSim/Graphics/OsgLight.cpp | 14 +-
SurgSim/Graphics/OsgLight.h | 34 +-
SurgSim/Graphics/OsgLog.h | 2 +-
SurgSim/Graphics/OsgManager.cpp | 89 +
SurgSim/Graphics/OsgManager.h | 23 +-
SurgSim/Graphics/OsgMaterial.cpp | 125 +-
SurgSim/Graphics/OsgMaterial.h | 64 +-
SurgSim/Graphics/OsgMeshRepresentation.cpp | 292 +-
SurgSim/Graphics/OsgMeshRepresentation.h | 57 +-
SurgSim/Graphics/OsgModel.cpp | 53 +
SurgSim/Graphics/OsgModel.h | 60 +
SurgSim/Graphics/OsgOctreeRepresentation.cpp | 10 +-
SurgSim/Graphics/OsgOctreeRepresentation.h | 8 +-
SurgSim/Graphics/OsgPlane.h | 2 +-
SurgSim/Graphics/OsgPointCloudRepresentation.cpp | 40 +-
SurgSim/Graphics/OsgPointCloudRepresentation.h | 31 +-
SurgSim/Graphics/OsgProgram.cpp | 266 +
SurgSim/Graphics/OsgProgram.h | 164 +
SurgSim/Graphics/OsgRenderTarget-inl.h | 23 +-
SurgSim/Graphics/OsgRenderTarget.h | 10 +-
SurgSim/Graphics/OsgRepresentation.cpp | 107 +-
SurgSim/Graphics/OsgRepresentation.h | 79 +-
SurgSim/Graphics/OsgSceneryRepresentation.cpp | 58 +-
SurgSim/Graphics/OsgSceneryRepresentation.h | 28 +-
SurgSim/Graphics/OsgScreenSpacePass.h | 2 +-
.../Graphics/OsgScreenSpaceQuadRepresentation.cpp | 75 +-
.../Graphics/OsgScreenSpaceQuadRepresentation.h | 29 +-
SurgSim/Graphics/OsgShader.cpp | 259 -
SurgSim/Graphics/OsgShader.h | 150 -
SurgSim/Graphics/OsgSkeletonRepresentation.cpp | 423 +
SurgSim/Graphics/OsgSkeletonRepresentation.h | 134 +
SurgSim/Graphics/OsgTextRepresentation.cpp | 294 +
SurgSim/Graphics/OsgTextRepresentation.h | 141 +
SurgSim/Graphics/OsgTextureCubeMap.cpp | 45 +-
SurgSim/Graphics/OsgTextureCubeMap.h | 51 +-
SurgSim/Graphics/OsgTextureUniform-inl.h | 18 +-
SurgSim/Graphics/OsgTextureUniform.h | 2 +
SurgSim/Graphics/OsgTrackballZoomManipulator.cpp | 5 +
SurgSim/Graphics/OsgTrackballZoomManipulator.h | 10 +-
SurgSim/Graphics/OsgUniform-inl.h | 15 +
SurgSim/Graphics/OsgUniform.h | 21 +-
SurgSim/Graphics/OsgUniformBase.cpp | 2 +-
SurgSim/Graphics/OsgUniformBase.h | 10 +
SurgSim/Graphics/OsgUniformFactory.cpp | 2 +
SurgSim/Graphics/OsgVectorFieldRepresentation.cpp | 43 +-
SurgSim/Graphics/OsgVectorFieldRepresentation.h | 39 +-
SurgSim/Graphics/OsgView.cpp | 183 +-
SurgSim/Graphics/OsgView.h | 38 +-
SurgSim/Graphics/OsgViewElement.cpp | 6 +-
SurgSim/Graphics/OsgViewElement.h | 10 +-
SurgSim/Graphics/PaintBehavior.cpp | 314 +
SurgSim/Graphics/PaintBehavior.h | 139 +
SurgSim/Graphics/PointCloudRepresentation.cpp | 17 +
SurgSim/Graphics/PointCloudRepresentation.h | 13 +-
SurgSim/Graphics/Program.h | 133 +
SurgSim/Graphics/RenderPass.cpp | 16 +-
SurgSim/Graphics/RenderPass.h | 6 +-
SurgSim/Graphics/RenderTests/CMakeLists.txt | 8 +-
.../wound_deformable.ply | 2391 ----
.../OsgScreenSpaceQuadRenderTests/CheckerBoard.png | Bin 4009 -> 0 bytes
.../Data/OsgShaderRenderTests/L_forcep.obj | 132 +
.../{shader.geom => shader_axis_mirrored.geom} | 0
.../rigged_cylinder.osgt | 11743 +++++++++++++++++++
.../RenderTests/ImplicitSurfaceRenderTests.cpp | 350 +
.../Graphics/RenderTests/OsgCameraRenderTests.cpp | 90 +-
.../OsgCurveRepresentationRenderTests.cpp | 143 +
.../OsgMeshRepresentationRenderTests.cpp | 72 +-
.../OsgOctreeRepresentationRenderTests.cpp | 16 +-
.../OsgPointCloudRepresentationRenderTests.cpp | 39 +
.../Graphics/RenderTests/OsgProgramRenderTests.cpp | 452 +
.../RenderTests/OsgRepresentationRenderTests.cpp | 166 +-
.../OsgSceneryRepresentationRenderTests.cpp | 9 +-
.../RenderTests/OsgScreenSpaceQuadRenderTests.cpp | 4 +-
.../Graphics/RenderTests/OsgShaderRenderTests.cpp | 253 -
.../OsgSkeletonRepresentationRenderTests.cpp | 193 +
.../OsgSphereRepresentationRenderTests.cpp | 25 +-
.../OsgTextRepresentationRenderTests.cpp | 136 +
.../OsgVectorFieldRepresentationRenderTests.cpp | 8 +-
.../RenderTests/OsgViewElementRenderTests.cpp | 9 +
.../RenderTests/PaintBehaviorRenderTests.cpp | 181 +
SurgSim/Graphics/RenderTests/RenderTest.cpp | 4 +-
SurgSim/Graphics/RenderTests/RenderTest.h | 2 +
SurgSim/Graphics/RenderTests/config.txt.in | 6 +-
SurgSim/Graphics/Representation.cpp | 18 +-
SurgSim/Graphics/Representation.h | 30 +-
SurgSim/Graphics/SceneryRepresentation.cpp | 38 +
SurgSim/Graphics/SceneryRepresentation.h | 27 +-
SurgSim/Graphics/ScreenSpaceQuadRepresentation.h | 14 +-
SurgSim/Graphics/Shader.h | 123 -
SurgSim/Graphics/SkeletonRepresentation.h | 109 +
SurgSim/Graphics/TangentSpaceGenerator.cpp | 287 +
SurgSim/Graphics/TangentSpaceGenerator.h | 149 +
SurgSim/Graphics/TextRepresentation.cpp | 60 +
SurgSim/Graphics/TextRepresentation.h | 145 +
SurgSim/Graphics/Texture.cpp | 38 +
SurgSim/Graphics/Texture.h | 11 +
SurgSim/Graphics/UnitTests/CMakeLists.txt | 10 +-
.../shader.frag | 0
.../shader.geom | 0
.../shader.vert | 0
.../rigged_cylinder.osgt | 11743 +++++++++++++++++++
.../Data/OsgTextureTests/CheckerBoard.png | Bin 4009 -> 0 bytes
SurgSim/Graphics/UnitTests/ManagerTests.cpp | 8 +-
SurgSim/Graphics/UnitTests/MockObjects.h | 168 +-
SurgSim/Graphics/UnitTests/MockOsgObjects.h | 26 +-
.../UnitTests/OsgAxesRepresentationTests.cpp | 49 +
.../UnitTests/OsgBoxRepresentationTests.cpp | 89 -
SurgSim/Graphics/UnitTests/OsgCameraTests.cpp | 164 +-
.../UnitTests/OsgCurveRepresentationTests.cpp | 87 +
SurgSim/Graphics/UnitTests/OsgGroupTests.cpp | 16 +-
SurgSim/Graphics/UnitTests/OsgLightTests.cpp | 44 +-
SurgSim/Graphics/UnitTests/OsgMaterialTests.cpp | 101 +-
.../UnitTests/OsgMeshRepresentationTests.cpp | 23 +-
.../UnitTests/OsgOctreeRepresentationTests.cpp | 19 +-
.../UnitTests/OsgPlaneRepresentationTests.cpp | 120 +-
.../UnitTests/OsgPointCloudRepresentationTests.cpp | 1 -
SurgSim/Graphics/UnitTests/OsgProgramTests.cpp | 294 +
.../Graphics/UnitTests/OsgRepresentationTests.cpp | 189 +-
.../UnitTests/OsgSceneryRepresentationTests.cpp | 79 +-
.../Graphics/UnitTests/OsgScreenSpaceQuadTests.cpp | 8 +
SurgSim/Graphics/UnitTests/OsgShaderTests.cpp | 297 -
.../UnitTests/OsgSkeletonRepresentationTests.cpp | 235 +
.../UnitTests/OsgSphereRepresentationTests.cpp | 118 -
.../UnitTests/OsgTextRepresentationTests.cpp | 153 +
SurgSim/Graphics/UnitTests/OsgTexture1dTests.cpp | 10 +-
SurgSim/Graphics/UnitTests/OsgTexture2dTests.cpp | 10 +-
SurgSim/Graphics/UnitTests/OsgTexture3dTests.cpp | 20 +-
.../Graphics/UnitTests/OsgTextureCubeMapTests.cpp | 28 +-
.../UnitTests/OsgTextureRectangleTests.cpp | 10 +-
SurgSim/Graphics/UnitTests/OsgTextureTests.cpp | 10 +-
SurgSim/Graphics/UnitTests/OsgUniformBaseTests.cpp | 11 +-
SurgSim/Graphics/UnitTests/OsgUniformTests.cpp | 40 +-
.../OsgVectorFieldRepresentationTests.cpp | 18 +-
SurgSim/Graphics/UnitTests/OsgViewTests.cpp | 59 +-
SurgSim/Graphics/UnitTests/PaintBehaviorTests.cpp | 55 +
SurgSim/Graphics/UnitTests/RenderPassTests.cpp | 3 +-
SurgSim/Graphics/UnitTests/ViewElementTests.cpp | 16 +-
SurgSim/Graphics/UnitTests/config.txt.in | 6 +-
SurgSim/Graphics/VectorFieldRepresentation.h | 7 +-
SurgSim/Graphics/View.cpp | 16 +-
SurgSim/Graphics/View.h | 12 +-
SurgSim/Graphics/ViewElement.h | 2 +-
SurgSim/Input/CMakeLists.txt | 4 +-
SurgSim/Input/CombiningOutputComponent.cpp | 167 +
SurgSim/Input/CombiningOutputComponent.h | 86 +
SurgSim/Input/CommonDevice.cpp | 121 +-
SurgSim/Input/CommonDevice.h | 73 +-
SurgSim/Input/DeviceInterface.h | 20 +-
SurgSim/Input/InputComponent.cpp | 83 +-
SurgSim/Input/InputComponent.h | 62 +-
SurgSim/Input/InputManager.cpp | 107 +-
SurgSim/Input/InputManager.h | 19 +-
SurgSim/Input/OutputComponent.cpp | 86 +-
SurgSim/Input/OutputComponent.h | 55 +-
SurgSim/Input/UnitTests/CMakeLists.txt | 16 +-
.../UnitTests/CombiningOutputComponentTest.cpp | 309 +
SurgSim/Input/UnitTests/CommonDeviceTests.cpp | 11 +
.../UnitTests/Data/CombiningOutputComponent.yaml | 25 +
SurgSim/Input/UnitTests/InputComponentTest.cpp | 28 +-
SurgSim/Input/UnitTests/InputManagerTest.cpp | 33 +-
SurgSim/Input/UnitTests/OutputComponentTest.cpp | 33 +-
SurgSim/Input/UnitTests/TestDevice.cpp | 13 +-
SurgSim/Input/UnitTests/TestDevice.h | 14 +-
SurgSim/{Blocks => Input}/UnitTests/config.txt.in | 0
SurgSim/Math/Aabb.h | 36 +-
SurgSim/Math/BoxShape.cpp | 38 +-
SurgSim/Math/BoxShape.h | 17 +-
SurgSim/Math/CMakeLists.txt | 43 +-
SurgSim/Math/CapsuleShape.cpp | 16 +-
SurgSim/Math/CapsuleShape.h | 14 +-
SurgSim/Math/CardinalSplines.cpp | 104 +
SurgSim/Math/CardinalSplines.h | 58 +
SurgSim/Math/CompoundShape.cpp | 279 +
SurgSim/Math/CompoundShape.h | 135 +
SurgSim/Math/CubicSolver-inl.h | 132 +
SurgSim/Math/CubicSolver.h | 54 +
SurgSim/Math/CylinderShape.cpp | 12 +-
SurgSim/Math/CylinderShape.h | 15 +-
SurgSim/Math/DoubleSidedPlaneShape.cpp | 2 +-
SurgSim/Math/DoubleSidedPlaneShape.h | 10 +-
SurgSim/Math/GaussLegendreQuadrature.cpp | 45 +-
SurgSim/Math/GaussLegendreQuadrature.h | 71 +-
SurgSim/Math/Geometry.h | 945 +-
SurgSim/Math/IntervalArithmetic-inl.h | 899 ++
SurgSim/Math/IntervalArithmetic.h | 568 +
SurgSim/Math/KalmanFilter-inl.h | 122 +
SurgSim/Math/KalmanFilter.h | 111 +
SurgSim/Math/LinearMotionArithmetic-inl.h | 705 ++
SurgSim/Math/LinearMotionArithmetic.h | 499 +
SurgSim/Math/LinearSolveAndInverse-inl.h | 59 +-
SurgSim/Math/LinearSolveAndInverse.cpp | 58 +-
SurgSim/Math/LinearSolveAndInverse.h | 59 +-
SurgSim/Math/LinearSparseSolveAndInverse.cpp | 78 +
SurgSim/Math/LinearSparseSolveAndInverse.h | 129 +
SurgSim/Math/MathConvert-inl.h | 163 +-
SurgSim/Math/MathConvert.cpp | 69 +-
SurgSim/Math/MathConvert.h | 110 +-
SurgSim/Math/Matrix.h | 32 +-
SurgSim/Math/MeshShape-inl.h | 16 +-
SurgSim/Math/MeshShape.cpp | 139 +-
SurgSim/Math/MeshShape.h | 110 +-
SurgSim/Math/MinMax-inl.h | 169 +
SurgSim/Math/MinMax.h | 78 +
SurgSim/Math/MlcpGaussSeidelSolver.cpp | 2099 +---
SurgSim/Math/MlcpGaussSeidelSolver.h | 198 +-
SurgSim/Math/MlcpProblem.cpp | 9 +-
SurgSim/Math/MlcpProblem.h | 3 +-
SurgSim/Math/MlcpSolution.h | 2 +-
SurgSim/Math/OctreeShape.cpp | 61 +-
SurgSim/Math/OctreeShape.h | 31 +-
SurgSim/Math/OdeEquation.cpp | 51 +
SurgSim/Math/OdeEquation.h | 121 +-
SurgSim/Math/OdeSolver.cpp | 36 +-
SurgSim/Math/OdeSolver.h | 122 +-
SurgSim/Math/OdeSolverEulerExplicit.cpp | 66 +-
SurgSim/Math/OdeSolverEulerExplicit.h | 46 +-
SurgSim/Math/OdeSolverEulerExplicitModified.cpp | 66 +-
SurgSim/Math/OdeSolverEulerExplicitModified.h | 39 +-
SurgSim/Math/OdeSolverEulerImplicit.cpp | 168 +-
SurgSim/Math/OdeSolverEulerImplicit.dox | 109 +
SurgSim/Math/OdeSolverEulerImplicit.h | 39 +-
SurgSim/Math/OdeSolverLinearEulerExplicit.cpp | 13 +-
SurgSim/Math/OdeSolverLinearEulerExplicit.h | 4 +-
.../Math/OdeSolverLinearEulerExplicitModified.cpp | 13 +-
.../Math/OdeSolverLinearEulerExplicitModified.h | 4 +-
SurgSim/Math/OdeSolverLinearEulerImplicit.cpp | 29 +-
SurgSim/Math/OdeSolverLinearEulerImplicit.h | 12 +-
SurgSim/Math/OdeSolverLinearRungeKutta4.cpp | 27 +-
SurgSim/Math/OdeSolverLinearRungeKutta4.h | 4 +-
SurgSim/Math/OdeSolverLinearStatic.cpp | 11 +-
SurgSim/Math/OdeSolverLinearStatic.h | 4 +-
SurgSim/Math/OdeSolverRungeKutta4.cpp | 63 +-
SurgSim/Math/OdeSolverRungeKutta4.h | 55 +-
SurgSim/Math/OdeSolverStatic.cpp | 60 +-
SurgSim/Math/OdeSolverStatic.h | 19 +-
SurgSim/Math/OdeState.cpp | 75 +-
SurgSim/Math/OdeState.h | 43 +-
SurgSim/Math/ParticlesShape-inl.h | 45 +
SurgSim/Math/ParticlesShape.cpp | 149 +
SurgSim/Math/ParticlesShape.h | 118 +
SurgSim/Math/PlaneShape.cpp | 2 +-
SurgSim/Math/PlaneShape.h | 10 +-
.../Math/PointTriangleCcdContactCalculation-inl.h | 121 +
SurgSim/Math/Polynomial-inl.h | 699 ++
SurgSim/Math/Polynomial.h | 388 +
SurgSim/Math/PolynomialRoots-inl.h | 127 +
SurgSim/Math/PolynomialRoots.h | 135 +
SurgSim/Math/PolynomialValues-inl.h | 156 +
SurgSim/Math/PolynomialValues.h | 167 +
SurgSim/Math/Scalar-inl.h | 42 +
SurgSim/Math/Scalar.h | 76 +
SurgSim/Math/SegmentMeshShape-inl.h | 37 +
SurgSim/Math/SegmentMeshShape.cpp | 158 +
SurgSim/Math/SegmentMeshShape.h | 98 +
SurgSim/Math/SegmentMeshShapePlyReaderDelegate.cpp | 84 +
SurgSim/Math/SegmentMeshShapePlyReaderDelegate.h | 65 +
.../Math/SegmentSegmentCcdContactCalculation-inl.h | 143 +
SurgSim/Math/Shape.cpp | 27 +-
SurgSim/Math/Shape.h | 80 +-
SurgSim/Math/Shapes.h | 4 +-
SurgSim/Math/SparseMatrix.h | 395 +
SurgSim/Math/SphereShape.cpp | 12 +-
SurgSim/Math/SphereShape.h | 12 +-
SurgSim/Math/SurfaceMeshShape-inl.h | 9 +-
SurgSim/Math/SurfaceMeshShape.cpp | 66 +-
SurgSim/Math/SurfaceMeshShape.h | 60 +-
.../Math/TriangleCapsuleContactCalculation-inl.h | 579 +
.../Math/TriangleTriangleContactCalculation-inl.h | 200 +
SurgSim/Math/TriangleTriangleIntersection-inl.h | 22 +-
SurgSim/Math/UnitTests/AabbTests.cpp | 20 +-
SurgSim/Math/UnitTests/AngleAxisTests.cpp | 66 +
SurgSim/Math/UnitTests/CMakeLists.txt | 23 +-
SurgSim/Math/UnitTests/CardinalSplinesTests.cpp | 92 +
SurgSim/Math/UnitTests/CompoundShapeTests.cpp | 238 +
SurgSim/Math/UnitTests/CubicSolverTests.cpp | 185 +
SurgSim/Math/UnitTests/Data/segmentmesh.ply | 21 +
SurgSim/Math/UnitTests/GeometryTests.cpp | 391 +-
SurgSim/Math/UnitTests/IntervalArithmeticTests.cpp | 734 ++
SurgSim/Math/UnitTests/KalmanFilterTests.cpp | 110 +
.../Math/UnitTests/LinearMotionArithmeticTests.cpp | 435 +
.../Math/UnitTests/LinearSolveAndInverseTests.cpp | 62 +-
.../UnitTests/LinearSparseSolveAndInverseTests.cpp | 197 +
SurgSim/Math/UnitTests/MeshShapeTests.cpp | 256 +-
SurgSim/Math/UnitTests/MinMaxTests.cpp | 217 +
.../UnitTests/MlcpTestData/mlcpOriginalTest.txt | 2 +-
.../Math/UnitTests/MlcpTestData/mlcpTest001.txt | 2 +-
.../Math/UnitTests/MlcpTestData/mlcpTest002.txt | 2 +-
.../Math/UnitTests/MlcpTestData/mlcpTest003.txt | 2 +-
.../Math/UnitTests/MlcpTestData/mlcpTest009.txt | 2 +-
SurgSim/Math/UnitTests/MockCapsule.h | 93 +
SurgSim/Math/UnitTests/MockObject.h | 273 +-
SurgSim/Math/UnitTests/MockTriangle.h | 9 +
SurgSim/Math/UnitTests/OdeEquationTests.cpp | 62 +-
.../OdeSolverEulerExplicitModifiedTests.cpp | 66 +-
.../Math/UnitTests/OdeSolverEulerExplicitTests.cpp | 65 +-
.../Math/UnitTests/OdeSolverEulerImplicitTests.cpp | 152 +-
.../Math/UnitTests/OdeSolverRungeKutta4Tests.cpp | 87 +-
SurgSim/Math/UnitTests/OdeSolverStaticTests.cpp | 68 +-
SurgSim/Math/UnitTests/OdeSolverTests.cpp | 55 +-
SurgSim/Math/UnitTests/OdeStateTests.cpp | 131 +-
SurgSim/Math/UnitTests/ParticlesShapeTests.cpp | 151 +
SurgSim/Math/UnitTests/PolynomialRootTests.cpp | 123 +
SurgSim/Math/UnitTests/PolynomialTests.cpp | 377 +
SurgSim/Math/UnitTests/PolynomialValuesTests.cpp | 105 +
SurgSim/Math/UnitTests/QuaternionTests.cpp | 10 +
SurgSim/Math/UnitTests/ScalarTests.cpp | 184 +
SurgSim/Math/UnitTests/SegmentMeshShapeTests.cpp | 145 +
SurgSim/Math/UnitTests/ShapeTests.cpp | 206 +-
SurgSim/Math/UnitTests/SparseMatrixTests.cpp | 363 +
SurgSim/Math/UnitTests/SurfaceMeshShapeTests.cpp | 30 +-
.../TriangleCapsuleContactCalculationTests.cpp | 483 +
...iangleSeparatingAxisContactCalculationTests.cpp | 174 +
.../UnitTests/TriangleTriangleTestParameters.h | 12 +
SurgSim/Math/UnitTests/VectorTests.cpp | 362 +-
SurgSim/Math/UnitTests/config.txt.in | 4 +-
SurgSim/Math/Vector.h | 41 +-
SurgSim/Particles/CMakeLists.txt | 61 +
SurgSim/Particles/DefaultPointGenerator.cpp | 48 +
SurgSim/Particles/DefaultPointGenerator.h | 49 +
SurgSim/Particles/Emitter.cpp | 218 +
SurgSim/Particles/Emitter.h | 182 +
SurgSim/Particles/Particles.h | 60 +
.../Particles/ParticlesCollisionRepresentation.cpp | 109 +
.../Particles/ParticlesCollisionRepresentation.h | 94 +
SurgSim/Particles/PointGenerator.cpp | 40 +
SurgSim/Particles/PointGenerator.h | 71 +
SurgSim/Particles/RandomBoxPointGenerator.cpp | 65 +
SurgSim/Particles/RandomBoxPointGenerator.h | 47 +
SurgSim/Particles/RandomMeshPointGenerator.cpp | 77 +
SurgSim/Particles/RandomMeshPointGenerator.h | 49 +
SurgSim/Particles/RandomPointGenerator.cpp | 65 +
SurgSim/Particles/RandomPointGenerator.h | 53 +
SurgSim/Particles/RandomSpherePointGenerator.cpp | 73 +
SurgSim/Particles/RandomSpherePointGenerator.h | 47 +
SurgSim/Particles/RenderTests/CMakeLists.txt | 50 +
SurgSim/Particles/RenderTests/RenderTest.cpp | 67 +
SurgSim/Particles/RenderTests/RenderTest.h | 56 +
.../RenderTests/RenderTestSphRepresentation.cpp | 166 +
SurgSim/Particles/RenderTests/config.txt.in | 2 +
SurgSim/Particles/Representation.cpp | 160 +
SurgSim/Particles/Representation.h | 122 +
SurgSim/Particles/Sink.cpp | 107 +
SurgSim/Particles/Sink.h | 94 +
SurgSim/Particles/SphRepresentation.cpp | 367 +
SurgSim/Particles/SphRepresentation.h | 205 +
SurgSim/Particles/UnitTests/CMakeLists.txt | 50 +
SurgSim/Particles/UnitTests/EmitterTests.cpp | 296 +
SurgSim/Particles/UnitTests/MockObjects.cpp | 47 +
SurgSim/Particles/UnitTests/MockObjects.h | 47 +
.../ParticlesCollisionRepresentationTests.cpp | 110 +
.../Particles/UnitTests/PointGeneratorTests.cpp | 105 +
.../UnitTests/RandomPointGeneratorTests.cpp | 53 +
.../Particles/UnitTests/RepresentationTests.cpp | 147 +
SurgSim/Particles/UnitTests/SinkTests.cpp | 165 +
.../Particles/UnitTests/SphRepresentationTests.cpp | 327 +
SurgSim/Particles/UnitTests/config.txt.in | 2 +
SurgSim/Physics/BuildMlcp.cpp | 27 +-
SurgSim/Physics/BuildMlcp.h | 7 +-
SurgSim/Physics/CMakeLists.txt | 116 +-
SurgSim/Physics/CcdCollision.cpp | 73 +
SurgSim/Physics/CcdCollision.h | 69 +
SurgSim/Physics/CcdCollisionLoop.cpp | 228 +
SurgSim/Physics/CcdCollisionLoop.h | 124 +
SurgSim/Physics/Computation.cpp | 43 +-
SurgSim/Physics/Computation.h | 14 +
SurgSim/Physics/ComputationGroup.cpp | 93 +
SurgSim/Physics/ComputationGroup.h | 79 +
SurgSim/Physics/Constraint.cpp | 116 +-
SurgSim/Physics/Constraint.h | 53 +-
SurgSim/Physics/ConstraintComponent.h | 4 +-
SurgSim/Physics/ConstraintImplementation.cpp | 22 +
SurgSim/Physics/ConstraintImplementation.h | 28 +-
.../Physics/ConstraintImplementationFactory.cpp | 75 +-
SurgSim/Physics/ConstraintImplementationFactory.h | 27 +-
SurgSim/Physics/ConstraintType.h | 39 +
SurgSim/Physics/ContactConstraintData.h | 28 +-
SurgSim/Physics/ContactConstraintGeneration.cpp | 172 +-
SurgSim/Physics/ContactConstraintGeneration.h | 21 +-
SurgSim/Physics/ContactFiltering.cpp | 98 +
SurgSim/Physics/ContactFiltering.h | 57 +
SurgSim/Physics/DcdCollision.cpp | 127 +-
SurgSim/Physics/DcdCollision.h | 29 +-
.../Physics/DeformableCollisionRepresentation.cpp | 175 +-
.../Physics/DeformableCollisionRepresentation.h | 35 +-
SurgSim/Physics/DeformableRepresentation.cpp | 179 +-
SurgSim/Physics/DeformableRepresentation.h | 95 +-
SurgSim/Physics/Fem-inl.h | 116 +
SurgSim/Physics/Fem.h | 104 +
SurgSim/Physics/Fem1D.cpp | 35 +
SurgSim/Physics/Fem1D.h | 45 +
SurgSim/Physics/Fem1DElementBeam.cpp | 168 +-
SurgSim/Physics/Fem1DElementBeam.h | 128 +-
SurgSim/Physics/Fem1DLocalization.cpp | 108 +
SurgSim/Physics/Fem1DLocalization.h | 54 +
SurgSim/Physics/Fem1DPlyReaderDelegate.cpp | 89 +-
SurgSim/Physics/Fem1DPlyReaderDelegate.h | 45 +-
SurgSim/Physics/Fem1DRepresentation.cpp | 194 +-
SurgSim/Physics/Fem1DRepresentation.h | 68 +-
.../Physics/Fem1DRepresentationLocalization.cpp | 98 -
SurgSim/Physics/Fem1DRepresentationLocalization.h | 72 -
SurgSim/Physics/Fem2D.cpp | 36 +
SurgSim/Physics/Fem2D.h | 46 +
SurgSim/Physics/Fem2DElementTriangle.cpp | 815 +-
SurgSim/Physics/Fem2DElementTriangle.h | 298 +-
...m2DElementTriangle_computeLocalMembraneMass.dox | 141 +
.../Fem2DElementTriangle_computeLocalPlateMass.dox | 165 +
SurgSim/Physics/Fem2DLocalization.cpp | 72 +
SurgSim/Physics/Fem2DLocalization.h | 57 +
SurgSim/Physics/Fem2DPlyReaderDelegate.cpp | 92 +-
SurgSim/Physics/Fem2DPlyReaderDelegate.h | 44 +-
SurgSim/Physics/Fem2DRepresentation.cpp | 201 +-
SurgSim/Physics/Fem2DRepresentation.h | 59 +-
.../Physics/Fem2DRepresentationLocalization.cpp | 98 -
SurgSim/Physics/Fem2DRepresentationLocalization.h | 72 -
SurgSim/Physics/Fem3D.cpp | 37 +
SurgSim/Physics/Fem3D.h | 47 +
.../Fem3DElementCorotationalTetrahedron.cpp | 307 +-
.../Physics/Fem3DElementCorotationalTetrahedron.h | 51 +-
SurgSim/Physics/Fem3DElementCube.cpp | 269 +-
SurgSim/Physics/Fem3DElementCube.h | 170 +-
SurgSim/Physics/Fem3DElementTetrahedron.cpp | 166 +-
SurgSim/Physics/Fem3DElementTetrahedron.h | 152 +-
SurgSim/Physics/Fem3DLocalization.cpp | 50 +
SurgSim/Physics/Fem3DLocalization.h | 57 +
SurgSim/Physics/Fem3DPlyReaderDelegate.cpp | 83 +-
SurgSim/Physics/Fem3DPlyReaderDelegate.h | 38 +-
SurgSim/Physics/Fem3DRepresentation.cpp | 285 +-
SurgSim/Physics/Fem3DRepresentation.h | 80 +-
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/FemConstraintFixedPoint.cpp | 124 +
SurgSim/Physics/FemConstraintFixedPoint.h | 55 +
.../Physics/FemConstraintFixedRotationVector.cpp | 133 +
SurgSim/Physics/FemConstraintFixedRotationVector.h | 60 +
SurgSim/Physics/FemConstraintFrictionalSliding.cpp | 107 +
SurgSim/Physics/FemConstraintFrictionalSliding.h | 55 +
.../Physics/FemConstraintFrictionlessContact.cpp | 124 +
SurgSim/Physics/FemConstraintFrictionlessContact.h | 62 +
.../Physics/FemConstraintFrictionlessSliding.cpp | 101 +
SurgSim/Physics/FemConstraintFrictionlessSliding.h | 55 +
SurgSim/Physics/FemElement-inl.h | 56 +
SurgSim/Physics/FemElement.cpp | 109 +-
SurgSim/Physics/FemElement.h | 97 +-
SurgSim/Physics/FemElementStructs.h | 89 +
SurgSim/Physics/FemLocalization.cpp | 132 +
SurgSim/Physics/FemLocalization.h | 68 +
SurgSim/Physics/FemPlyFormat.dox | 88 +
SurgSim/Physics/FemPlyReaderDelegate.cpp | 188 +-
SurgSim/Physics/FemPlyReaderDelegate.h | 73 +-
SurgSim/Physics/FemRepresentation.cpp | 386 +-
SurgSim/Physics/FemRepresentation.h | 164 +-
SurgSim/Physics/FemRepresentationParameters.cpp | 198 -
SurgSim/Physics/FemRepresentationParameters.h | 171 -
SurgSim/Physics/FixedConstraintFixedPoint.cpp | 71 +
SurgSim/Physics/FixedConstraintFixedPoint.h | 70 +
.../Physics/FixedConstraintFixedRotationVector.cpp | 72 +
.../Physics/FixedConstraintFixedRotationVector.h | 68 +
.../Physics/FixedConstraintFrictionlessContact.cpp | 84 +
.../Physics/FixedConstraintFrictionlessContact.h | 72 +
SurgSim/Physics/FixedRepresentation.cpp | 8 +-
SurgSim/Physics/FixedRepresentation.h | 10 +-
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 | 33 +-
SurgSim/Physics/FreeMotion.h | 15 +-
SurgSim/Physics/LinearSpring.cpp | 299 +-
SurgSim/Physics/LinearSpring.h | 35 +-
SurgSim/Physics/Localization.cpp | 61 +-
SurgSim/Physics/Localization.h | 62 +-
SurgSim/Physics/MassSpringConstraintFixedPoint.cpp | 119 +
SurgSim/Physics/MassSpringConstraintFixedPoint.h | 57 +
.../MassSpringConstraintFrictionlessContact.cpp | 106 +
.../MassSpringConstraintFrictionlessContact.h | 75 +
SurgSim/Physics/MassSpringLocalization.cpp | 110 +
SurgSim/Physics/MassSpringLocalization.h | 83 +
SurgSim/Physics/MassSpringRepresentation.cpp | 252 +-
SurgSim/Physics/MassSpringRepresentation.h | 74 +-
.../Physics/MassSpringRepresentationContact.cpp | 109 -
SurgSim/Physics/MassSpringRepresentationContact.h | 79 -
.../MassSpringRepresentationLocalization.cpp | 88 -
.../Physics/MassSpringRepresentationLocalization.h | 81 -
SurgSim/Physics/MlcpPhysicsProblem-inl.h | 57 -
SurgSim/Physics/MlcpPhysicsProblem.cpp | 36 +-
SurgSim/Physics/MlcpPhysicsProblem.h | 18 +-
SurgSim/Physics/ParticleCollisionResponse.cpp | 51 +
SurgSim/Physics/ParticleCollisionResponse.h | 50 +
SurgSim/Physics/PerformanceTests/CMakeLists.txt | 6 +-
.../Data/Fem3DPerformanceTest/wound_deformable.ply | 2389 ----
.../DivisibleCubeRepresentation.cpp | 160 +
.../PerformanceTests/DivisibleCubeRepresentation.h | 75 +
.../PerformanceTests/Fem3DPerformanceTest.cpp | 267 +-
.../Fem3DSolutionComponentsTest.cpp | 335 +
SurgSim/Physics/PerformanceTests/config.txt.in | 2 +-
SurgSim/Physics/PhysicsConvert.cpp | 8 +-
SurgSim/Physics/PhysicsConvert.h | 8 +-
SurgSim/Physics/PhysicsManager.cpp | 189 +-
SurgSim/Physics/PhysicsManager.h | 121 +-
SurgSim/Physics/PhysicsManagerState.cpp | 81 +-
SurgSim/Physics/PhysicsManagerState.h | 83 +-
SurgSim/Physics/PostUpdate.cpp | 33 +-
SurgSim/Physics/PostUpdate.h | 7 +-
SurgSim/Physics/PreUpdate.cpp | 6 +-
SurgSim/Physics/PreUpdate.h | 7 +-
SurgSim/Physics/PrepareCollisionPairs.cpp | 112 +
SurgSim/Physics/PrepareCollisionPairs.h | 76 +
SurgSim/Physics/PushResults.cpp | 31 +-
SurgSim/Physics/PushResults.h | 14 +-
SurgSim/Physics/RenderTests/CMakeLists.txt | 3 +-
.../RenderTests/CompoundCollisionRenderTest.cpp | 317 +
SurgSim/Physics/RenderTests/Data/bar.ply | 49 +
SurgSim/Physics/RenderTests/Data/collider.ply | 49 +
SurgSim/Physics/RenderTests/Data/cylinder.ply | 345 +
SurgSim/Physics/RenderTests/Data/half_knot.ply | 158 +
SurgSim/Physics/RenderTests/Data/loop.ply | 101 +
.../Data/prolene 3.0-fixedExtremity.ply | 159 +
.../Physics/RenderTests/Fem3DMeshRenderTest.cpp | 86 +-
.../RenderTests/Fem3DvsTruthCubeRenderTest.cpp | 27 +-
.../Physics/RenderTests/RenderTestCcdSuture.cpp | 341 +
SurgSim/Physics/RenderTests/RenderTestFem1D.cpp | 33 +-
SurgSim/Physics/RenderTests/RenderTestFem2D.cpp | 8 +-
SurgSim/Physics/RenderTests/RenderTestFem3D.cpp | 16 +-
.../RenderTests/RenderTestFem3DCorotational.cpp | 10 +-
.../Physics/RenderTests/RenderTestMassSprings.cpp | 42 +-
.../Physics/RenderTests/RenderTestRigidBodies.cpp | 145 +-
SurgSim/Physics/RenderTests/config.txt.in | 2 +-
SurgSim/Physics/Representation.cpp | 22 +-
SurgSim/Physics/Representation.h | 37 +-
SurgSim/Physics/RigidCollisionRepresentation.cpp | 68 +-
SurgSim/Physics/RigidCollisionRepresentation.h | 12 +-
SurgSim/Physics/RigidConstraintFixedPoint.cpp | 133 +
SurgSim/Physics/RigidConstraintFixedPoint.h | 70 +
.../Physics/RigidConstraintFixedRotationVector.cpp | 73 +
.../Physics/RigidConstraintFixedRotationVector.h | 68 +
.../Physics/RigidConstraintFrictionlessContact.cpp | 109 +
.../Physics/RigidConstraintFrictionlessContact.h | 71 +
SurgSim/Physics/RigidLocalization.cpp | 122 +
SurgSim/Physics/RigidLocalization.h | 74 +
SurgSim/Physics/RigidRepresentation.cpp | 103 +-
SurgSim/Physics/RigidRepresentation.h | 25 +-
SurgSim/Physics/RigidRepresentationBase-inl.h | 5 +-
SurgSim/Physics/RigidRepresentationBase.cpp | 21 +-
SurgSim/Physics/RigidRepresentationBase.h | 39 +-
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 -
SurgSim/Physics/RigidState.cpp | 114 +
SurgSim/Physics/RigidState.h | 108 +
SurgSim/Physics/RotationVectorConstraint.cpp | 98 +
SurgSim/Physics/RotationVectorConstraint.h | 61 +
SurgSim/Physics/RotationVectorConstraintData.h | 121 +
SurgSim/Physics/SlidingConstraint.cpp | 79 +
SurgSim/Physics/SlidingConstraint.h | 77 +
SurgSim/Physics/SlidingConstraintData.cpp | 89 +
SurgSim/Physics/SlidingConstraintData.h | 93 +
SurgSim/Physics/SolveMlcp.cpp | 65 +-
SurgSim/Physics/SolveMlcp.h | 31 +-
SurgSim/Physics/Spring.cpp | 46 +
SurgSim/Physics/Spring.h | 29 +-
SurgSim/Physics/UnitTests/BuildMlcpTests.cpp | 194 +-
SurgSim/Physics/UnitTests/CMakeLists.txt | 66 +-
SurgSim/Physics/UnitTests/CcdCollisionLoopTest.cpp | 92 +
SurgSim/Physics/UnitTests/CommonTests.h | 25 +-
SurgSim/Physics/UnitTests/ComputationGroupTest.cpp | 129 +
SurgSim/Physics/UnitTests/ComputationTests.cpp | 61 +-
.../ConstraintImplementationFactoryTests.cpp | 105 +
.../UnitTests/ConstraintImplementationTests.cpp | 31 +
SurgSim/Physics/UnitTests/ConstraintTests.cpp | 242 +-
.../UnitTests/ContactConstraintDataTests.cpp | 9 +
.../UnitTests/ContactConstraintGenerationTests.cpp | 15 +-
SurgSim/Physics/UnitTests/ContactFilteringTest.cpp | 145 +
.../Data/PlyReaderTests/Fem1DMaterial.ply | 32 +
.../Data/PlyReaderTests/Fem1DNoMaterial.ply | 29 +
.../Data/PlyReaderTests/Fem2DMaterial.ply | 29 +
.../Data/PlyReaderTests/Fem2DNoMaterial.ply | 26 +
.../Data/PlyReaderTests/Fem3DCubeMaterial.ply | 30 +
.../Data/PlyReaderTests/Fem3DCubeNoMaterial.ply | 27 +
.../PlyReaderTests/Wrong3DFileWithRotationData.ply | 26 +
SurgSim/Physics/UnitTests/DcdCollisionTests.cpp | 168 +-
.../DeformableCollisionRepresentationTest.cpp | 43 +-
.../UnitTests/DeformableRepresentationTest.cpp | 148 +-
.../Physics/UnitTests/DeformableTestsUtility-inl.h | 49 +
SurgSim/Physics/UnitTests/DeformableTestsUtility.h | 44 +
.../UnitTests/Fem1DConstraintFixedPointTests.cpp | 223 +
.../Fem1DConstraintFixedRotationVectorTests.cpp | 247 +
.../Fem1DConstraintFrictionalSlidingTests.cpp | 283 +
.../Fem1DConstraintFrictionlessContactTests.cpp | 319 +
.../Fem1DConstraintFrictionlessSlidingTests.cpp | 287 +
.../Physics/UnitTests/Fem1DElementBeamTests.cpp | 226 +-
.../Physics/UnitTests/Fem1DLocalizationTest.cpp | 188 +
.../UnitTests/Fem1DMechanicalValidationTests.cpp | 27 +-
.../UnitTests/Fem1DPlyReaderDelegateTests.cpp | 83 +-
.../Fem1DRepresentationLocalizationTest.cpp | 228 -
.../Physics/UnitTests/Fem1DRepresentationTests.cpp | 248 +-
.../UnitTests/Fem2DConstraintFixedPointTests.cpp | 227 +
.../Fem2DConstraintFrictionalSlidingTests.cpp | 288 +
.../Fem2DConstraintFrictionlessContactTests.cpp | 318 +
.../Fem2DConstraintFrictionlessSlidingTests.cpp | 290 +
.../UnitTests/Fem2DElementTriangleTests.cpp | 865 +-
.../Physics/UnitTests/Fem2DLocalizationTest.cpp | 177 +
.../UnitTests/Fem2DMechanicalValidationTests.cpp | 12 +-
.../UnitTests/Fem2DPlyReaderDelegateTests.cpp | 83 +-
.../Fem2DRepresentationLocalizationTest.cpp | 235 -
.../Physics/UnitTests/Fem2DRepresentationTests.cpp | 235 +-
.../UnitTests/Fem3DConstraintFixedPointTests.cpp | 223 +
.../Fem3DConstraintFrictionalSlidingTests.cpp | 299 +
.../Fem3DConstraintFrictionlessContactTests.cpp | 318 +
.../Fem3DConstraintFrictionlessSlidingTests.cpp | 299 +
.../Fem3DElementCorotationalTetrahedronTests.cpp | 402 +-
.../Physics/UnitTests/Fem3DElementCubeTests.cpp | 222 +-
.../UnitTests/Fem3DElementTetrahedronTests.cpp | 196 +-
.../Physics/UnitTests/Fem3DLocalizationTest.cpp | 185 +
.../UnitTests/Fem3DPlyReaderDelegateTests.cpp | 176 +-
.../Fem3DRepresentationBilateral3DTests.cpp | 237 -
.../UnitTests/Fem3DRepresentationContactTests.cpp | 275 -
.../Fem3DRepresentationLocalizationTest.cpp | 371 -
.../Physics/UnitTests/Fem3DRepresentationTests.cpp | 269 +-
SurgSim/Physics/UnitTests/FemElementTests.cpp | 100 +-
SurgSim/Physics/UnitTests/FemLocalizationTest.cpp | 291 +
.../UnitTests/FemRepresentationParametersTest.cpp | 183 -
.../Physics/UnitTests/FemRepresentationTests.cpp | 302 +-
.../UnitTests/FixedConstraintFixedPointTests.cpp | 114 +
.../FixedConstraintFixedRotationVectorTests.cpp | 106 +
.../FixedConstraintFrictionlessContactTests.cpp | 86 +
.../FixedRepresentationBilateral3DTests.cpp | 116 -
.../UnitTests/FixedRepresentationContactTests.cpp | 87 -
.../FixedRepresentationLocalizationTest.cpp | 99 -
.../Physics/UnitTests/FixedRepresentationTest.cpp | 22 +-
SurgSim/Physics/UnitTests/LinearSpringTest.cpp | 309 +-
.../MassSpringConstraintFixedPointTest.cpp | 251 +
...MassSpringConstraintFrictionlessContactTest.cpp | 271 +
.../UnitTests/MassSpringLocalizationTest.cpp | 150 +
.../MassSpringMechanicalValidationTests.cpp | 12 +-
.../MassSpringRepresentationContactTest.cpp | 259 -
.../MassSpringRepresentationLocalizationTest.cpp | 107 -
.../UnitTests/MassSpringRepresentationTests.cpp | 157 +-
SurgSim/Physics/UnitTests/MockObjects.cpp | 401 +-
SurgSim/Physics/UnitTests/MockObjects.h | 370 +-
.../UnitTests/ParticleCollisionResponseTests.cpp | 83 +
.../Physics/UnitTests/PhysicsManagerStateTests.cpp | 105 +-
SurgSim/Physics/UnitTests/PhysicsManagerTests.cpp | 146 +-
.../UnitTests/PrepareCollisionPairsTests.cpp | 217 +
SurgSim/Physics/UnitTests/PushResultsTests.cpp | 151 +-
SurgSim/Physics/UnitTests/RepresentationTest.cpp | 35 +
.../UnitTests/RigidCollisionRepresentationTest.cpp | 52 +-
.../UnitTests/RigidConstraintFixedPointTests.cpp | 161 +
.../RigidConstraintFixedRotationVectorTests.cpp | 118 +
.../RigidConstraintFrictionlessContactTests.cpp | 100 +
.../Physics/UnitTests/RigidLocalizationTest.cpp | 182 +
.../RigidRepresentationBilateral3DTests.cpp | 163 -
.../UnitTests/RigidRepresentationContactTests.cpp | 99 -
.../RigidRepresentationLocalizationTest.cpp | 123 -
.../UnitTests/RigidRepresentationStateTest.cpp | 134 -
.../Physics/UnitTests/RigidRepresentationTest.cpp | 179 +-
SurgSim/Physics/UnitTests/RigidStateTest.cpp | 134 +
.../RotationVectorConstraintDataTests.cpp | 70 +
.../UnitTests/SlidingConstraintDataTests.cpp | 65 +
SurgSim/Physics/UnitTests/SolveMlcpTests.cpp | 7 +-
.../UpdateCollisionRepresentationsTest.cpp | 69 +
.../Physics/UnitTests/VirtualToolCouplerTest.cpp | 204 +-
SurgSim/Physics/UnitTests/config.txt.in | 4 +-
SurgSim/Physics/UpdateCcdData.cpp | 81 +
SurgSim/Physics/UpdateCcdData.h | 55 +
SurgSim/Physics/UpdateCollisionData.cpp | 66 +
SurgSim/Physics/UpdateCollisionData.h | 49 +
SurgSim/Physics/UpdateCollisionRepresentations.cpp | 18 +-
SurgSim/Physics/UpdateCollisionRepresentations.h | 7 +-
SurgSim/Physics/UpdateDcdData.cpp | 81 +
SurgSim/Physics/UpdateDcdData.h | 49 +
SurgSim/Physics/VirtualToolCoupler.cpp | 214 +-
SurgSim/Physics/VirtualToolCoupler.h | 117 +-
SurgSim/Serialize/CMakeLists.txt | 3 +-
SurgSim/Serialize/GraphicsConvert.h | 36 +-
SurgSim/Testing/CMakeLists.txt | 17 +-
.../Data/Geometry}/Cube.ply | 0
.../Data/Geometry/Cube_with_texture.ply} | 0
.../Geometry}/InvalidMesh.ply | 0
.../Geometry}/Torus.mtl | 0
.../Geometry}/Torus.obj | 0
.../Geometry}/Torus.osgb | Bin
.../Data => Testing/Data/Geometry}/box.ply | 0
SurgSim/Testing/Data/Geometry/cube.osgt | 162 +
.../Geometry/invalid-staple.ply} | 0
SurgSim/Testing/Data/Geometry/plane.ply | 70 +
.../Data => Testing/Data/Geometry}/sphere.ply | 0
SurgSim/Testing/Data/Geometry/sphere0_025.ply | 492 +
SurgSim/Testing/Data/Geometry/sphere0_5.mtl | 10 +
SurgSim/Testing/Data/Geometry/sphere0_5.obj | 1604 +++
SurgSim/Testing/Data/Geometry/staple.ply | 49 +
.../Geometry}/staple_collision.ply | 0
.../Testing/Data/Geometry/stapler_collision.ply | 679 ++
.../Geometry/wound_deformable_with_texture.ply | 2391 ++++
.../Data/Textures}/Brdf0.png | Bin
.../Data/Textures}/Brdf1.png | Bin
.../Data/Textures/CubeMap_axes.png} | Bin
.../Data/Textures/CubeMap_reflection_diffuse.png | Bin 0 -> 76684 bytes
.../Data/Textures/CubeMap_reflection_specular.png | Bin 0 -> 194986 bytes
SurgSim/Testing/Data/Textures/CubeMap_rgb.png | Bin 0 -> 7224 bytes
.../Data/Textures/CubeMap_rgb_rotate.png} | Bin
.../Data/Textures}/Gradient.png | Bin
.../Data/Textures}/NegativeX.png | Bin
.../Data/Textures}/NegativeY.png | Bin
.../Data/Textures}/NegativeZ.png | Bin
.../Data/Textures}/PositiveX.png | Bin
.../Data/Textures}/PositiveY.png | Bin
.../Data/Textures}/PositiveZ.png | Bin
.../Data/Textures}/Rectangle.png | Bin
SurgSim/Testing/Data/Textures/bricks.png | Bin 0 -> 110078 bytes
.../Data/Textures/wound_deformable.png} | Bin
SurgSim/Testing/MlcpIO/CMakeLists.txt | 13 +-
SurgSim/Testing/MlcpIO/ReadText.cpp | 42 +-
SurgSim/Testing/MockInputComponent.cpp | 35 +
SurgSim/Testing/MockInputComponent.h | 43 +
SurgSim/Testing/MockInputOutput.cpp | 1 +
SurgSim/Testing/MockPhysicsManager.h | 8 +-
SurgSim/Testing/OctreeShapeData/staple.vox | 26 -
SurgSim/Testing/TestingMain.cpp | 1 -
SurgSim/Testing/TriangleMeshBaseTests/Cube.ply | 51 -
SurgSim/Testing/Utilities.h | 40 +
SurgSim/Testing/VisualTestCommon/CMakeLists.txt | 14 +-
SurgSim/Testing/VisualTestCommon/GlutRenderer.cpp | 105 +-
SurgSim/Testing/VisualTestCommon/GlutRenderer.h | 46 +-
.../Testing/VisualTestCommon/MovingSquareForce.h | 7 +-
.../VisualTestCommon/MovingSquareGlutWindow.h | 5 +-
.../Testing/VisualTestCommon/ToolSquareTest.cpp | 1 +
ThirdParty/google-style-lint/cpplint.py | 4684 ++++++--
Tools/CMakeLists.txt | 22 +
Tools/Converters/tetgen2ply.py | 167 +
Tools/NeedleSutureGeneration/CMakeLists.txt | 43 +
Tools/NeedleSutureGeneration/Data/properties.ini | 9 +
.../NeedleSutureGeneration.cpp | 292 +
Tools/run-lint.py | 2 +-
1433 files changed, 133594 insertions(+), 37163 deletions(-)
diff --git a/.gitignore b/.gitignore
index 317be26..4a2791f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -9,7 +9,6 @@
## object files and other build output
*.o
-*.obj
*.pdb
*.so
*.a
diff --git a/CMake/External_yamlcpp.cmake b/CMake/External_yamlcpp.cmake
index d08e19a..c08d8d0 100644
--- a/CMake/External_yamlcpp.cmake
+++ b/CMake/External_yamlcpp.cmake
@@ -13,35 +13,48 @@
# See the License for the specific language governing permissions and
# limitations under the License.
-set(file_id "bAvdlSnfqr5ikadmr6bg7m")
+set(CMAKE_ARGS
+ -DCMAKE_INSTALL_PREFIX:PATH=<INSTALL_DIR>
+ -DYAML_CPP_BUILD_TOOLS:BOOL=OFF
+ -DBUILD_SHARED_LIBS:BOOL=${BUILD_SHARED_LIBS}
+)
+if(DEFINED CMAKE_BUILD_TYPE)
+ LIST(APPEND CMAKE_ARGS -DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE})
+endif(DEFINED CMAKE_BUILD_TYPE)
+
ExternalProject_Add(yaml-cpp
- URL "https://www.assembla.com/spaces/OpenSurgSim/documents/${file_id}/download/yaml-cpp.tar.gz"
- URL_MD5 "6bd2a7b4cc31ad0b65209a8030dda7ed"
+ URL "https://github.com/jbeder/yaml-cpp/archive/release-0.5.2.tar.gz"
+ URL_MD5 "2728af8a15e2b2c407730c45b99b274b"
PREFIX yaml-cpp
- CMAKE_ARGS
- -DCMAKE_INSTALL_PREFIX:PATH=<INSTALL_DIR>
- -DYAML_CPP_BUILD_TOOLS:BOOL=OFF
- -DBUILD_SHARED_LIBS:BOOL=${BUILD_SHARED_LIBS}
+ CMAKE_ARGS ${CMAKE_ARGS}
)
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 "")
+ add_library(yaml-cpp-lib STATIC IMPORTED GLOBAL)
+ set_target_properties(yaml-cpp-lib PROPERTIES IMPORTED_LOCATION_DEBUG
+ "${install_dir}/lib/libyaml-cppmdd${CMAKE_STATIC_LIBRARY_SUFFIX}")
+ set_target_properties(yaml-cpp-lib PROPERTIES IMPORTED_LOCATION_RELEASE
+ "${install_dir}/lib/libyaml-cppmd${CMAKE_STATIC_LIBRARY_SUFFIX}")
+ set_target_properties(yaml-cpp-lib PROPERTIES IMPORTED_LOCATION_RELWITHDEBINFO
+ "${install_dir}/lib/libyaml-cppmd${CMAKE_STATIC_LIBRARY_SUFFIX}")
+ set_target_properties(yaml-cpp-lib PROPERTIES IMPORTED_LOCATION_MINSIZEREL
+ "${install_dir}/lib/libyaml-cppmd${CMAKE_STATIC_LIBRARY_SUFFIX}")
+
else()
if(BUILD_SHARED_LIBS)
- set(YAML_CPP_LIBRARIES
- "${install_dir}/lib/${CMAKE_SHARED_LIBRARY_PREFIX}yaml-cpp${CMAKE_SHARED_LIBRARY_SUFFIX}"
- CACHE INTERNAL "")
+ add_library(yaml-cpp-lib SHARED IMPORTED)
+ set_target_properties(yaml-cpp-lib PROPERTIES IMPORTED_LOCATION
+ "${install_dir}/lib/${CMAKE_SHARED_LIBRARY_PREFIX}yaml-cpp${CMAKE_SHARED_LIBRARY_SUFFIX}")
else()
- set(YAML_CPP_LIBRARIES
- "${install_dir}/lib/${CMAKE_STATIC_LIBRARY_PREFIX}yaml-cpp${CMAKE_STATIC_LIBRARY_SUFFIX}"
- CACHE INTERNAL "")
+ add_library(yaml-cpp-lib STATIC IMPORTED)
+ set_target_properties(yaml-cpp-lib PROPERTIES IMPORTED_LOCATION
+ "${install_dir}/lib/${CMAKE_STATIC_LIBRARY_PREFIX}yaml-cpp${CMAKE_STATIC_LIBRARY_SUFFIX}")
endif()
endif()
+add_dependencies(yaml-cpp-lib yaml-cpp)
+set(YAML_CPP_LIBRARIES "yaml-cpp-lib" CACHE INTERNAL "")
set(YAML_CPP_INCLUDE_DIR "${install_dir}/include" CACHE INTERNAL "")
diff --git a/CMake/FindLabJack.cmake b/CMake/FindLabJack.cmake
index bc67fa9..26f5043 100644
--- a/CMake/FindLabJack.cmake
+++ b/CMake/FindLabJack.cmake
@@ -36,7 +36,7 @@ find_library(LABJACK_LIBRARY
mark_as_advanced(LABJACK_LIBRARY)
include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(LABJACK
+find_package_handle_standard_args(LabJack
DEFAULT_MSG LABJACK_ROOT_DIR LABJACK_INCLUDE_DIR
LABJACK_LIBRARY)
diff --git a/CMake/FindLeapSdk.cmake b/CMake/FindLeapSdk.cmake
new file mode 100644
index 0000000..eae5b43
--- /dev/null
+++ b/CMake/FindLeapSdk.cmake
@@ -0,0 +1,52 @@
+# 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 Leap SDK
+#
+# Once done this will define
+# LEAPSDK_FOUND
+# LEAPSDK_INCLUDE_DIR
+# LEAPSDK_LIBRARY
+
+set(LEAPSDK_DIR CACHE PATH "The directory containing the Leap Motion SDK.")
+mark_as_advanced(LEAPSDK_DIR)
+
+find_path(LEAPSDK_INCLUDE_DIR Leap.h
+ HINTS
+ ENV LEAPSDK_DIR
+ ${LEAPSDK_DIR}
+ PATH_SUFFIXES include
+ PATHS /usr /usr/local
+)
+mark_as_advanced(LEAPSDK_INCLUDE_DIR)
+
+if(CMAKE_SIZEOF_VOID_P EQUAL 8)
+ set(LIB_SUFFIX x64)
+else()
+ set(LIB_SUFFIX x86)
+endif()
+
+find_library(LEAPSDK_LIBRARY
+ NAMES libLeap Leap
+ HINTS ENV LEAPSDK_DIR ${LEAPSDK_DIR}
+ PATH_SUFFIXES lib/${LIB_SUFFIX}
+ PATHS /usr /usr/local
+)
+mark_as_advanced(LEAPSDK_LIBRARY)
+
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(LeapSdk DEFAULT_MSG LEAPSDK_LIBRARY LEAPSDK_INCLUDE_DIR)
diff --git a/CMake/FindMathJax.cmake b/CMake/FindMathJax.cmake
new file mode 100644
index 0000000..ce9ef4e
--- /dev/null
+++ b/CMake/FindMathJax.cmake
@@ -0,0 +1,38 @@
+# 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 MathJax
+#
+# Once done this will define
+# MATHJAX_FOUND
+# MATHJAX_DIR
+#
+
+if(NOT MATHJAX_DIR)
+ find_path(MATHJAX_DIR
+ NAMES MathJax.js
+ PATHS "$ENV{MATHJAX_DIR}" "/usr/share/javascript/mathjax/"
+ DOC "Path to local MathJax.js"
+ )
+endif()
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(MathJax
+ "Could NOT find MathJax. To use a local version, set MATHJAX_DIR to the directory containing MathJax.js."
+ MATHJAX_DIR
+)
+
+mark_as_advanced(MATHJAX_DIR)
diff --git a/CMake/FindNovintHdalSdk.cmake b/CMake/FindNovintHdalSdk.cmake
index 9d604c1..df8c346 100644
--- a/CMake/FindNovintHdalSdk.cmake
+++ b/CMake/FindNovintHdalSdk.cmake
@@ -1,9 +1,9 @@
# - 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
+# NOVINTHDALSDK_FOUND - system has the Novint HDAL SDK directory
+# NOVINTHDALSDK_INCLUDE_DIR - the Novint HDAL SDK include directory
+# NOVINTHDALSDK_LIBRARIES - the Novint HDAL SDK libraries
# This file is a part of the OpenSurgSim project.
# Copyright 2013, SimQuest Solutions Inc.
@@ -12,7 +12,7 @@
# 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
+find_path(NOVINTHDALSDK_INCLUDE_DIR
NAMES hdl/hdl.h
PATHS "$ENV{NOVINT_DEVICE_SUPPORT}"
PATH_SUFFIXES "include"
@@ -23,25 +23,25 @@ find_path(NOVINT_HDAL_SDK_INCLUDE_DIR
)
# TODO(advornik): Shake down some usual suspects under Windows/Linux?
-find_path(NOVINT_HDAL_SDK_INCLUDE_DIR
+find_path(NOVINTHDALSDK_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}")
+if(NOVINTHDALSDK_INCLUDE_DIR)
+ get_filename_component(PARENT_DIR "${NOVINTHDALSDK_INCLUDE_DIR}" PATH)
+ set(NOVINTHDALSDK_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}")
+ list(APPEND NOVINTHDALSDK_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)
+endif(NOVINTHDALSDK_INCLUDE_DIR)
# Look for (shared) libraries.
set(LIB_SUFFIX "")
@@ -51,7 +51,7 @@ if(WIN32 AND MSVC)
endif()
endif(WIN32 AND MSVC)
if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
- if("${CMAKE_SIZEOF_VOID_P}" STREQUAL "64")
+ if("${CMAKE_SIZEOF_VOID_P}" STREQUAL "8") # 64 bit
set(LIB_SUFFIX "${LIB_SUFFIX}_64")
endif()
endif(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
@@ -78,22 +78,22 @@ macro(novint_shared_from_link OUTPUT)
endmacro()
macro(novint_find_library LIB_NAME)
- find_library(NOVINT_HDAL_SDK_${LIB_NAME}_LIBRARY
+ find_library(NOVINTHDALSDK_${LIB_NAME}_LIBRARY
NAMES "${LIB_NAME}${LIB_SUFFIX}"
- HINTS ${NOVINT_HDAL_SDK_ROOT_DIRS}
+ HINTS ${NOVINTHDALSDK_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
+ find_library(NOVINTHDALSDK_${LIB_NAME}_LIBRARY
NAMES "${LIB_NAME}${LIB_SUFFIX}"
PATH_SUFFIXES "lib"
)
- mark_as_advanced(NOVINT_HDAL_SDK_${LIB_NAME}_LIBRARY)
+ mark_as_advanced(NOVINTHDALSDK_${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}")
+ if(NOVINTHDALSDK_${LIB_NAME}_LIBRARY AND
+ NOT NOVINTHDALSDK_${LIB_NAME}_SHARED)
+ novint_shared_from_link(NOVINTHDALSDK_${LIB_NAME}_SHARED
+ "${NOVINTHDALSDK_${LIB_NAME}_LIBRARY}")
endif()
endmacro()
@@ -107,10 +107,11 @@ novint_find_library(hdl)
# 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)
+find_package_handle_standard_args(NovintHdalSdk
+ DEFAULT_MSG NOVINTHDALSDK_INCLUDE_DIR NOVINTHDALSDK_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)
+if(NOVINTHDALSDK_FOUND)
+ set(NOVINTHDALSDK_LIBRARIES ${NOVINTHDALSDK_hdl_LIBRARY})
+endif(NOVINTHDALSDK_FOUND)
+
+mark_as_advanced(NOVINTHDALSDK_LIBRARIES)
diff --git a/CMake/FindOculusSdk.cmake b/CMake/FindOculusSdk.cmake
new file mode 100644
index 0000000..ff25d40
--- /dev/null
+++ b/CMake/FindOculusSdk.cmake
@@ -0,0 +1,132 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2015, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT 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(OCULUSSDK_DIR CACHE PATH "The directory containing the Oculus SDK.")
+mark_as_advanced(OCULUSSDK_DIR)
+
+find_path(OCULUSSDK_INCLUDE_DIR
+ NAMES OVR.h
+ PATH_SUFFIXES LibOVR/Include
+ HINTS
+ ${OCULUSSDK_DIR}
+ $ENV{OCULUSSDK_DIR}
+ PATHS /usr /usr/local
+)
+mark_as_advanced(OCULUSSDK_INCLUDE_DIR)
+
+set(OCULUSSDK_VERSION_H "${OCULUSSDK_INCLUDE_DIR}/OVR_Version.h")
+if(EXISTS "${OCULUSSDK_VERSION_H}")
+ file(READ "${OCULUSSDK_VERSION_H}" _oculussdk_version_header)
+
+ string(REGEX MATCH "define[ \t]+OVR_PRODUCT_VERSION[ \t]+([0-9]+)" _oculussdk_product_version_match "${_oculussdk_version_header}")
+ set(OVR_PRODUCT_VERSION "${CMAKE_MATCH_1}")
+
+ string(REGEX MATCH "define[ \t]+OVR_MAJOR_VERSION[ \t]+([0-9]+)" _oculussdk_major_version_match "${_oculussdk_version_header}")
+ set(OVR_MAJOR_VERSION "${CMAKE_MATCH_1}")
+
+ string(REGEX MATCH "define[ \t]+OVR_MINOR_VERSION[ \t]+([0-9]+)" _oculussdk_minor_version_match "${_oculussdk_version_header}")
+ set(OVR_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+ string(REGEX MATCH "define[ \t]+OVR_PATCH_VERSION[ \t]+([0-9]+)" _oculussdk_patch_version_match "${_oculussdk_version_header}")
+ set(OVR_PATCH_VERSION "${CMAKE_MATCH_1}")
+
+ set(OCULUSSDK_VERSION ${OVR_PRODUCT_VERSION}.${OVR_MAJOR_VERSION}.${OVR_MINOR_VERSION}.${OVR_PATCH_VERSION})
+endif()
+
+if(WIN32)
+ if(CMAKE_SIZEOF_VOID_P EQUAL 8)
+ set(ARCH x64)
+ else()
+ set(ARCH Win32)
+ endif()
+
+ set(LIB_DIR LibOVR/Lib/Windows/${ARCH}/)
+
+ if(MSVC_VERSION LESS 1700)
+ set(DEBUG_LIB_DIR ${LIB_DIR}Debug/VS2010)
+ set(RELEASE_LIB_DIR ${LIB_DIR}Release/VS2010)
+ elseif(MSVC_VERSION EQUAL 1700)
+ set(DEBUG_LIB_DIR ${LIB_DIR}Debug/VS2012)
+ set(RELEASE_LIB_DIR ${LIB_DIR}Release/VS2012)
+ else()
+ set(DEBUG_LIB_DIR ${LIB_DIR}Debug/VS2013)
+ set(RELEASE_LIB_DIR ${LIB_DIR}Release/VS2013)
+ endif()
+
+else()
+ if(CMAKE_SIZEOF_VOID_P EQUAL 8)
+ set(ARCH x86_64)
+ else()
+ set(ARCH i386)
+ endif()
+
+ set(DEBUG_LIB_DIR LibOVR/Lib/Linux/${ARCH}/Debug)
+ set(RELEASE_LIB_DIR LibOVR/Lib/Linux/${ARCH}/Release)
+endif()
+
+find_library(OCULUSSDK_LIBRARY_RELEASE
+ NAMES libOVR libOVR.a
+ PATH_SUFFIXES ${RELEASE_LIB_DIR}
+ HINTS
+ ${OCULUSSDK_DIR}
+ $ENV{OCULUSSDK_DIR}
+ PATHS /usr /usr/local
+)
+
+find_library(OCULUSSDK_LIBRARY_DEBUG
+ NAMES libOVR libOVR.a
+ PATH_SUFFIXES ${DEBUG_LIB_DIR}
+ HINTS
+ ${OCULUSSDK_DIR}
+ $ENV{OCULUSSDK_DIR}
+)
+
+if(NOT OCULUSSDK_LIBRARY)
+ if(OCULUSSDK_LIBRARY_RELEASE AND OCULUSSDK_LIBRARY_DEBUG)
+ set(OCULUSSDK_LIBRARY
+ optimized ${OCULUSSDK_LIBRARY_RELEASE}
+ debug ${OCULUSSDK_LIBRARY_DEBUG}
+ CACHE STRING "Oculus Release and Debug libraries were FOUND.")
+ endif()
+
+ if(OCULUSSDK_LIBRARY_RELEASE AND NOT OCULUSSDK_LIBRARY_DEBUG)
+ set(OCULUSSDK_LIBRARY
+ ${OCULUSSDK_LIBRARY_RELEASE}
+ CACHE STRING "Oculus Release library version was found.")
+ endif()
+
+ if(NOT OCULUSSDK_LIBRARY_RELEASE AND OCULUSSDK_LIBRARY_DEBUG)
+ set(OCULUSSDK_LIBRARY
+ ${OCULUSSDK_LIBRARY_DEBUG}
+ CACHE STRING "Oculus Debug library version was found.")
+ endif()
+
+ mark_as_advanced(OCULUSSDK_LIBRARY)
+endif()
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(OculusSdk
+ REQUIRED_VARS OCULUSSDK_INCLUDE_DIR OCULUSSDK_VERSION_H OCULUSSDK_LIBRARY
+ VERSION_VAR OCULUSSDK_VERSION
+)
+
+#Workaround, appending missing 3rd party libraries
+if(OCULUSSDK_LIBRARY)
+ if(WIN32)
+ set(OCULUSSDK_LIBRARY ${OCULUSSDK_LIBRARY} winmm.lib; ws2_32.lib)
+ else()
+ set(OCULUSSDK_LIBRARY ${OCULUSSDK_LIBRARY} X11 GL Xrandr dl)
+ endif()
+endif(OCULUSSDK_LIBRARY)
diff --git a/CMake/FindOpenNI2.cmake b/CMake/FindOpenNI2.cmake
new file mode 100644
index 0000000..7b352b4
--- /dev/null
+++ b/CMake/FindOpenNI2.cmake
@@ -0,0 +1,117 @@
+# Software License Agreement (BSD License)
+#
+# Point Cloud Library (PCL) - www.pointclouds.org
+# Copyright (c) 2009-2012, Willow Garage, Inc.
+# Copyright (c) 2012-, Open Perception, Inc.
+# Copyright (c) XXX, respective authors.
+#
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the copyright holder(s) 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.
+
+
+###############################################################################
+# Find OpenNI 2
+#
+# This sets the following variables:
+# OPENNI2_FOUND - True if OPENNI 2 was found.
+# OPENNI2_INCLUDE_DIRS - Directories containing the OPENNI 2 include files.
+# OPENNI2_LIBRARIES - Libraries needed to use OPENNI 2.
+# OPENNI2_DEFINITIONS - Compiler flags for OPENNI 2.
+#
+# For libusb-1.0, add USB_10_ROOT if not found
+
+find_package(PkgConfig QUIET)
+
+# Find LibUSB
+if(NOT WIN32)
+ pkg_check_modules(PC_USB_10 libusb-1.0)
+ find_path(USB_10_INCLUDE_DIR libusb-1.0/libusb.h
+ HINTS ${PC_USB_10_INCLUDEDIR} ${PC_USB_10_INCLUDE_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}"
+ PATH_SUFFIXES libusb-1.0)
+
+ find_library(USB_10_LIBRARY
+ NAMES usb-1.0
+ HINTS ${PC_USB_10_LIBDIR} ${PC_USB_10_LIBRARY_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}"
+ PATH_SUFFIXES lib)
+
+ include(FindPackageHandleStandardArgs)
+ find_package_handle_standard_args(USB_10 DEFAULT_MSG USB_10_LIBRARY USB_10_INCLUDE_DIR)
+
+ if(NOT USB_10_FOUND)
+ message(STATUS "OpenNI 2 disabled because libusb-1.0 not found.")
+ return()
+ else()
+ include_directories(SYSTEM ${USB_10_INCLUDE_DIR})
+ endif()
+endif(NOT WIN32)
+
+if(${CMAKE_VERSION} VERSION_LESS 2.8.2)
+ pkg_check_modules(PC_OPENNI2 libopenni2)
+else()
+ pkg_check_modules(PC_OPENNI2 QUIET libopenni2)
+endif()
+
+set(OPENNI2_DEFINITIONS ${PC_OPENNI_CFLAGS_OTHER})
+
+set(OPENNI2_SUFFIX)
+if(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8)
+ set(OPENNI2_SUFFIX 64)
+endif(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8)
+
+find_path(OPENNI2_INCLUDE_DIRS OpenNI.h
+ PATHS
+ "$ENV{OPENNI2_INCLUDE${OPENNI2_SUFFIX}}" # Win64 needs '64' suffix
+ /usr/include/openni2 # common path for deb packages
+)
+
+find_library(OPENNI2_LIBRARY
+ NAMES OpenNI2 # No suffix needed on Win64
+ libOpenNI2 # Linux
+ PATHS "$ENV{OPENNI2_LIB${OPENNI2_SUFFIX}}" # Windows default path, Win64 needs '64' suffix
+ "$ENV{OPENNI2_REDIST}" # Linux install does not use a separate 'lib' directory
+ )
+
+if(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
+ set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY} ${LIBUSB_1_LIBRARIES})
+else()
+ set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY})
+endif()
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(OpenNI2 DEFAULT_MSG OPENNI2_LIBRARY OPENNI2_INCLUDE_DIRS)
+
+mark_as_advanced(OPENNI2_LIBRARY OPENNI2_INCLUDE_DIRS)
+
+if(OPENNI2_FOUND)
+ # Add the include directories
+ set(OPENNI2_INCLUDE_DIRS ${OPENNI2_INCLUDE_DIR})
+ set(OPENNI2_REDIST_DIR $ENV{OPENNI2_REDIST${OPENNI2_SUFFIX}})
+ message(STATUS "OpenNI 2 found (include: ${OPENNI2_INCLUDE_DIRS}, lib: ${OPENNI2_LIBRARY}, redist: ${OPENNI2_REDIST_DIR})")
+endif(OPENNI2_FOUND)
+
diff --git a/CMake/FindOptiTrack.cmake b/CMake/FindOptiTrack.cmake
index 469549a..2a10a7b 100644
--- a/CMake/FindOptiTrack.cmake
+++ b/CMake/FindOptiTrack.cmake
@@ -106,7 +106,7 @@ else()
endif()
include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(OPTITRACK
+find_package_handle_standard_args(OptiTrack
DEFAULT_MSG OPTITRACK_ROOT_DIR OPTITRACK_INCLUDE_DIR
OPTITRACK_LIBRARY)
diff --git a/CMake/FindSixenseSdk.cmake b/CMake/FindSixenseSdk.cmake
index f6aba21..c644258 100644
--- a/CMake/FindSixenseSdk.cmake
+++ b/CMake/FindSixenseSdk.cmake
@@ -1,9 +1,9 @@
# - 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
+# SIXENSESDK_FOUND - system has the Sixense SDK directory
+# SIXENSESDK_INCLUDE_DIR - the Sixense SDK include directory
+# SIXENSESDK_LIBRARIES - the Sixense SDK libraries
# This file is a part of the OpenSurgSim project.
# Copyright 2013, SimQuest Solutions Inc.
@@ -12,9 +12,9 @@
# 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
+find_path(SIXENSESDK_INCLUDE_DIR
NAMES sixense.h
- PATHS "$ENV{SIXENSE_SDK_PATH}" "$ENV{SIXENSE_ROOT}"
+ PATHS "$ENV{SIXENSESDK_PATH}" "$ENV{SIXENSE_ROOT}"
PATH_SUFFIXES "include"
NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
)
@@ -28,7 +28,7 @@ if(WIN32)
# 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
+ find_path(SIXENSESDK_INCLUDE_DIR
NAMES sixense.h
PATHS
"C:/Program Files (x86)/Steam/steamapps/common/sixense sdk/SixenseSDK"
@@ -40,7 +40,7 @@ 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
+ find_path(SIXENSESDK_INCLUDE_DIR
NAMES sixense.h
PATHS
"$ENV{HOME}/.local/share/Steam/SteamApps/common/sixense sdk/SixenseSDK"
@@ -49,7 +49,7 @@ if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH
)
endif(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
-find_path(SIXENSE_SDK_INCLUDE_DIR
+find_path(SIXENSESDK_INCLUDE_DIR
NAMES sixense.h
PATH_SUFFIXES "include"
)
@@ -68,22 +68,27 @@ if(WIN32 AND MSVC)
else()
set(LIB_ARCH "win32")
endif()
+ if(MSVC_VERSION LESS "1800")
+ set(LIB_ARCH "${LIB_ARCH}/VS2010")
+ else()
+ set(LIB_ARCH "${LIB_ARCH}/VS2013")
+ endif()
endif(WIN32 AND MSVC)
if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
- if("${CMAKE_SIZEOF_VOID_P}" STREQUAL "64")
+ if("${CMAKE_SIZEOF_VOID_P}" STREQUAL "8") # 64 bit
set(LIB_ARCH "linux_x64")
set(LIB_SYSDIR "lib64")
set(LIB_SUFFIX "${LIB_SUFFIX}_x64")
- else("${CMAKE_SIZEOF_VOID_P}" STREQUAL "64")
+ else("${CMAKE_SIZEOF_VOID_P}" STREQUAL "8") # 32 bit
set(LIB_ARCH "linux")
set(LIB_SYSDIR "lib")
- endif("${CMAKE_SIZEOF_VOID_P}" STREQUAL "64")
+ endif("${CMAKE_SIZEOF_VOID_P}" STREQUAL "8")
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)
+if(SIXENSESDK_INCLUDE_DIR)
+ get_filename_component(SIXENSESDK_ROOT_DIR
+ ${SIXENSESDK_INCLUDE_DIR} PATH)
+endif(SIXENSESDK_INCLUDE_DIR)
macro(sixense_shared_from_link OUTPUT)
@@ -107,49 +112,49 @@ macro(sixense_shared_from_link OUTPUT)
endmacro()
macro(sixense_find_library LIB_NAME)
- find_library(SIXENSE_SDK_${LIB_NAME}_LIBRARY_RELEASE
+ find_library(SIXENSESDK_${LIB_NAME}_LIBRARY_RELEASE
NAMES ${LIB_NAME}${LIB_SUFFIX}
- HINTS "${SIXENSE_SDK_ROOT_DIR}"
+ HINTS "${SIXENSESDK_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
+ find_library(SIXENSESDK_${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
+ find_library(SIXENSESDK_${LIB_NAME}_LIBRARY_DEBUG
NAMES ${LIB_NAME}d${LIB_SUFFIX}
- HINTS "${SIXENSE_SDK_ROOT_DIR}"
+ HINTS "${SIXENSESDK_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
+ find_library(SIXENSESDK_${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}
+ if(SIXENSESDK_${LIB_NAME}_LIBRARY_RELEASE AND
+ SIXENSESDK_${LIB_NAME}_LIBRARY_DEBUG AND
+ NOT SIXENSESDK_${LIB_NAME}_LIBRARY)
+ set(SIXENSESDK_${LIB_NAME}_LIBRARY
+ optimized ${SIXENSESDK_${LIB_NAME}_LIBRARY_RELEASE}
+ debug ${SIXENSESDK_${LIB_NAME}_LIBRARY_DEBUG}
CACHE STRING "The ${LIB_NAME} library from the Sixense SDK.")
- mark_as_advanced(SIXENSE_SDK_${LIB_NAME}_LIBRARY)
+ mark_as_advanced(SIXENSESDK_${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}")
+ if(SIXENSESDK_${LIB_NAME}_LIBRARY_RELEASE AND
+ NOT SIXENSESDK_${LIB_NAME}_SHARED_RELEASE)
+ sixense_shared_from_link(SIXENSESDK_${LIB_NAME}_SHARED_RELEASE
+ "${SIXENSESDK_${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}")
+ if(SIXENSESDK_${LIB_NAME}_LIBRARY_DEBUG AND
+ NOT SIXENSESDK_${LIB_NAME}_SHARED_DEBUG)
+ sixense_shared_from_link(SIXENSESDK_${LIB_NAME}_SHARED_DEBUG
+ "${SIXENSESDK_${LIB_NAME}_LIBRARY_DEBUG}")
endif()
endmacro()
@@ -157,34 +162,34 @@ 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)
+find_package_handle_standard_args(SixenseSdk
+ DEFAULT_MSG SIXENSESDK_ROOT_DIR SIXENSESDK_INCLUDE_DIR
+ SIXENSESDK_sixense_LIBRARY SIXENSESDK_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(SIXENSESDK_FOUND)
+ set(SIXENSESDK_LIBRARIES
+ ${SIXENSESDK_sixense_LIBRARY} ${SIXENSESDK_sixense_utils_LIBRARY})
+endif(SIXENSESDK_FOUND)
+mark_as_advanced(SIXENSESDK_LIBRARIES)
-if(SIXENSE_SDK_FOUND)
+if(SIXENSESDK_FOUND)
# HACK: for debug, we may also need the mysterious DeviceDLL.dll on Windows.
if(WIN32)
- find_file(SIXENSE_SDK_DeviceDLL_SHARED_DEBUG
+ find_file(SIXENSESDK_DeviceDLL_SHARED_DEBUG
NAMES DeviceDLL.dll
- HINTS "${SIXENSE_SDK_ROOT_DIR}"
+ HINTS "${SIXENSESDK_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 SIXENSESDK_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
+ set(SIXENSESDK_DeviceDLL_SHARED_DEBUG
CACHE PATH "Path to DeviceDLL, if any." FORCE)
- endif(NOT SIXENSE_SDK_DeviceDLL_SHARED_DEBUG)
+ endif(NOT SIXENSESDK_DeviceDLL_SHARED_DEBUG)
else()
- set(SIXENSE_SDK_DeviceDLL_SHARED_DEBUG
+ set(SIXENSESDK_DeviceDLL_SHARED_DEBUG
CACHE PATH "Path to DeviceDLL, if any.")
- mark_as_advanced(SIXENSE_SDK_DeviceDLL_SHARED_DEBUG)
+ mark_as_advanced(SIXENSESDK_DeviceDLL_SHARED_DEBUG)
endif()
-endif(SIXENSE_SDK_FOUND)
+endif(SIXENSESDK_FOUND)
diff --git a/CMake/Library.h.in b/CMake/Library.h.in
new file mode 100644
index 0000000..d9b770a
--- /dev/null
+++ b/CMake/Library.h.in
@@ -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.
+
+#ifndef @HEADER_GUARD@
+#define @HEADER_GUARD@
+
+ at HEADER_FILES_INCLUDES@
+
+#endif // @HEADER_GUARD@
+
+
diff --git a/CMake/OpenSurgSimConfig.cmake.in b/CMake/OpenSurgSimConfig.cmake.in
index 565b1c7..ecd57d4 100644
--- a/CMake/OpenSurgSimConfig.cmake.in
+++ b/CMake/OpenSurgSimConfig.cmake.in
@@ -12,7 +12,7 @@ set(OPENSURGSIM_INCLUDE_DIRS "@CONFIG_INCLUDE_DIRS@")
include("${OPENSURGSIM_CMAKE_DIR}/OpenSurgSimTargets.cmake")
# These are IMPORTED targets created by OpenSurgSimTargets.cmake
-set(OPENSURGSIM_LIBRARIES "@EXPORT_TARGETS@")
+set(OPENSURGSIM_LIBRARIES "@SURGSIM_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/SurgSimBuildFlags.cmake b/CMake/SurgSimBuildFlags.cmake
index 9de6014..7e9dcdc 100644
--- a/CMake/SurgSimBuildFlags.cmake
+++ b/CMake/SurgSimBuildFlags.cmake
@@ -17,11 +17,12 @@
## Set build flags through CMake.
## Splitting this out removes excessive verbiage from CMakeLists.txt.
+cmake_policy(SET CMP0054 NEW)
+
# 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 "")
+if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
set(CMAKE_BUILD_TYPE "Release")
-endif("${CMAKE_BUILD_TYPE}" STREQUAL "")
+endif()
# We always want to use defines from <math.h>.
if(MSVC)
@@ -128,18 +129,14 @@ if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
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})
+# Enable alignement on 64bit systems by default
+if("${CMAKE_SIZEOF_VOID_P}" STREQUAL "8")
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
index 09a61f9..3900016 100644
--- a/CMake/SurgSimUtilities.cmake
+++ b/CMake/SurgSimUtilities.cmake
@@ -1,5 +1,5 @@
# This file is a part of the OpenSurgSim project.
-# Copyright 2012-2013, SimQuest Solutions Inc.
+# Copyright 2012-2015, SimQuest Solutions Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -132,44 +132,90 @@ macro(surgsim_copy_to_target_directory_for_release TARGET)
"${TARGET}" ${ARGN})
endmacro()
+# Add an executable and store the target.
+unset(SURGSIM_EXECUTABLE_TARGETS CACHE)
+macro(surgsim_add_executable EXECUTABLE_NAME SOURCES HEADERS)
+ add_executable(${EXECUTABLE_NAME} ${SOURCES} ${HEADERS})
+ set(SURGSIM_EXECUTABLE_TARGETS ${SURGSIM_EXECUTABLE_TARGETS} ${EXECUTABLE_NAME} CACHE INTERNAL "executable targets")
+ surgsim_show_ide_folders("${SOURCES}" "${HEADERS}")
+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")
+macro(surgsim_add_unit_tests TEST_NAME)
+ add_executable(${TEST_NAME} ${UNIT_TEST_SOURCES} ${UNIT_TEST_HEADERS})
+ set(SURGSIM_EXECUTABLE_TARGETS ${SURGSIM_EXECUTABLE_TARGETS} ${TEST_NAME} CACHE INTERNAL "executable targets")
+ target_link_libraries(${TEST_NAME} SurgSimTesting gmock ${LIBS})
+ add_test(NAME ${TEST_NAME} COMMAND ${TEST_NAME} "--gtest_output=xml")
if(SURGSIM_RUN_TEST_WITHIN_BUILD)
- add_custom_command(TARGET ${TESTNAME} POST_BUILD
- COMMAND ${SURGSIM_TEST_RUN_PREFIX} $<TARGET_FILE:${TESTNAME}>
+ add_custom_command(TARGET ${TEST_NAME} POST_BUILD
+ COMMAND ${SURGSIM_TEST_RUN_PREFIX} $<TARGET_FILE:${TEST_NAME}>
${SURGSIM_TEST_RUN_SUFFIX}
VERBATIM)
endif()
+ # Enable bigobj for unit tests, this lets us buid the bigger templated unit tests without problems
+ if(MSVC)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /bigobj")
+ endif()
# copy all ${UNIT_TEST_SHARED..._LIBS} to the test executable directory:
- surgsim_copy_to_target_directory(${TESTNAME}
+ surgsim_copy_to_target_directory(${TEST_NAME}
${UNIT_TEST_SHARED_LIBS})
- surgsim_copy_to_target_directory_for_release(${TESTNAME}
+ surgsim_copy_to_target_directory_for_release(${TEST_NAME}
${UNIT_TEST_SHARED_RELEASE_LIBS})
- surgsim_copy_to_target_directory_for_debug(${TESTNAME}
+ surgsim_copy_to_target_directory_for_debug(${TEST_NAME}
${UNIT_TEST_SHARED_DEBUG_LIBS})
surgsim_show_ide_folders("${UNIT_TEST_SOURCES}" "${UNIT_TEST_HEADERS}")
endmacro()
+# This function will create a new header file (LIBRARY_HEADER) that includes
+# all the headers (except all "-inl.h") in HEADER_FILES.
+function(surgsim_create_library_header LIBRARY_HEADER HEADER_FILES)
+ if(";${HEADER_FILES};" MATCHES ";${LIBRARY_HEADER};")
+ message(FATAL_ERROR
+ "Cannot create library header named '${LIBRARY_HEADER}' because there is already a header with that name")
+ endif()
+
+ file(RELATIVE_PATH HEADER_DIRECTORY ${SURGSIM_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR})
+ foreach(HEADER ${HEADER_FILES})
+ # Filters "-inl.h" files as they are not intended to be included directly but through a 'normal' header .h
+ # Example: Fem-inl.h is included only in Fem.h
+ # So only the 'normal' header files are exposed here in the library header file.
+ string(FIND ${HEADER} "-inl.h" result)
+ if(result STREQUAL -1)
+ set(HEADER_FILES_INCLUDES "${HEADER_FILES_INCLUDES}#include \"${HEADER_DIRECTORY}/${HEADER}\"\n")
+ endif()
+ endforeach()
+
+ string(REPLACE "/" "_" HEADER_GUARD "${HEADER_DIRECTORY}/${LIBRARY_HEADER}")
+ string(REPLACE "." "_" HEADER_GUARD ${HEADER_GUARD})
+ string(TOUPPER ${HEADER_GUARD} HEADER_GUARD)
+ configure_file(${SURGSIM_SOURCE_DIR}/CMake/Library.h.in "${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_HEADER}" @ONLY)
+ install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_HEADER}"
+ DESTINATION ${INSTALL_INCLUDE_DIR}/${HEADER_DIRECTORY})
+endfunction()
+
# 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})
+unset(SURGSIM_EXPORT_TARGETS CACHE)
+function(surgsim_add_library LIBRARY_NAME SOURCES HEADERS)
+ file(RELATIVE_PATH HEADER_DIRECTORY ${SURGSIM_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR})
+ if (SOURCES)
+ add_library(${LIBRARY_NAME} ${SOURCES} ${HEADERS})
+
+ set_target_properties(${LIBRARY_NAME} PROPERTIES
+ PUBLIC_HEADER "${HEADERS}"
+ VERSION ${OPENSURGSIM_VERSION}
+ SOVERSION ${OPENSURGSIM_VERSION})
- set_target_properties(${LIBRARY_NAME} PROPERTIES PUBLIC_HEADER "${HEADER_FILES}")
install(TARGETS ${LIBRARY_NAME}
EXPORT ${PROJECT_NAME}Targets
RUNTIME DESTINATION "${INSTALL_BIN_DIR}"
@@ -177,11 +223,11 @@ function(surgsim_add_library LIBRARY_NAME SOURCE_FILES HEADER_FILES HEADER_DIREC
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}")
+ set(SURGSIM_EXPORT_TARGETS ${LIBRARY_NAME} ${SURGSIM_EXPORT_TARGETS} CACHE INTERNAL "export targets")
+ surgsim_show_ide_folders("${SOURCES}" "${HEADERS}")
else()
- install(FILES ${HEADER_FILES} DESTINATION ${INSTALL_INCLUDE_DIR}/${HEADER_DIRECTORY})
- surgsim_show_ide_folders("" "${HEADER_FILES}")
+ install(FILES ${HEADERS} DESTINATION ${INSTALL_INCLUDE_DIR}/${HEADER_DIRECTORY})
+ surgsim_show_ide_folders("" "${HEADERS}")
endif()
endfunction()
@@ -215,8 +261,10 @@ mark_as_advanced(SURGSIM_RUNLINT_EXTRA_FLAGS)
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
+ -whitespace/ending_newline -whitespace/indent
+ -whitespace/empty_loop_body
# these flag some useful stuff, but also currently produce a lot of noise:
+ -readability/namespace
-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 ":":
@@ -229,7 +277,7 @@ set(CPPLINT_DEFAULT_FILTER_LIST
# (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
+ -runtime/rtti -readability/streams -build/c++11
)
string(REPLACE ";" "," CPPLINT_DEFAULT_FILTERS
"--filter=${CPPLINT_DEFAULT_FILTER_LIST}")
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 875d89d..8002944 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
# This file is a part of the OpenSurgSim project.
-# Copyright 2012-2013, SimQuest Solutions Inc.
+# Copyright 2012-2016, SimQuest Solutions Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -14,12 +14,12 @@
# limitations under the License.
-cmake_minimum_required(VERSION 2.8)
+cmake_minimum_required(VERSION 2.8.11)
project(OpenSurgSim)
set_property(GLOBAL PROPERTY USE_FOLDERS ON)
set(OPENSURGSIM_MAJOR_VERSION 0)
-set(OPENSURGSIM_MINOR_VERSION 0)
+set(OPENSURGSIM_MINOR_VERSION 7)
set(OPENSURGSIM_PATCH_VERSION 0)
set(OPENSURGSIM_VERSION
${OPENSURGSIM_MAJOR_VERSION}.${OPENSURGSIM_MINOR_VERSION}.${OPENSURGSIM_PATCH_VERSION})
@@ -52,7 +52,11 @@ include(SurgSimUtilities)
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)
+ find_package(Eigen3 3.2.3 REQUIRED)
+else(WIN32)
+ find_package(Eigen3 3.2.1 REQUIRED)
+endif(WIN32)
if(WIN32)
set(GLUT_ROOT_PATH "$ENV{GLUT_ROOT_PATH}")
@@ -85,17 +89,20 @@ 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)
+find_package(yaml-cpp 0.5.2 QUIET)
+if(yaml-cpp_FOUND)
+ message(STATUS "Found yaml-cpp")
+else()
+ message(STATUS "Yaml-cpp was not found. It will be downloaded and built during the build.")
include(External_yamlcpp)
-endif(USE_SYSTEM_YAMLCPP)
+endif()
include_directories(${SURGSIM_SOURCE_DIR})
-include_directories(SYSTEM
+include_directories(${CMAKE_CURRENT_BINARY_DIR})
+include_directories(
${Boost_INCLUDE_DIR}
${EIGEN3_INCLUDE_DIR}
+ ${OSG_INCLUDE_DIR}
${YAML_CPP_INCLUDE_DIR}
)
@@ -111,7 +118,6 @@ if(BUILD_TESTING)
add_subdirectory(${GOOGLEMOCK_DIR} GoogleMock)
endif()
-unset(EXPORT_TARGETS CACHE)
add_subdirectory(SurgSim)
option(BUILD_EXAMPLES "Build the examples." ON)
@@ -120,6 +126,8 @@ if(BUILD_EXAMPLES)
add_subdirectory(Examples)
endif()
+add_subdirectory(Modules)
+
option(BUILD_DOCUMENTATION "Build the documentation using Doxygen." OFF)
if(BUILD_DOCUMENTATION)
@@ -137,11 +145,11 @@ file(RELATIVE_PATH REL_INCLUDE_DIR "${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFI
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)
+ 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})
+ 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}")
@@ -165,4 +173,13 @@ surgsim_cpplint_this_tree(cpplint)
# 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")
+set(OPENSURGSIM_LIBRARIES ${SURGSIM_EXPORT_TARGETS} CACHE INTERNAL "OSS Exported Libraries")
+
+# Copy OpenSurgSim Data folder containing common shaders and fonts to the Build folder
+file(COPY Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+
+option(BUILD_TOOLS "Build Tool Utilities" OFF)
+
+if(BUILD_TOOLS)
+ add_subdirectory(Tools)
+endif(BUILD_TOOLS)
diff --git a/Data/Fonts/COPYRIGHT.TXT b/Data/Fonts/COPYRIGHT.TXT
new file mode 100644
index 0000000..0a74863
--- /dev/null
+++ b/Data/Fonts/COPYRIGHT.TXT
@@ -0,0 +1,57 @@
+Bitstream Vera Fonts Copyright
+
+The fonts have a generous copyright, allowing derivative works (as
+long as "Bitstream" or "Vera" are not in the names), and full
+redistribution (so long as they are not *sold* by themselves). They
+can be be bundled, redistributed and sold with any software.
+
+The fonts are distributed under the following copyright:
+
+Copyright
+=========
+
+Copyright (c) 2003 by Bitstream, Inc. All Rights Reserved. Bitstream
+Vera is a trademark of Bitstream, Inc.
+
+Permission is hereby granted, free of charge, to any person obtaining
+a copy of the fonts accompanying this license ("Fonts") and associated
+documentation files (the "Font Software"), to reproduce and distribute
+the Font Software, including without limitation the rights to use,
+copy, merge, publish, distribute, and/or sell copies of the Font
+Software, and to permit persons to whom the Font Software is furnished
+to do so, subject to the following conditions:
+
+The above copyright and trademark notices and this permission notice
+shall be included in all copies of one or more of the Font Software
+typefaces.
+
+The Font Software may be modified, altered, or added to, and in
+particular the designs of glyphs or characters in the Fonts may be
+modified and additional glyphs or characters may be added to the
+Fonts, only if the fonts are renamed to names not containing either
+the words "Bitstream" or the word "Vera".
+
+This License becomes null and void to the extent applicable to Fonts
+or Font Software that has been modified and is distributed under the
+"Bitstream Vera" names.
+
+The Font Software may be sold as part of a larger software package but
+no copy of one or more of the Font Software typefaces may be sold by
+itself.
+
+THE FONT SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO ANY WARRANTIES OF
+MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT
+OF COPYRIGHT, PATENT, TRADEMARK, OR OTHER RIGHT. IN NO EVENT SHALL
+BITSTREAM OR THE GNOME FOUNDATION BE LIABLE FOR ANY CLAIM, DAMAGES OR
+OTHER LIABILITY, INCLUDING ANY GENERAL, SPECIAL, INDIRECT, INCIDENTAL,
+OR CONSEQUENTIAL DAMAGES, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+OTHERWISE, ARISING FROM, OUT OF THE USE OR INABILITY TO USE THE FONT
+SOFTWARE OR FROM OTHER DEALINGS IN THE FONT SOFTWARE.
+
+Except as contained in this notice, the names of Gnome, the Gnome
+Foundation, and Bitstream Inc., shall not be used in advertising or
+otherwise to promote the sale, use or other dealings in this Font
+Software without prior written authorization from the Gnome Foundation
+or Bitstream Inc., respectively. For further information, contact:
+fonts at gnome dot org.
\ No newline at end of file
diff --git a/Data/Fonts/Vera.ttf b/Data/Fonts/Vera.ttf
new file mode 100644
index 0000000..58cd6b5
Binary files /dev/null and b/Data/Fonts/Vera.ttf differ
diff --git a/Data/Shaders/bilateral_blur.frag b/Data/Shaders/bilateral_blur.frag
new file mode 100644
index 0000000..e6512f4
--- /dev/null
+++ b/Data/Shaders/bilateral_blur.frag
@@ -0,0 +1,68 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file bilateral_blur.frag
+/// Fragment Shader, for a bilateral filtering blur, used in both the
+/// vertical and horizontal pass
+
+/// Use version 120 for const array capability
+#version 120
+
+/// Texture to sample, e.g. on a ScreenSpaceQuad
+uniform sampler2D texture;
+
+/// Texture coordinates for sampling
+varying vec2 texCoord0;
+varying vec2 taps[7];
+
+/// Fixed weights for blur
+const float weights[7] = float[](
+ 0.04779035227281,
+ 0.11086490165864,
+ 0.21078608625031,
+ 0.26111731963647,
+ 0.21078608625031,
+ 0.11086490165864,
+ 0.04779035227281);
+
+void main(void)
+{
+ float depth = texture2D(texture, texCoord0).x;
+ float result = 0.0;
+ float normalization = 0.0;
+
+ for (int i = 0; i < 7; i++)
+ {
+ float sample = texture2D(texture, taps[i]).x;
+ float gaussian = weights[i];
+
+ float closeness = distance(sample, depth); // length(vec3(1,1,1));
+
+ if(closeness < 0.6)
+ {
+ float sampleWeight = exp(-closeness * closeness) * gaussian;
+
+ result += sample * sampleWeight;
+ normalization += sampleWeight;
+ }
+ else
+ {
+ result += depth * gaussian;
+ normalization += gaussian;
+ }
+ }
+
+ gl_FragDepth = result / normalization;
+}
diff --git a/Data/Shaders/depth_map.frag b/Data/Shaders/depth_map.frag
index 1f164b5..788249c 100644
--- a/Data/Shaders/depth_map.frag
+++ b/Data/Shaders/depth_map.frag
@@ -17,15 +17,7 @@
/// 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);
+ gl_FragDepth = gl_FragCoord.z;
}
\ No newline at end of file
diff --git a/Data/Shaders/dns_mapping_material.frag b/Data/Shaders/dns_mapping_material.frag
new file mode 100644
index 0000000..0db86fa
--- /dev/null
+++ b/Data/Shaders/dns_mapping_material.frag
@@ -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.
+
+#version 120
+
+/// \file dns_mapping_material.frag
+/// Phong material with diffuse, shadow and normal map
+
+/// Material supplied values
+uniform sampler2D diffuseMap;
+uniform sampler2D normalMap;
+uniform sampler2D shadowMap;
+
+uniform float shininess;
+
+uniform float normalCorrection = -1.0;
+
+/// Oss supplied values
+struct LightSource {
+ vec4 diffuse;
+ vec4 specular;
+ vec4 position;
+ float constantAttenuation;
+ float linearAttenuation;
+ float quadraticAttenuation;
+};
+
+uniform LightSource lightSource;
+
+/// Fragment shader supplied values
+varying vec3 lightDir;
+varying vec3 eyeDir;
+
+varying vec2 texCoord0;
+varying vec4 clipCoord;
+
+varying vec3 vertexDiffuseColor;
+varying vec3 vertexSpecularColor;
+varying vec3 vertexAmbientColor;
+
+void main(void)
+{
+ vec2 shadowCoord = clipCoord.xy / clipCoord.w * vec2(0.5) + vec2(0.5);
+ float shadowAmount = 1.0 - texture2D(shadowMap, shadowCoord).r;
+
+ vec3 normalDir = texture2D(normalMap, texCoord0).rgb * 2.0 - 1.0;
+ normalDir.g = normalCorrection * normalDir.g;
+
+ vec3 lightDirNorm = normalize(lightDir);
+ vec3 eyeDirNorm = normalize(eyeDir);
+ vec3 normalDirNorm = normalize(normalDir);
+
+ float diffuse = max(dot(lightDirNorm, normalDirNorm), 0.0);
+
+ vec3 vDiffuse = vertexDiffuseColor * diffuse * shadowAmount;
+
+ float temp = max(dot(reflect(lightDirNorm, normalDirNorm), eyeDirNorm), 0.0);
+ float specular = temp / (shininess - temp * shininess + temp);
+ vec3 vSpecular = vertexSpecularColor * specular * shadowAmount;
+
+ vec3 base = texture2D(diffuseMap, texCoord0).rgb;
+ vec3 color = (vertexAmbientColor + vDiffuse) * base + vSpecular;
+
+ gl_FragColor.rgb = color;
+ gl_FragColor.a = 1.0;
+}
\ No newline at end of file
diff --git a/Data/Shaders/dns_mapping_material.vert b/Data/Shaders/dns_mapping_material.vert
new file mode 100644
index 0000000..3cb934c
--- /dev/null
+++ b/Data/Shaders/dns_mapping_material.vert
@@ -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.
+
+/// \file dns_mapping_material.frag
+/// Phong material with diffuse, shadow and normal map
+
+// Material provided values
+uniform vec4 diffuseColor;
+uniform vec4 specularColor;
+
+// OSS provided values
+uniform mat4 viewMatrix;
+
+uniform vec4 ambientColor;
+
+struct LightSource {
+ vec4 diffuse;
+ vec4 specular;
+ vec4 position;
+ float constantAttenuation;
+ float linearAttenuation;
+ float quadraticAttenuation;
+};
+
+uniform LightSource lightSource;
+
+attribute vec3 tangent;
+attribute vec3 bitangent;
+
+/// Outgoing
+varying vec3 lightDir;
+varying vec3 eyeDir;
+
+varying vec2 texCoord0;
+varying vec4 clipCoord;
+
+varying vec3 vertexDiffuseColor;
+varying vec3 vertexSpecularColor;
+varying vec3 vertexAmbientColor;
+
+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;
+
+ vec3 n = normalize(gl_NormalMatrix * gl_Normal);
+ vec3 t = normalize(gl_NormalMatrix * tangent);
+ vec3 b = normalize(gl_NormalMatrix * bitangent);
+
+ vec3 v;
+ vec3 l = (viewMatrix * lightSource.position).xyz - eyeDir4.xyz;
+ float lightDistance = length(l);
+ v.x = dot(l, t);
+ v.y = dot(l, b);
+ v.z = dot(l, n);
+ lightDir = normalize(v);
+
+ float eyeDistance = length(eyeDir);
+ v.x = dot(eyeDir, t);
+ v.y = dot(eyeDir, b);
+ v.z = dot(eyeDir, n);
+ eyeDir = normalize(v);
+
+ 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;
+ vertexAmbientColor = (ambientColor * diffuseColor).xyz;
+}
diff --git a/Data/Shaders/dns_mapping_material_twosided.frag b/Data/Shaders/dns_mapping_material_twosided.frag
new file mode 100644
index 0000000..2e3b117
--- /dev/null
+++ b/Data/Shaders/dns_mapping_material_twosided.frag
@@ -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.
+
+/// \file dns_mapping_material.frag
+/// Phong material with diffuse, shadow and normal map
+
+/// Material supplied values
+uniform sampler2D diffuseMap;
+uniform sampler2D normalMap;
+uniform sampler2D shadowMap;
+
+uniform float shininess;
+
+/// Fragment shader supplied values
+varying vec3 lightDir;
+varying vec3 eyeDir;
+
+varying vec2 texCoord0;
+varying vec4 clipCoord;
+
+varying vec3 vertexDiffuseColor;
+varying vec3 vertexSpecularColor;
+varying vec3 vertexAmbientColor;
+
+void calculateLighting(
+ in vec3 l, in vec3 n, in vec3 v, in float shininess, in float shadow,
+ inout vec3 diffuse, inout vec3 specular)
+{
+
+ float dotNL = max(dot(l, n), 0.0);
+
+ diffuse = diffuse * dotNL * shadow;
+
+ float temp = max(dot(reflect(l, n), v), 0.0);
+ float specularity = temp / (shininess - temp * shininess + temp);
+
+ specular = specular * specularity * shadow;
+
+}
+
+void main(void)
+{
+ vec2 shadowCoord = clipCoord.xy / clipCoord.w * vec2(0.5) + vec2(0.5);
+ float shadowAmount = 1.0 - texture2D(shadowMap, shadowCoord).r;
+
+ vec3 normalDir = texture2D(normalMap, texCoord0).rgb * 2.0 - 1.0;
+ normalDir.g = -normalDir.g;
+
+ vec3 lightDirNorm = normalize(lightDir);
+ vec3 eyeDirNorm = normalize(eyeDir);
+ vec3 normalDirNorm = normalize(normalDir);
+
+ vec3 base = texture2D(diffuseMap, texCoord0).rgb;
+
+ vec3 vDiffuse = vertexDiffuseColor;
+ vec3 vSpecular = vertexSpecularColor;
+
+ calculateLighting(lightDirNorm, normalDirNorm, eyeDirNorm,
+ shininess, shadowAmount, vDiffuse, vSpecular);
+
+ vec3 color = vDiffuse * base + vSpecular;
+
+ vDiffuse = vertexDiffuseColor;
+ vSpecular = vertexSpecularColor;
+
+ calculateLighting(lightDirNorm, -normalDirNorm, eyeDirNorm,
+ shininess, shadowAmount, vDiffuse, vSpecular);
+
+ color += vDiffuse * base + vSpecular;
+
+ color += vertexAmbientColor * base;
+
+ gl_FragColor.rgb = color;
+ gl_FragColor.a = 1.0;
+}
\ No newline at end of file
diff --git a/Data/Shaders/dns_mapping_multitexture.frag b/Data/Shaders/dns_mapping_multitexture.frag
new file mode 100644
index 0000000..0fb474b
--- /dev/null
+++ b/Data/Shaders/dns_mapping_multitexture.frag
@@ -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.
+
+#version 120
+
+/// \file dns_mapping_material.frag
+/// Phong material with diffuse, shadow and normal map
+
+/// Material supplied values
+uniform sampler2D diffuseMap;
+uniform sampler2D normalMap;
+uniform sampler2D shadowMap;
+uniform sampler2D paintMap;
+
+uniform float shininess;
+
+uniform float normalCorrection = -1.0;
+
+/// Oss supplied values
+struct LightSource {
+ vec4 diffuse;
+ vec4 specular;
+ vec4 position;
+ float constantAttenuation;
+ float linearAttenuation;
+ float quadraticAttenuation;
+};
+
+uniform LightSource lightSource;
+
+/// Fragment shader supplied values
+varying vec3 lightDir;
+varying vec3 eyeDir;
+
+varying vec2 texCoord0;
+varying vec4 clipCoord;
+
+varying vec3 vertexDiffuseColor;
+varying vec3 vertexSpecularColor;
+varying vec3 vertexAmbientColor;
+
+void main(void)
+{
+ vec2 shadowCoord = clipCoord.xy / clipCoord.w * vec2(0.5) + vec2(0.5);
+ float shadowAmount = 1.0 - texture2D(shadowMap, shadowCoord).r;
+
+ vec3 normalDir = texture2D(normalMap, texCoord0).rgb * 2.0 - 1.0;
+ normalDir.g = normalCorrection * normalDir.g;
+
+ vec3 lightDirNorm = normalize(lightDir);
+ vec3 eyeDirNorm = normalize(eyeDir);
+ vec3 normalDirNorm = normalize(normalDir);
+
+ vec4 paint = texture2D(paintMap, texCoord0);
+
+ float diffuse = max(dot(lightDirNorm, normalDirNorm), 0.0);
+
+ vec3 vDiffuse = mix(vertexDiffuseColor * diffuse, paint.rgb, paint.a) * shadowAmount;
+
+ float temp = max(dot(reflect(lightDirNorm, normalDirNorm), eyeDirNorm), 0.0);
+ float specular = temp / (shininess - temp * shininess + temp);
+ vec3 vSpecular = vertexSpecularColor * specular * shadowAmount;
+
+ vec3 base = texture2D(diffuseMap, texCoord0).rgb;
+ vec3 color = (vertexAmbientColor + 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.frag b/Data/Shaders/ds_mapping_material.frag
index 2a440b9..e48333a 100644
--- a/Data/Shaders/ds_mapping_material.frag
+++ b/Data/Shaders/ds_mapping_material.frag
@@ -21,9 +21,6 @@ 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;
@@ -33,28 +30,28 @@ varying vec2 texCoord0;
varying vec3 vertexDiffuseColor;
varying vec3 vertexSpecularColor;
+varying vec3 vertexAmbientColor;
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;
+ vec3 lightDirNorm = normalize(lightDir);
+ vec3 eyeDirNorm = normalize(eyeDir);
+ vec3 normalDirNorm = normalize(normalDir);
- float diffuse = max(dot(lightDir, normalDir), 0.0);
+ float diffuse = max(dot(lightDirNorm, normalDirNorm), 0.0);
vec3 vDiffuse = vertexDiffuseColor * diffuse * shadowAmount;
- float temp = max(dot(reflect(lightDir, normalDir), eyeDir), 0.0);
+ float temp = max(dot(reflect(lightDirNorm, normalDirNorm), eyeDirNorm), 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;
+ vec3 color = (vertexAmbientColor + vDiffuse) * base + vSpecular;
gl_FragColor.rgb = color;
gl_FragColor.a = 1.0;
diff --git a/Data/Shaders/ds_mapping_material.vert b/Data/Shaders/ds_mapping_material.vert
index 2d88948..0c0e476 100644
--- a/Data/Shaders/ds_mapping_material.vert
+++ b/Data/Shaders/ds_mapping_material.vert
@@ -22,7 +22,7 @@ uniform vec4 specularColor;
// Oss provided uniforms
uniform mat4 viewMatrix;
-
+uniform vec4 ambientColor;
struct LightSource {
vec4 diffuse;
@@ -46,6 +46,7 @@ varying vec4 clipCoord; ///< Projected and transformed vertex coordinates
varying vec3 vertexDiffuseColor;
varying vec3 vertexSpecularColor;
+varying vec3 vertexAmbientColor;
void main(void)
@@ -71,4 +72,5 @@ void main(void)
vertexDiffuseColor = (attenuation * diffuseColor * lightSource.diffuse).xyz;
vertexSpecularColor = (attenuation * specularColor * lightSource.specular).xyz;
+ vertexAmbientColor = (diffuseColor * ambientColor).xyz;
}
diff --git a/Data/Shaders/ds_mapping_material_twosided.frag b/Data/Shaders/ds_mapping_material_twosided.frag
new file mode 100644
index 0000000..c1aa1c9
--- /dev/null
+++ b/Data/Shaders/ds_mapping_material_twosided.frag
@@ -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 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;
+
+// Incoming from the vertex shader
+varying vec3 lightDir;
+varying vec3 eyeDir;
+varying vec3 normalDir;
+
+varying vec2 texCoord0;
+
+varying vec3 vertexDiffuseColor;
+varying vec3 vertexSpecularColor;
+varying vec3 vertexAmbientColor;
+
+varying vec4 clipCoord;
+
+
+void calculateLighting(
+ in vec3 l, in vec3 n, in vec3 v, in float shininess, in float shadow,
+ inout vec3 diffuse, inout vec3 specular)
+{
+
+ float dotNL = max(dot(l, n), 0.0);
+
+ diffuse = diffuse * dotNL * shadow;
+
+ float temp = max(dot(reflect(l, n), v), 0.0);
+ float specularity = temp / (shininess - temp * shininess + temp);
+
+ specular = specular * specularity * shadow;
+
+}
+
+void main(void)
+{
+ vec2 shadowCoord = clipCoord.xy / clipCoord.w * vec2(0.5) + vec2(0.5);
+ float shadowAmount = 1.0 - texture2D(shadowMap, shadowCoord).r;
+
+ vec3 lightDirNorm = normalize(lightDir);
+ vec3 eyeDirNorm = normalize(eyeDir);
+ vec3 normalDirNorm = normalize(normalDir);
+
+ vec3 base = texture2D(diffuseMap, texCoord0).rgb;
+
+ vec3 vDiffuse = vertexDiffuseColor;
+ vec3 vSpecular = vertexSpecularColor;
+
+ calculateLighting(lightDirNorm, normalDirNorm, eyeDirNorm,
+ shininess, shadowAmount, vDiffuse, vSpecular);
+
+ vec3 color = vDiffuse * base + vSpecular;
+
+ vDiffuse = vertexDiffuseColor;
+ vSpecular = vertexSpecularColor;
+
+ calculateLighting(lightDirNorm, -normalDirNorm, eyeDirNorm,
+ shininess, shadowAmount, vDiffuse, vSpecular);
+
+ color += vDiffuse * base + vSpecular;
+
+ color += vertexAmbientColor * base;
+
+ gl_FragColor.rgb = color ;
+ gl_FragColor.a = 1.0;
+}
\ No newline at end of file
diff --git a/Data/Shaders/ds_mapping_multitexture.frag b/Data/Shaders/ds_mapping_multitexture.frag
new file mode 100644
index 0000000..0e08ea7
--- /dev/null
+++ b/Data/Shaders/ds_mapping_multitexture.frag
@@ -0,0 +1,61 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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;
+uniform sampler2D paintMap;
+
+// Incoming from the vertex shader
+varying vec3 lightDir;
+varying vec3 eyeDir;
+varying vec3 normalDir;
+
+varying vec2 texCoord0;
+
+varying vec3 vertexDiffuseColor;
+varying vec3 vertexSpecularColor;
+varying vec3 vertexAmbientColor;
+
+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;
+
+ vec3 lightDirNorm = normalize(lightDir);
+ vec3 eyeDirNorm = normalize(eyeDir);
+ vec3 normalDirNorm = normalize(normalDir);
+
+ vec4 paint = texture2D(paintMap, texCoord0);
+
+ float diffuse = max(dot(lightDirNorm, normalDirNorm), 0.0);
+ vec3 vDiffuse = mix(vertexDiffuseColor * diffuse, paint.rgb, paint.a) * shadowAmount;
+
+ float temp = max(dot(reflect(lightDirNorm, normalDirNorm), eyeDirNorm), 0.0);
+ float specular = temp / (shininess - temp * shininess + temp);
+ vec3 vSpecular = vertexSpecularColor * specular * shadowAmount;
+
+ vec3 base = texture2D(diffuseMap, texCoord0).rgb;
+ vec3 color = (vertexAmbientColor + vDiffuse) * base + vSpecular;
+
+ gl_FragColor.rgb = color;
+ gl_FragColor.a = 1.0;
+}
\ No newline at end of file
diff --git a/Data/Shaders/gauss_blur.frag b/Data/Shaders/gauss_blur.frag
new file mode 100644
index 0000000..e3a51e3
--- /dev/null
+++ b/Data/Shaders/gauss_blur.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 gauss_blur.frag
+/// Fragment Shader, for a simple 2-pass gaussian blur, used in both the
+/// vertical and horizontal pass
+
+/// Use version 120 for const array capability
+#version 120
+
+/// Texture to sample, e.g. on a ScreenSpaceQuad
+uniform sampler2D texture;
+
+/// Texture coordinates for sampling
+varying vec2 taps[7];
+
+/// Fixed weights for blur
+const float weights[7] = float[](
+ 0.04779035227281,
+ 0.11086490165864,
+ 0.21078608625031,
+ 0.26111731963647,
+ 0.21078608625031,
+ 0.11086490165864,
+ 0.04779035227281);
+
+void main(void)
+{
+ vec4 sampledValue = vec4(0.0);
+
+ sampledValue += weights[0] * texture2D(texture, taps[0]);
+ sampledValue += weights[1] * texture2D(texture, taps[1]);
+ sampledValue += weights[2] * texture2D(texture, taps[2]);
+ sampledValue += weights[3] * texture2D(texture, taps[3]);
+ sampledValue += weights[4] * texture2D(texture, taps[4]);
+ sampledValue += weights[5] * texture2D(texture, taps[5]);
+ sampledValue += weights[6] * texture2D(texture, taps[6]);
+
+ gl_FragColor.rgba = sampledValue;
+}
diff --git a/Data/Shaders/gauss_blur_horizontal.vert b/Data/Shaders/gauss_blur_horizontal.vert
new file mode 100644
index 0000000..9edb0fa
--- /dev/null
+++ b/Data/Shaders/gauss_blur_horizontal.vert
@@ -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.
+
+/// \file gauss_blur_horizontal.vert
+/// Vertex Shader, for a simple 2-pass gaussian blur, horizontal pass
+
+/// Width of the Texture that is incoming
+uniform float width;
+
+/// Sampling radius
+uniform float blurRadius;
+
+varying vec2 taps[7];
+
+void main(void)
+{
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+
+ vec2 texCoord0 = gl_MultiTexCoord0.xy;
+
+ float dx = (blurRadius / 3.0) / width;
+
+ taps[0] = texCoord0 - vec2(-3.0, 0.0) * dx;
+ taps[1] = texCoord0 - vec2(-2.0, 0.0) * dx;
+ taps[2] = texCoord0 - vec2(-1.0, 0.0) * dx;
+ taps[3] = texCoord0 - vec2(0.0, 0.0) * dx;
+ taps[4] = texCoord0 - vec2(1.0, 0.0) * dx;
+ taps[5] = texCoord0 - vec2(2.0, 0.0) * dx;
+ taps[6] = texCoord0 - vec2(3.0, 0.0) * dx;
+}
+
diff --git a/Data/Shaders/gauss_blur_vertical.vert b/Data/Shaders/gauss_blur_vertical.vert
new file mode 100644
index 0000000..5a05f21
--- /dev/null
+++ b/Data/Shaders/gauss_blur_vertical.vert
@@ -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.
+
+/// \file verticalBlurPass.vert
+/// Vertex Shader, for a simple 2-pass gaussian blur, vertical pass
+
+/// Height of the Texture that is incoming
+uniform float height;
+
+/// Sampling radius
+uniform float blurRadius;
+
+/// Prepopulate coordinates for sampling, HW will interpolate
+varying vec2 taps[7];
+
+void main(void)
+{
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+
+ vec2 texCoord0 = gl_MultiTexCoord0.xy;
+
+ float dy = (blurRadius / 3.0) / height;
+
+ taps[0] = texCoord0 - vec2(0.0, -3.0) * dy;
+ taps[1] = texCoord0 - vec2(0.0, -2.0) * dy;
+ taps[2] = texCoord0 - vec2(0.0, -1.0) * dy;
+ taps[3] = texCoord0 - vec2(0.0, 0.0) * dy;
+ taps[4] = texCoord0 - vec2(0.0, 1.0) * dy;
+ taps[5] = texCoord0 - vec2(0.0, 2.0) * dy;
+ taps[6] = texCoord0 - vec2(0.0, 3.0) * dy;
+}
+
\ No newline at end of file
diff --git a/Data/Shaders/implicit_surface/depth.frag b/Data/Shaders/implicit_surface/depth.frag
new file mode 100644
index 0000000..0c028df
--- /dev/null
+++ b/Data/Shaders/implicit_surface/depth.frag
@@ -0,0 +1,58 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file sphere_depth.frag
+/// Fragment Shader to calculate depth of point sprite spheres
+/// Most of the equations and concepts come from the nVidia
+/// Screen Space Fluid Rendering paper and presentation from GDC '10
+/// found here: http://developer.download.nvidia.com/presentations/2010/gdc/Direct3D_Effects.pdf
+
+#version 120
+
+// These are 'free' uniforms to be set for this shader, they won't be provided by OSS
+uniform float sphereRadius;
+
+// Incoming from the vertex shader
+varying vec3 eyeSpacePos;
+
+// Main Camera Matrices
+struct MainCamera
+{
+ mat4 viewMatrix;
+ mat4 inverseViewMatrix;
+ mat4 projectionMatrix;
+ mat4 inverseProjectionMatrix;
+};
+
+uniform MainCamera mainCamera;
+
+void main(void)
+{
+ // calculate normal from texture coordinates provided by gl_PointCoord
+ vec3 normal;
+ normal.xy = gl_PointCoord.st * 2.0 - vec2(1.0);
+ float mag = dot(normal.xy, normal.xy);
+ if (mag > 1.0)
+ {
+ discard; // kill pixels outside circle
+ }
+ normal.z = sqrt(1.0-mag);
+
+ // calculate depth
+ vec4 pixelPos = vec4(eyeSpacePos + normal*sphereRadius, 1.0);
+ vec4 clipSpacePos = mainCamera.projectionMatrix * pixelPos;
+ float normDepth = clipSpacePos.z / clipSpacePos.w;
+ gl_FragDepth = 0.5 + 0.5 * normDepth;
+}
diff --git a/Data/Shaders/implicit_surface/depth.vert b/Data/Shaders/implicit_surface/depth.vert
new file mode 100644
index 0000000..54b181f
--- /dev/null
+++ b/Data/Shaders/implicit_surface/depth.vert
@@ -0,0 +1,51 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file sphere_depth.vert
+/// Vertex Shader to calculate depth of point sprite spheres
+/// Most of the equations and concepts come from the nVidia
+/// Screen Space Fluid Rendering paper and presentation from GDC '10
+/// found here: http://developer.download.nvidia.com/presentations/2010/gdc/Direct3D_Effects.pdf
+
+#version 120
+
+// These are 'free' uniforms to be set for this shader, they won't be provided by OSS
+uniform float sphereRadius;
+uniform float sphereScale;
+
+// Main Camera Matrices
+struct MainCamera
+{
+ mat4 viewMatrix;
+ mat4 inverseViewMatrix;
+ mat4 projectionMatrix;
+ mat4 inverseProjectionMatrix;
+};
+
+uniform MainCamera mainCamera;
+
+// OSS provided uniform
+uniform mat4 modelMatrix;
+
+// Outgoing values
+varying vec3 eyeSpacePos;
+
+void main(void)
+{
+ eyeSpacePos = vec3(mainCamera.viewMatrix * modelMatrix * gl_Vertex);
+ float dist = length(eyeSpacePos);
+ gl_PointSize = sphereRadius * (sphereScale / dist);
+ gl_Position = mainCamera.projectionMatrix * mainCamera.viewMatrix * modelMatrix * gl_Vertex;
+}
diff --git a/Data/Shaders/implicit_surface/normal.frag b/Data/Shaders/implicit_surface/normal.frag
new file mode 100644
index 0000000..f7afd57
--- /dev/null
+++ b/Data/Shaders/implicit_surface/normal.frag
@@ -0,0 +1,82 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file sphere_normal.frag
+/// Fragment Shader to calculate normals of a point sprite sphere generated surface
+/// Most of the equations and concepts come from the nVidia
+/// Screen Space Fluid Rendering paper and presentation from GDC '10
+/// found here: http://developer.download.nvidia.com/presentations/2010/gdc/Direct3D_Effects.pdf
+
+#version 120
+
+// These are 'free' uniforms to be set for this shader, they won't be provided by OSS
+uniform sampler2D depthMap;
+uniform float maxDepth = 0.999999f;
+uniform float texelSize = 1.0/1024.0;
+
+// Main Camera Matrices
+struct MainCamera
+{
+ mat4 viewMatrix;
+ mat4 inverseViewMatrix;
+ mat4 projectionMatrix;
+ mat4 inverseProjectionMatrix;
+};
+
+uniform MainCamera mainCamera;
+
+vec3 getEyeSpacePos(vec3 coord)
+{
+ vec3 homogenous = coord * 2.0 - 1.0;
+ vec4 clipSpacePos = vec4(homogenous, 1.0);
+ vec4 eyeSpacePos = mainCamera.inverseProjectionMatrix * clipSpacePos;
+ return eyeSpacePos.xyz/eyeSpacePos.w;
+}
+
+void main(void)
+{
+ float depth = texture2D(depthMap, gl_TexCoord[0].xy).x;
+
+ if(depth > maxDepth)
+ {
+ discard;
+ }
+
+ vec3 eyePos = getEyeSpacePos(vec3(gl_TexCoord[0].xy, depth));
+ vec2 texCoordX1 = gl_TexCoord[0].xy + vec2(texelSize, 0);
+ vec2 texCoordX2 = gl_TexCoord[0].xy - vec2(texelSize, 0);
+
+ vec3 ddx = getEyeSpacePos(vec3(texCoordX1, texture2D(depthMap, texCoordX1).x)) - eyePos;
+ vec3 ddx2 = eyePos - getEyeSpacePos(vec3(texCoordX2, texture2D(depthMap, texCoordX2).x));
+ if(abs(ddx.z) > abs(ddx2.z))
+ {
+ ddx = ddx2;
+ }
+
+ vec2 texCoordY1 = gl_TexCoord[0].xy + vec2(0, texelSize);
+ vec2 texCoordY2 = gl_TexCoord[0].xy - vec2(0, texelSize);
+
+ vec3 ddy = getEyeSpacePos(vec3(texCoordY1, texture2D(depthMap, texCoordY1).x)) - eyePos;
+ vec3 ddy2 = eyePos - getEyeSpacePos(vec3(texCoordY2, texture2D(depthMap, texCoordY2).x));
+ if(abs(ddy.z) > abs(ddy2.z))
+ {
+ ddy = ddy2;
+ }
+
+ vec3 normal = cross(ddx, ddy);
+ normal = normalize(normal);
+
+ gl_FragColor = 0.5 + 0.5 * vec4(normal, 1.0);
+}
diff --git a/Data/Shaders/implicit_surface/normal.vert b/Data/Shaders/implicit_surface/normal.vert
new file mode 100644
index 0000000..e62c113
--- /dev/null
+++ b/Data/Shaders/implicit_surface/normal.vert
@@ -0,0 +1,28 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file sphere_normal.vert
+/// Vertex Shader to calculate normals of a point sprite sphere generated surface
+/// Most of the equations and concepts come from the nVidia
+/// Screen Space Fluid Rendering paper and presentation from GDC '10
+/// found here: http://developer.download.nvidia.com/presentations/2010/gdc/Direct3D_Effects.pdf
+
+#version 120
+
+void main(void)
+{
+ gl_TexCoord[0] = gl_MultiTexCoord0;
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+}
diff --git a/Data/Shaders/implicit_surface/shading.frag b/Data/Shaders/implicit_surface/shading.frag
new file mode 100644
index 0000000..055dc6d
--- /dev/null
+++ b/Data/Shaders/implicit_surface/shading.frag
@@ -0,0 +1,130 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file surface.frag
+/// Fragment Shader to render fluid surface
+/// Most of the equations and concepts come from the nVidia
+/// 'Screen Space Fluid Rendering' paper and presentation from GDC '10
+/// found here: http://developer.download.nvidia.com/presentations/2010/gdc/Direct3D_Effects.pdf
+
+#version 120
+
+// These are 'free' uniforms to be set for this shader, they won't be provided by OSS
+uniform sampler2D lightMap;
+uniform sampler2D depthMap;
+uniform sampler2D normalMap;
+uniform samplerCube diffuseEnvMap;
+uniform samplerCube specularEnvMap;
+uniform vec4 diffuseColor;
+uniform vec4 specularColor;
+uniform float diffusePercent = 0.9;
+uniform float specularPercent = 0.1;
+uniform float shininess;
+
+/// 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;
+
+uniform float bias = 0.0005;
+uniform float intensity = 0.75;
+
+// Main Camera Matrices
+struct MainCamera
+{
+ mat4 viewMatrix;
+ mat4 inverseViewMatrix;
+ mat4 projectionMatrix;
+ mat4 inverseProjectionMatrix;
+};
+
+uniform MainCamera mainCamera;
+
+struct LightSource {
+ vec4 diffuse;
+ vec4 specular;
+ vec4 position;
+ float constantAttenuation;
+ float linearAttenuation;
+ float quadraticAttenuation;
+};
+
+uniform LightSource lightSource;
+
+// Oss provided uniforms
+uniform vec4 ambientColor;
+
+// Incoming from the vertex shader
+varying vec2 texCoord0; ///< Texture unit 0 texture coordinates
+varying vec4 clipCoord; ///< Projected and transformed vertex coordinates
+
+vec3 getEyeSpacePos(vec3 coord)
+{
+ vec3 homogenous = coord * 2.0 - 1.0;
+ vec4 clipSpacePos = vec4(homogenous, 1.0);
+ vec4 eyeSpacePos = mainCamera.inverseProjectionMatrix * clipSpacePos;
+ return eyeSpacePos.xyz/eyeSpacePos.w;
+}
+
+void main(void)
+{
+ float maxDepth = 0.999999f;
+ float depth = texture2D(depthMap, texCoord0).x;
+ if (depth > maxDepth)
+ {
+ discard;
+ }
+
+ vec4 eyeDir4 = vec4(getEyeSpacePos(vec3(texCoord0, depth)), 1.0);
+
+ vec4 lightCoord = lightProjectionMatrix * lightViewMatrix * (mainCamera.inverseViewMatrix * eyeDir4);
+ vec3 lightCoord3 = (lightCoord.xyz / lightCoord.w) * vec3(0.5) + vec3(0.5);
+ float lightDepth = texture2D(lightMap, lightCoord3.xy).x;
+ float shadowAmount = 1.0 - (lightDepth + bias > lightCoord3.z || lightCoord3.z > 1.0 ? 0.0 : intensity);
+
+ vec3 lightDir = (mainCamera.viewMatrix * lightSource.position - eyeDir4).xyz;
+ float lightDistance = length(lightDir);
+
+ float attenuation = 1.0 / (lightSource.constantAttenuation + lightSource.linearAttenuation*lightDistance + lightSource.quadraticAttenuation*lightDistance*lightDistance);
+
+ vec3 normal = (texture2D(normalMap, texCoord0).xyz * 2.0) - 1.0;
+
+ vec3 lightDirNorm = normalize(lightDir);
+ vec3 eyeDirNorm = normalize(eyeDir4.xyz);
+ vec3 normalDirNorm = normalize(normal);
+
+ vec3 vAmbient = ambientColor.rgb * diffuseColor.rgb;
+
+ vec3 vertexDiffuseColor = (attenuation * diffuseColor * lightSource.diffuse).rgb;
+ vec3 vertexSpecularColor = (attenuation * specularColor * lightSource.specular).rgb;
+
+ float diffuse = max(dot(lightDirNorm, normalDirNorm), 0.0);
+ vec3 vDiffuse = vec3(textureCube(diffuseEnvMap, normalDirNorm)) * vertexDiffuseColor * diffuse;
+
+ vec3 color = mix(vAmbient, vDiffuse, diffusePercent);
+
+ float temp = max(dot(reflect(lightDirNorm, normalDirNorm), eyeDirNorm), 0.0);
+ float specular = temp / (shininess - temp * shininess + temp);
+ vec3 reflectDir = reflect(eyeDirNorm, normalDirNorm);
+ vec3 vSpecular = vec3(textureCube(specularEnvMap, reflectDir)) * vertexSpecularColor;
+
+ color = (mix(color, vSpecular + color, specularPercent) +
+ (specular * specularColor * lightSource.specular * attenuation).rgb) * shadowAmount;
+
+ gl_FragColor.rgb = color;
+ gl_FragColor.a = 1.0;
+ gl_FragDepth = depth;
+}
diff --git a/Data/Shaders/implicit_surface/shading.vert b/Data/Shaders/implicit_surface/shading.vert
new file mode 100644
index 0000000..dc93041
--- /dev/null
+++ b/Data/Shaders/implicit_surface/shading.vert
@@ -0,0 +1,34 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file surface.vert
+/// Vertex Shader to render fluid surface
+/// Most of the equations and concepts come from the nVidia
+/// 'Screen Space Fluid Rendering' paper and presentation from GDC '10
+/// found here: http://developer.download.nvidia.com/presentations/2010/gdc/Direct3D_Effects.pdf
+
+#version 120
+
+// Outgoing Values
+varying vec2 texCoord0; ///< Texture unit 0 texture coordinates
+varying vec4 clipCoord; ///< Projected and transformed vertex coordinates
+
+void main(void)
+{
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+ clipCoord = gl_Position;
+
+ texCoord0 = gl_MultiTexCoord0.xy;
+}
diff --git a/Data/Shaders/material.frag b/Data/Shaders/material.frag
index 1a16d53..051795c 100644
--- a/Data/Shaders/material.frag
+++ b/Data/Shaders/material.frag
@@ -19,9 +19,6 @@
// 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;
@@ -31,22 +28,26 @@ varying vec2 texCoord0;
varying vec3 vertexDiffuseColor;
varying vec3 vertexSpecularColor;
+varying vec3 vertexAmbientColor;
varying vec4 clipCoord;
+
void main(void)
{
- vec3 vAmbient = ambientColor.xyz; // Old Term ... osg_ambientColor * _lightColor;
-
- float diffuse = max(dot(lightDir, normalDir), 0.0);
+ vec3 lightDirNorm = normalize(lightDir);
+ vec3 eyeDirNorm = normalize(eyeDir);
+ vec3 normalDirNorm = normalize(normalDir);
+
+ float diffuse = max(dot(lightDirNorm, normalDirNorm), 0.0);
vec3 vDiffuse = vertexDiffuseColor * diffuse;
- float temp = max(dot(reflect(lightDir, normalDir), eyeDir), 0.0);
+ float temp = max(dot(reflect(lightDirNorm, normalDirNorm), eyeDirNorm), 0.0);
float specular = temp / (shininess - temp * shininess + temp);
vec3 vSpecular = vertexSpecularColor * specular;
- vec3 color = vAmbient + vDiffuse + vSpecular;
+ vec3 color = vertexAmbientColor + 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
index 08c4513..221cf8b 100644
--- a/Data/Shaders/material.vert
+++ b/Data/Shaders/material.vert
@@ -23,14 +23,15 @@ uniform vec4 specularColor;
// Oss provided uniforms
uniform mat4 viewMatrix;
+uniform vec4 ambientColor;
struct LightSource {
- vec4 diffuse;
- vec4 specular;
- vec4 position;
- float constantAttenuation;
- float linearAttenuation;
- float quadraticAttenuation;
+ vec4 diffuse;
+ vec4 specular;
+ vec4 position;
+ float constantAttenuation;
+ float linearAttenuation;
+ float quadraticAttenuation;
};
uniform LightSource lightSource;
@@ -46,29 +47,31 @@ varying vec4 clipCoord; ///< Projected and transformed vertex coordinates
varying vec3 vertexDiffuseColor;
varying vec3 vertexSpecularColor;
-
+varying vec3 vertexAmbientColor;
void main(void)
{
gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
clipCoord = gl_Position;
-
+
vec4 eyeDir4 = gl_ModelViewMatrix * gl_Vertex;
- eyeDir = eyeDir4.xyz;
-
+ 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;
+
+ vertexDiffuseColor = (attenuation * diffuseColor * lightSource.diffuse).xyz;
vertexSpecularColor = (attenuation * specularColor * lightSource.specular).xyz;
-}
+
+ vertexAmbientColor = ambientColor.xyz * diffuseColor.xyz;
+}
diff --git a/Data/Shaders/material_curve.vert b/Data/Shaders/material_curve.vert
new file mode 100644
index 0000000..18cc03a
--- /dev/null
+++ b/Data/Shaders/material_curve.vert
@@ -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.
+
+/// \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);
+
+ // In the case of the curve gl_Normal carries the information about the segment
+ // vector from this vertex to the next
+ vec3 segmentDir = gl_NormalMatrix * gl_Normal;
+
+ // Calculate the normal of the segment that is in the plane spanned between
+ // lightDir and segmentDir, i.e. the normal that is closest to the light direction
+ if (abs(dot(lightDir, segmentDir)) > 1 - 0.0001)
+ {
+ normalDir = lightDir;
+ }
+ else
+ {
+ normalDir = cross(lightDir, segmentDir);
+ }
+
+ normalDir = -cross(normalDir, segmentDir);
+
+ 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_multitexture.frag b/Data/Shaders/material_multitexture.frag
new file mode 100644
index 0000000..c14e4d0
--- /dev/null
+++ b/Data/Shaders/material_multitexture.frag
@@ -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 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;
+uniform sampler2D paintMap;
+
+// Incoming from the vertex shader
+varying vec3 lightDir;
+varying vec3 eyeDir;
+varying vec3 normalDir;
+
+varying vec2 texCoord0;
+
+varying vec3 vertexDiffuseColor;
+varying vec3 vertexSpecularColor;
+varying vec3 vertexAmbientColor;
+
+varying vec4 clipCoord;
+
+
+void main(void)
+{
+ Nvec3 lightDirNorm = normalize(lightDir);
+ vec3 eyeDirNorm = normalize(eyeDir);
+ vec3 normalDirNorm = normalize(normalDir);
+
+ vec4 decal = texture2D(paintMap, texCoord0);
+
+ float diffuse = max(dot(lightDirNorm, normalDirNorm), 0.0);
+ vec3 vDiffuse = mix(vertexDiffuseColor * diffuse, decal.rgb, decal.a);
+
+ float temp = max(dot(reflect(lightDirNorm, normalDirNorm), eyeDirNorm), 0.0);
+ float specular = temp / (shininess - temp * shininess + temp);
+ vec3 vSpecular = vertexSpecularColor * specular;
+
+ vec4 color = vec4(vertexAmbientColor + vDiffuse + vSpecular, 1.0);
+
+ gl_FragColor = color;
+}
diff --git a/Data/Shaders/s_mapping_material.frag b/Data/Shaders/s_mapping_material.frag
new file mode 100644
index 0000000..e2de2dc
--- /dev/null
+++ b/Data/Shaders/s_mapping_material.frag
@@ -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.
+
+/// \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 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;
+
+ vec3 lightDirNorm = normalize(lightDir);
+ vec3 eyeDirNorm = normalize(eyeDir);
+ vec3 normalDirNorm = normalize(normalDir);
+
+ float diffuse = max(dot(lightDirNorm, normalDirNorm), 0.0);
+ vec3 vDiffuse = vertexDiffuseColor * diffuse * shadowAmount;
+
+ float temp = max(dot(reflect(lightDirNorm, normalDirNorm), eyeDirNorm), 0.0);
+ float specular = temp / (shininess - temp * shininess + temp);
+ vec3 vSpecular = vertexSpecularColor * specular * shadowAmount;
+
+ 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/ds_mapping_material.vert b/Data/Shaders/s_mapping_material.vert
similarity index 100%
copy from Data/Shaders/ds_mapping_material.vert
copy to Data/Shaders/s_mapping_material.vert
diff --git a/Data/Shaders/s_mapping_metal.frag b/Data/Shaders/s_mapping_metal.frag
new file mode 100644
index 0000000..4a7e9c5
--- /dev/null
+++ b/Data/Shaders/s_mapping_metal.frag
@@ -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.
+
+/// \file s_mapping_metal.frag
+/// Metallic material, environment maps plus shadow maps
+/// for use with s_mapping_metal.vert.
+/// Ported from legacy SimQuest code, the origin of the material and lighting
+/// calculation is unkown
+
+// These are 'free' uniforms to be set for this shader, they won't be provided by OSS
+uniform float specularPercent;
+uniform float diffusePercent;
+uniform float shininess;
+
+uniform vec4 specularColor;
+uniform samplerCube specularEnvMap;
+uniform samplerCube diffuseEnvMap;
+uniform sampler2D shadowMap;
+
+// OSS provided uniforms
+uniform vec4 ambientColor;
+
+struct LightSource {
+ vec4 diffuse;
+ vec4 specular;
+ vec4 position;
+ float constantAttenuation;
+ float linearAttenuation;
+ float quadraticAttenuation;
+};
+
+uniform LightSource lightSource;
+
+// From Vertex Shader
+varying vec3 lightDir;
+varying vec3 eyeDir;
+varying vec3 reflectDir;
+varying vec3 normalDir;
+
+varying vec4 clipCoord;
+
+varying float attenuation;
+
+void main()
+{
+ // Calculate Shadowing
+ vec2 shadowCoord = clipCoord.xy / clipCoord.w * vec2(0.5) + vec2(0.5);
+ float shadowAmount = 1.0 - texture2D(shadowMap, shadowCoord).r;
+
+ // Look up environment map values in cube maps
+ vec3 mappedDiffuseColor = vec3(textureCube(diffuseEnvMap, normalDir)) *
+ (lightSource.diffuse * attenuation * shadowAmount).rgb;
+ vec3 mappedSpecularColor = vec3(textureCube(specularEnvMap, reflectDir)) *
+ (lightSource.specular * attenuation).rgb;
+
+ // Add lighting to base color and mix
+ vec3 color = mix(ambientColor.rgb, mappedDiffuseColor, diffusePercent);
+
+ vec3 lightDirNorm = normalize(lightDir);
+ vec3 normalDirNorm = normalize(normalDir);
+ vec3 eyeDirNorm = normalize(eyeDir);
+
+ float temp = max(dot(reflect(lightDirNorm, normalDirNorm), eyeDirNorm), 0.0);
+ float specular = temp / (shininess - temp * shininess + temp);
+
+ color = mix(color, mappedSpecularColor + color, specularPercent) * shadowAmount +
+ (specular * specularColor * lightSource.specular * attenuation).rgb * shadowAmount;
+
+ gl_FragColor.rgb = color;
+ gl_FragColor.a = 1.0;
+}
+
diff --git a/Data/Shaders/s_mapping_metal.vert b/Data/Shaders/s_mapping_metal.vert
new file mode 100644
index 0000000..a357849
--- /dev/null
+++ b/Data/Shaders/s_mapping_metal.vert
@@ -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.
+
+
+/// \file s_mapping_metal.vert
+/// Metallic material, environment maps plus shadow maps
+
+// Oss provided uniforms
+uniform mat4 viewMatrix;
+
+struct LightSource {
+ vec4 diffuse;
+ vec4 specular;
+ vec4 position;
+ float constantAttenuation;
+ float linearAttenuation;
+ float quadraticAttenuation;
+};
+
+uniform LightSource lightSource;
+
+
+varying vec3 lightDir;
+varying vec3 eyeDir;
+varying vec3 reflectDir;
+varying vec3 normalDir;
+
+varying vec4 clipCoord;
+
+varying float attenuation;
+
+void main()
+{
+ gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+
+ clipCoord = gl_Position;
+
+ vec4 eyeDir4 = gl_ModelViewMatrix * gl_Vertex;
+ eyeDir = eyeDir4.xyz;
+
+ lightDir = (viewMatrix * lightSource.position).xyz - eyeDir4.xyz;
+ float lightDistance = length(lightDir);
+
+ attenuation = 1.0 / (lightSource.constantAttenuation +
+ lightSource.linearAttenuation*lightDistance +
+ lightSource.quadraticAttenuation*lightDistance*lightDistance);
+
+ normalDir = gl_NormalMatrix * gl_Normal;
+
+ // normalDir needs to be normalized for reflect to work
+ reflectDir = reflect(eyeDir, normalize(normalDir));
+}
diff --git a/Data/Shaders/shadow_map.frag b/Data/Shaders/shadow_map.frag
index 6a34ebc..2e64a3b 100644
--- a/Data/Shaders/shadow_map.frag
+++ b/Data/Shaders/shadow_map.frag
@@ -12,34 +12,34 @@
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
+#version 120
/// \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,
+/// 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
+/// 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;
+uniform sampler2D depthMap;
+/// Changes Intensity of the shadow default is all black
+uniform float intensity = 1.0;
+
+uniform float bias = 0.0005;
/// The coordinates of the fragment in the space of the projected depth map
varying vec4 lightCoord;
-float decodeDepth(vec4 color)
+void main(void)
{
- 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));
+ float depth = texture2D(depthMap, lightCoord3.xy).x;
- gl_FragColor = vec4(depth + 0.00001 > lightCoord3.z ? 0.0 : 1.0);
+ gl_FragColor = vec4(depth + bias > lightCoord3.z || lightCoord3.z > 1.0 ? 0.0 : intensity);
}
diff --git a/Data/Shaders/shadow_map.vert b/Data/Shaders/shadow_map.vert
index 8001d2d..c06c5cc 100644
--- a/Data/Shaders/shadow_map.vert
+++ b/Data/Shaders/shadow_map.vert
@@ -17,8 +17,18 @@
/// 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;
+/// Main Camera Matrices
+struct MainCamera
+{
+ mat4 viewMatrix;
+ mat4 inverseViewMatrix;
+ mat4 projectionMatrix;
+ mat4 mainProjectionMatrix;
+};
+
+uniform MainCamera mainCamera;
+
+uniform mat4 modelMatrix;
/// view matrix for the camera that rendered the original depth map (see shadow_map.frag)
uniform mat4 lightViewMatrix;
@@ -30,10 +40,11 @@ varying vec4 lightCoord;
void main(void)
{
- gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex;
+ vec4 transformed = modelMatrix * gl_Vertex;
+ gl_Position = mainCamera.projectionMatrix * mainCamera.viewMatrix * transformed;
// 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;
+ lightCoord = lightProjectionMatrix * lightViewMatrix * transformed;
}
\ No newline at end of file
diff --git a/Data/Shaders/skinning.vert b/Data/Shaders/skinning.vert
new file mode 100644
index 0000000..c77e645
--- /dev/null
+++ b/Data/Shaders/skinning.vert
@@ -0,0 +1,76 @@
+/* -*-c++-*-
+ * Copyright (C) 2008 Cedric Pinson <cedric.pinson at plopbyte.net>
+ *
+ * This library is open source and may be redistributed and/or modified under
+ * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
+ * (at your option) any later version. The full license is in LICENSE file
+ * included with this distribution, and on the openscenegraph.org website.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * OpenSceneGraph Public License for more details.
+*/
+
+in vec4 boneWeight0;
+in vec4 boneWeight1;
+in vec4 boneWeight2;
+in vec4 boneWeight3;
+
+uniform int nbBonesPerVertex;
+uniform mat4 matrixPalette[MAX_MATRIX];
+
+vec4 position;
+vec3 normal;
+
+
+// accumulate position and normal in global scope
+void computeAcummulatedNormalAndPosition(vec4 boneWeight)
+{
+ int matrixIndex;
+ float matrixWeight;
+ for (int i = 0; i < 2; i++)
+ {
+ matrixIndex = int(boneWeight[0]);
+ matrixWeight = boneWeight[1];
+ mat4 matrix = matrixPalette[matrixIndex];
+ // correct for normal if no scale in bone
+ mat3 matrixNormal = mat3(matrix);
+ position += matrixWeight * (matrix * gl_Vertex );
+ normal += matrixWeight * (matrixNormal * gl_Normal );
+
+ boneWeight = boneWeight.zwxy;
+ }
+}
+
+void main( void )
+{
+ position = vec4(0.0,0.0,0.0,0.0);
+ normal = vec3(0.0,0.0,0.0);
+
+ // there is 2 bone data per attributes
+ if (nbBonesPerVertex > 0)
+ computeAcummulatedNormalAndPosition(boneWeight0);
+ if (nbBonesPerVertex > 2)
+ computeAcummulatedNormalAndPosition(boneWeight1);
+ if (nbBonesPerVertex > 4)
+ computeAcummulatedNormalAndPosition(boneWeight2);
+ if (nbBonesPerVertex > 6)
+ computeAcummulatedNormalAndPosition(boneWeight3);
+
+ normal = gl_NormalMatrix * normal;
+
+ vec3 lightDir = normalize(vec3(gl_LightSource[0].position));
+ float NdotL = max(dot(normal, lightDir), 0.0);
+ vec4 diffuse = NdotL * gl_FrontMaterial.diffuse * gl_LightSource[0].diffuse;
+
+ vec4 ambient = gl_FrontMaterial.ambient * gl_LightSource[0].ambient;
+ vec4 globalAmbient = gl_LightModel.ambient * gl_FrontMaterial.ambient;
+
+ float NdotHV = max(dot(normal, gl_LightSource[0].halfVector.xyz),0.0);
+ vec4 specular = gl_FrontMaterial.specular * gl_LightSource[0].specular * pow(NdotHV,gl_FrontMaterial.shininess);
+
+ gl_FrontColor = specular + diffuse + globalAmbient + ambient;
+ gl_Position = gl_ProjectionMatrix * gl_ModelViewMatrix * position;
+ gl_TexCoord[0] = gl_MultiTexCoord0;
+}
diff --git a/Data/Shaders/unlit_text.frag b/Data/Shaders/unlit_text.frag
new file mode 100644
index 0000000..0be771e
--- /dev/null
+++ b/Data/Shaders/unlit_text.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 unlit_text.frag
+/// Fragment Shader to do text rendering
+/// use with unlit_texture.vert
+
+uniform sampler2D texture;
+
+varying vec2 texCoord0;
+varying vec4 color;
+
+void main(void)
+{
+ gl_FragColor.rgb = color.rgb;
+ gl_FragColor.a = texture2D(texture, texCoord0.st).a;
+}
\ No newline at end of file
diff --git a/Data/Shaders/unlit_texture_rectangle.frag b/Data/Shaders/unlit_texture_rectangle.frag
index c06ed98..e7ea6a6 100644
--- a/Data/Shaders/unlit_texture_rectangle.frag
+++ b/Data/Shaders/unlit_texture_rectangle.frag
@@ -1,3 +1,22 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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_rectangle.frag
+/// Fragment Shader to do simple unlit texture mapping on rectangle textures,
+/// use with unlit_texture.vert
+
#extension GL_ARB_texture_rectangle : enable
uniform sampler2DRect texture;
diff --git a/Data/devices.yaml b/Data/devices.yaml
new file mode 100644
index 0000000..b46ec7c
--- /dev/null
+++ b/Data/devices.yaml
@@ -0,0 +1,8 @@
+Novint:
+ FALCON_1: 14QAVEFF
+ FALCON_2: 16000166
+# To initialize a NovintDevice with a specific serial number in OSS:
+# 1) see NovintCommonDevice->setSerialNumber(), or
+# 2) copy this file to the working directory (same folder as the executable),
+# add or change a line here in the form "initializationName: serialNumber",
+# and pass that name to NovintCommonDevice->setInitializationName().
\ No newline at end of file
diff --git a/Documentation/CMakeLists.txt b/Documentation/CMakeLists.txt
index 44ca678..2ed869e 100644
--- a/Documentation/CMakeLists.txt
+++ b/Documentation/CMakeLists.txt
@@ -28,22 +28,10 @@ find_program(DOXYGEN_DOT_EXECUTABLE
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/*
+set(SURGSIM_DOCUMENTATION_EXCLUDE_LIST "*/UnitTests/* */RenderTests/* */PerformanceTests/*"
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")
@@ -66,20 +54,13 @@ 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)
+find_package(MathJax)
+if(MATHJAX_FOUND)
+ set(MATHJAX_RELPATH_CONFIG "MATHJAX_RELPATH = file://${MATHJAX_DIR}")
+ message(STATUS "Using local MathJax: " ${MATHJAX_DIR})
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)
+ set(MATHJAX_RELPATH_CONFIG "")
+ message(STATUS "Using default web-based MathJax")
endif()
# Generate Doxyfile from Doxyfile.in, with @VARIABLE@ substitution.
diff --git a/Documentation/Doxyfile.in b/Documentation/Doxyfile.in
index 2f4bb7a..97f4e81 100644
--- a/Documentation/Doxyfile.in
+++ b/Documentation/Doxyfile.in
@@ -619,7 +619,7 @@ CITE_BIB_FILES =
# 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
+QUIET = YES
# 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
@@ -674,7 +674,7 @@ WARN_LOGFILE =
# directories like "/usr/src/myproject". Separate the files or directories
# with spaces.
-INPUT = @PROJECT_SOURCE_DIR@/SurgSim @PROJECT_SOURCE_DIR@/Documentation
+INPUT = @PROJECT_SOURCE_DIR@/README.md @PROJECT_SOURCE_DIR@/SurgSim @PROJECT_SOURCE_DIR@/Documentation @PROJECT_SOURCE_DIR@/Modules
# 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
@@ -728,7 +728,7 @@ EXCLUDE_PATTERNS = @SURGSIM_DOCUMENTATION_EXCLUDE_LIST@
# wildcard * is used, a substring. Examples: ANamespace, AClass,
# AClass::ANamespace, ANamespace::*Test
-EXCLUDE_SYMBOLS = *::internal
+EXCLUDE_SYMBOLS = *::internal *SURGSIM_STATIC_REGISTRATION* *SURGSIM_REGISTER*
# The EXAMPLE_PATH tag can be used to specify one or more files or
# directories that contain example code fragments that are included (see
@@ -797,7 +797,7 @@ FILTER_SOURCE_PATTERNS =
# 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 =
+USE_MDFILE_AS_MAINPAGE = README.md
#---------------------------------------------------------------------------
# configuration options related to source browsing
@@ -1225,8 +1225,7 @@ USE_MATHJAX = YES
# 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@
+ at MATHJAX_RELPATH_CONFIG@
# The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension
# names that should be enabled during MathJax rendering.
diff --git a/Documentation/mainPage.dox b/Documentation/mainPage.dox
deleted file mode 100644
index 8bc4f51..0000000
--- a/Documentation/mainPage.dox
+++ /dev/null
@@ -1,50 +0,0 @@
-// 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.h b/Examples/AddSphereFromInput/AddSphereBehavior.h
index 91563e7..8295a0b 100644
--- a/Examples/AddSphereFromInput/AddSphereBehavior.h
+++ b/Examples/AddSphereFromInput/AddSphereBehavior.h
@@ -32,16 +32,16 @@ public:
/// Update the behavior
/// \param dt The length of time (seconds) between update calls.
- virtual void update(double dt) override;
+ void update(double dt) override;
/// Return the type of manager that should be responsible for this behavior
- virtual int getTargetManagerType() const override;
+ int getTargetManagerType() const override;
protected:
/// Initialize the behavior
- virtual bool doInitialize() override;
+ bool doInitialize() override;
/// Wakeup the behavior
- virtual bool doWakeUp() override;
+ bool doWakeUp() override;
private:
/// Input component to get the pose
diff --git a/Examples/AddSphereFromInput/AddSphereFromInput.cpp b/Examples/AddSphereFromInput/AddSphereFromInput.cpp
index 07575bf..d2b9656 100644
--- a/Examples/AddSphereFromInput/AddSphereFromInput.cpp
+++ b/Examples/AddSphereFromInput/AddSphereFromInput.cpp
@@ -20,31 +20,11 @@
#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/Framework/Framework.h"
+#include "SurgSim/Graphics/Graphics.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"
+#include "SurgSim/Math/Math.h"
+#include "SurgSim/Physics/Physics.h"
using SurgSim::Blocks::DriveElementFromInputBehavior;
using SurgSim::Framework::BasicSceneElement;
@@ -54,19 +34,19 @@ using SurgSim::Graphics::OsgBoxRepresentation;
using SurgSim::Graphics::OsgCamera;
using SurgSim::Graphics::OsgMaterial;
using SurgSim::Graphics::OsgPlaneRepresentation;
-using SurgSim::Graphics::OsgShader;
+using SurgSim::Graphics::OsgProgram;
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::Math::makeRigidTransform;
using SurgSim::Physics::FixedRepresentation;
-using SurgSim::Physics::Representation;
using SurgSim::Physics::PhysicsManager;
+using SurgSim::Physics::Representation;
std::shared_ptr<SceneElement> createPlane(const std::string& name)
@@ -81,19 +61,19 @@ std::shared_ptr<SceneElement> createPlane(const std::string& name)
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<OsgProgram> program = std::make_shared<OsgProgram>();
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(
+ program->setFragmentShaderSource(
"uniform vec4 color;\n"
"void main(void)\n"
"{\n"
" gl_FragColor = color;\n"
"}");
- material->setShader(shader);
+ material->setProgram(program);
graphicsRepresentation->setMaterial(material);
std::shared_ptr<SceneElement> planeElement = std::make_shared<BasicSceneElement>(name);
@@ -146,8 +126,8 @@ int main(int argc, char* argv[])
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");
+ std::shared_ptr<SurgSim::Devices::MultiAxisDevice> toolDevice =
+ std::make_shared<SurgSim::Devices::MultiAxisDevice>("MultiAxisDevice");
toolDevice->setPositionScale(toolDevice->getPositionScale() * 10.0);
toolDevice->setOrientationScale(toolDevice->getOrientationScale() * 3.0);
SURGSIM_ASSERT(toolDevice->initialize() == true) <<
diff --git a/Examples/AddSphereFromInput/CMakeLists.txt b/Examples/AddSphereFromInput/CMakeLists.txt
index 9bafb6a..375e654 100644
--- a/Examples/AddSphereFromInput/CMakeLists.txt
+++ b/Examples/AddSphereFromInput/CMakeLists.txt
@@ -22,7 +22,6 @@ link_directories(
include_directories(
${CMAKE_CURRENT_SOURCE_DIR}
- ${OSG_INCLUDE_DIR}
${OPENTHREADS_INCLUDE_DIR}
)
@@ -35,8 +34,7 @@ set(HEADERS
AddSphereBehavior.h
)
-add_executable(AddSphereFromInput ${SOURCES} ${HEADERS})
-surgsim_show_ide_folders("${SOURCES}" "${HEADERS}")
+surgsim_add_executable(AddSphereFromInput "${SOURCES}" "${HEADERS}")
set(LIBS
MultiAxisDevice
diff --git a/Examples/CMakeLists.txt b/Examples/CMakeLists.txt
index fb9dd05..741a3b5 100644
--- a/Examples/CMakeLists.txt
+++ b/Examples/CMakeLists.txt
@@ -18,10 +18,17 @@ if(BUILD_DEVICE_MULTIAXIS)
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(InputVtc)
add_subdirectory(DroppingBalls)
+
+# Not Required components
+find_package(Boost 1.54 COMPONENTS program_options)
+
+if(Boost_PROGRAM_OPTIONS_FOUND)
+add_subdirectory(ShowScenery)
add_subdirectory(GraphicsScene)
+else()
+message("Can't build example ShowScenery, missing library boost_program_options.")
+endif()
diff --git a/Examples/DroppingBalls/AddRandomSphereBehavior.h b/Examples/DroppingBalls/AddRandomSphereBehavior.h
index 0b30e48..9cd5d92 100644
--- a/Examples/DroppingBalls/AddRandomSphereBehavior.h
+++ b/Examples/DroppingBalls/AddRandomSphereBehavior.h
@@ -40,13 +40,13 @@ public:
/// Update the behavior
/// \param dt The length of time (seconds) between update calls.
- virtual void update(double dt) override;
+ void update(double dt) override;
protected:
/// Initialize the behavior
- virtual bool doInitialize() override;
+ bool doInitialize() override;
/// Wakeup the behavior
- virtual bool doWakeUp() override;
+ bool doWakeUp() override;
private:
/// Control how often a sphere is added
diff --git a/Examples/DroppingBalls/CMakeLists.txt b/Examples/DroppingBalls/CMakeLists.txt
index 57360fd..6c8d95e 100644
--- a/Examples/DroppingBalls/CMakeLists.txt
+++ b/Examples/DroppingBalls/CMakeLists.txt
@@ -19,7 +19,6 @@ link_directories(
include_directories(
${CMAKE_CURRENT_SOURCE_DIR}
- ${OSG_INCLUDE_DIR}
${OPENTHREADS_INCLUDE_DIR}
)
@@ -40,8 +39,7 @@ configure_file(
"${CMAKE_CURRENT_BINARY_DIR}/config.txt"
)
-add_executable(DroppingBalls ${SOURCES} ${HEADERS})
-surgsim_show_ide_folders("${SOURCES}" "${HEADERS}")
+surgsim_add_executable(DroppingBalls "${SOURCES}" "${HEADERS}")
SET(LIBS
SurgSimBlocks
diff --git a/Examples/DroppingBalls/DroppingBalls.cpp b/Examples/DroppingBalls/DroppingBalls.cpp
index bb13f99..a4d6c38 100644
--- a/Examples/DroppingBalls/DroppingBalls.cpp
+++ b/Examples/DroppingBalls/DroppingBalls.cpp
@@ -16,33 +16,10 @@
#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 "SurgSim/Framework/Framework.h"
+#include "SurgSim/Graphics/Graphics.h"
+#include "SurgSim/Math/Math.h"
+#include "SurgSim/Physics/Physics.h"
#include "Examples/DroppingBalls/AddRandomSphereBehavior.h"
@@ -52,22 +29,22 @@ using SurgSim::Framework::Logger;
using SurgSim::Framework::SceneElement;
using SurgSim::Graphics::OsgMaterial;
using SurgSim::Graphics::OsgPlaneRepresentation;
-using SurgSim::Graphics::OsgShader;
+using SurgSim::Graphics::OsgProgram;
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::Math::Vector4f;
+using SurgSim::Math::makeRigidTransform;
using SurgSim::Physics::FixedRepresentation;
+using SurgSim::Physics::PhysicsManager;
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.
@@ -152,7 +129,7 @@ std::shared_ptr<SceneElement> createPlane(const std::string& name)
// 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>();
+ std::shared_ptr<OsgProgram> program = std::make_shared<OsgProgram>();
// Create a Uniform for the RGBA color of the plane.
std::shared_ptr<OsgUniform<Vector4f>> uniform = std::make_shared<OsgUniform<Vector4f>>("color");
@@ -160,13 +137,13 @@ std::shared_ptr<SceneElement> createPlane(const std::string& name)
material->addUniform(uniform);
// This Shader sets the fragment's color to the value of the "color" uniform.
- shader->setFragmentShaderSource(
+ program->setFragmentShaderSource(
"uniform vec4 color;\n"
"void main(void)\n"
"{\n"
" gl_FragColor = color;\n"
"}");
- material->setShader(shader);
+ material->setProgram(program);
graphics->setMaterial(material);
// RigidCollisionRepresentation will use provided physics representation to do collisions. Collision detection
diff --git a/Examples/GraphicsScene/CMakeLists.txt b/Examples/GraphicsScene/CMakeLists.txt
index 1de07d0..52a43fc 100644
--- a/Examples/GraphicsScene/CMakeLists.txt
+++ b/Examples/GraphicsScene/CMakeLists.txt
@@ -22,7 +22,6 @@ link_directories(
include_directories(
"${CMAKE_CURRENT_SOURCE_DIR}"
- "${OSG_INCLUDE_DIR}"
"${OPENTHREADS_INCLUDE_DIR}"
)
@@ -41,12 +40,13 @@ configure_file(
"${CMAKE_CURRENT_BINARY_DIR}/config.txt"
)
-add_executable(GraphicsScene ${SOURCES} ${HEADERS})
-surgsim_show_ide_folders("${SOURCES}" "${HEADERS}")
+surgsim_add_executable(GraphicsScene "${SOURCES}" "${HEADERS}")
SET(LIBS
SurgSimBlocks
SurgSimGraphics
+ SurgSimDevices
+ ${Boost_LIBRARIES}
${YAML_CPP_LIBRARIES}
)
diff --git a/Examples/GraphicsScene/Data/StereoView.yaml b/Examples/GraphicsScene/Data/StereoView.yaml
new file mode 100644
index 0000000..c80a792
--- /dev/null
+++ b/Examples/GraphicsScene/Data/StereoView.yaml
@@ -0,0 +1,35 @@
+- SurgSim::Framework::BasicSceneElement:
+ Name: View
+ Components:
+ - SurgSim::Blocks::DriveElementFromInputBehavior:
+ Name: ViewElementDriver
+ Source:
+ SurgSim::Input::InputComponent:
+ Name: Oculus
+ Id: 9b77c4e2-e9ea-484f-bfca-72121189dcb6
+ - SurgSim::Input::InputComponent:
+ Name: Oculus
+ Id: 9b77c4e2-e9ea-484f-bfca-72121189dcb6
+ DeviceName: Oculus
+ LocalPose:
+ Quaternion: [0, 1, 0, 0]
+ Translation: [2.0, 1.0, 2.0]
+ - SurgSim::Device::OculusView:
+ InputComponent:
+ SurgSim::Input::InputComponent:
+ Id: 9b77c4e2-e9ea-484f-bfca-72121189dcb6
+ Name: Oculus
+ Camera:
+ SurgSim::Graphics::OsgCamera:
+ Id: 9018dd38-c1ae-4e80-a27a-dc24e4b75d35
+ Name: Camera
+ TargetScreen: 2
+ FullScreen: true
+ EyeSeparation: 0.06
+ Name: View
+ - SurgSim::Graphics::OsgCamera:
+ Id: 9018dd38-c1ae-4e80-a27a-dc24e4b75d35
+ Name: Camera
+ MainCamera: true
+ PerspectiveProjection: [120, 0.88, 0.1, 10.0]
+ RenderGroupReference: __OssDefault__
\ No newline at end of file
diff --git a/Examples/GraphicsScene/GraphicsScene.cpp b/Examples/GraphicsScene/GraphicsScene.cpp
index 1b5ce5b..7fb0553 100644
--- a/Examples/GraphicsScene/GraphicsScene.cpp
+++ b/Examples/GraphicsScene/GraphicsScene.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -13,61 +13,40 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include <boost/program_options.hpp>
#include <memory>
-
#include <osg/Matrix>
#include <osg/Camera>
+#include "SurgSim/Blocks/DriveElementFromInputBehavior.h"
+#include "SurgSim/Blocks/GraphicsUtilities.h"
#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"
+#include "SurgSim/Blocks/ShadowMapping.h"
+#include "SurgSim/Devices/Devices.h"
+#include "SurgSim/Framework/Framework.h"
+#include "SurgSim/Graphics/Graphics.h"
+#include "SurgSim/Input/Input.h"
+#include "SurgSim/Math/Math.h"
+
using SurgSim::Framework::Logger;
using SurgSim::Graphics::OsgTextureUniform;
using SurgSim::Graphics::OsgTexture2d;
using SurgSim::Graphics::OsgUniform;
using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::Matrix44f;
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.
+/// There are two preprocessing passes that use specific program, plus a main pass that renders the
+/// objects using a program that can process the output from the preprocessing steps.
+/// Each program output becomes the input for the next shader, some parameters from other scene elements
+/// is passed into the program 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
@@ -77,7 +56,9 @@ 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)
+std::shared_ptr<SurgSim::Graphics::ViewElement> createMonoView(
+ const std::string& name,
+ int x, int y, int width, int height)
{
using SurgSim::Graphics::OsgViewElement;
@@ -87,17 +68,20 @@ std::shared_ptr<SurgSim::Graphics::ViewElement> createView(const std::string& na
viewElement->getView()->setPosition(position);
viewElement->getView()->setDimensions(dimensions);
+ double aspectRatio = static_cast<double>(width) / static_cast<double>(height);
+ viewElement->getCamera()->setPerspectiveProjection(60.0, aspectRatio, 0.01, 10.0);
+ std::dynamic_pointer_cast<SurgSim::Graphics::OsgCamera>(viewElement->getCamera())->setMainCamera(true);
/// 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");
+ auto interpolator = std::make_shared<SurgSim::Blocks::PoseInterpolator>("Interpolator");
RigidTransform3d from = makeRigidTransform(
- Vector3d(-4.0, 2.0, -4.0),
+ Vector3d(-4.0, 1.0, -3.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(4.0, 1.0, -3.0),
Vector3d(0.0, 0.0, 0.0),
Vector3d(0.0, 1.0, 0.0));
interpolator->setTarget(viewElement);
@@ -108,7 +92,8 @@ std::shared_ptr<SurgSim::Graphics::ViewElement> createView(const std::string& na
viewElement->setPose(from);
- viewElement->addComponent(interpolator);
+ viewElement->enableManipulator(true);
+ // viewElement->addComponent(interpolator);
return viewElement;
}
@@ -118,13 +103,13 @@ std::shared_ptr<SurgSim::Graphics::OsgMaterial> createMaterialWithShaders(
const std::string& name)
{
- auto shader = SurgSim::Graphics::loadShader(data, name);
+ auto program = SurgSim::Graphics::loadProgram(data, name);
std::shared_ptr<SurgSim::Graphics::OsgMaterial> material;
- if (shader != nullptr)
+ if (program != nullptr)
{
material = std::make_shared<SurgSim::Graphics::OsgMaterial>(name);
- material->setShader(shader);
+ material->setProgram(program);
}
return material;
@@ -135,7 +120,7 @@ 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->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);
result->addComponent(light);
@@ -156,40 +141,11 @@ std::shared_ptr<SurgSim::Framework::SceneElement> createLight()
result->setPose(from);
- result->addComponent(interpolator);
+ // 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
@@ -236,13 +192,13 @@ public:
m_box->setMaterial(materials["basicShadowed"]);
// Assign this to the pass for shadowing objects
- m_box->addGroupReference("shadowing");
+ m_box->addGroupReference(SurgSim::Blocks::GROUP_SHADOW_CASTER);
// Assign this to the pass for shadowed objects
- m_box->addGroupReference("shadowed");
+ m_box->addGroupReference(SurgSim::Blocks::GROUP_SHADOW_RECEIVER);
}
- virtual bool doInitialize() override
+ bool doInitialize() override
{
addComponent(m_box);
return true;
@@ -270,11 +226,11 @@ public:
{
m_sphere = std::make_shared<SurgSim::Graphics::OsgSphereRepresentation>(getName() + " Graphics");
m_sphere->setMaterial(materials["basicShadowed"]);
- m_sphere->addGroupReference("shadowing");
- m_sphere->addGroupReference("shadowed");
+ m_sphere->addGroupReference(SurgSim::Blocks::GROUP_SHADOW_CASTER);
+ m_sphere->addGroupReference(SurgSim::Blocks::GROUP_SHADOW_RECEIVER);
}
- virtual bool doInitialize() override
+ bool doInitialize() override
{
addComponent(m_sphere);
return true;
@@ -319,24 +275,8 @@ void addSpheres(std::shared_ptr<SurgSim::Framework::Scene> scene)
}
-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)
+void createScene(std::shared_ptr<SurgSim::Framework::Runtime> runtime, bool useStereo = false)
{
- 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);
@@ -353,109 +293,109 @@ void createScene(std::shared_ptr<SurgSim::Framework::Runtime> runtime)
auto sphere = std::make_shared<SimpleSphere>("Shiny Sphere");
sphere->setRadius(0.25);
- sphere->setMaterial(materials["shiny"]);
+ sphere->setMaterial(materials["basicShadowed"]);
scene->addSceneElement(sphere);
+ if (useStereo == true)
+ {
+ // This will most like have to be edited to conform with the screen configuration
+ // TargetScreen, or Dimensions and Location are candidates for being incorrect depending
+ // on the system
+ runtime->addSceneElements("StereoView.yaml");
+ }
+ else
+ {
+ scene->addSceneElement(createMonoView("View", 40, 40, 1024, 768));
+ }
- 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);
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("Materials");
+ element->addComponent(materials["shiny"]);
+ element->addComponent(materials["texturedShadowed"]);
+ element->addComponent(materials["basicShadowed"]);
+ scene->addSceneElement(element);
+
+ scene->addSceneElement(createLight());
+
+ std::array<double, 6> lightProjection = { -2.6, 2.6, -2.6, 2.6, 4, 10};
+ auto elements = SurgSim::Blocks::createShadowMapping(
+ scene->getComponent("View", "Camera"),
+ scene->getComponent("Light", "Light"),
+ 4096,
+ 1024,
+ lightProjection,
+ 0.0005,
+ 0.8,
+ true,
+ 8.0,
+ false);
+ scene->addSceneElements(elements);
}
int main(int argc, char* argv[])
{
+ using boost::program_options::value;
+
+ bool useStereo = false;
+
+ boost::program_options::options_description visible("Allowed options");
+ visible.add_options()("help", "produce help message")
+ ("useStereo", value<bool>(&useStereo)->default_value(false), "Show scene via stereo");
+
+ boost::program_options::variables_map variables;
+ boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(visible).run(),
+ variables);
+ boost::program_options::notify(variables);
+
+ if (variables.count("help"))
+ {
+ std::cout << visible << "\n";
+ return 1;
+ }
+
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["texturedUnlit"] = createMaterialWithShaders(*data, "Shaders/unlit_texture");
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>());
+ auto graphics = std::make_shared<SurgSim::Graphics::OsgManager>();
+ graphics->setMultiThreading(true);
+
+ auto input = std::make_shared<SurgSim::Input::InputManager>();
+
+ if (useStereo)
+ {
+ // Only interested in the oculus
+ auto device = SurgSim::Devices::createDevice("SurgSim::Devices::OculusDevice", "Oculus");
+
+ if (device != nullptr)
+ {
+ input->addDevice(device);
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Could not initialize Oculus Device, falling back to mono display.";
+ useStereo = false;
+ }
+ }
+
+ auto behaviors = std::make_shared<SurgSim::Framework::BehaviorManager>();
+
+ runtime->addManager(graphics);
+ runtime->addManager(behaviors);
+ runtime->addManager(input);
+
+ configureShinyMaterial();
+ configureTexturedMaterial(runtime->getApplicationData()->findFile("Textures/CheckerBoard.png"));
- createScene(runtime);
+ createScene(runtime, useStereo);
runtime->execute();
diff --git a/Examples/GraphicsScene/config.txt.in b/Examples/GraphicsScene/config.txt.in
index 4686595..88957ec 100644
--- a/Examples/GraphicsScene/config.txt.in
+++ b/Examples/GraphicsScene/config.txt.in
@@ -1,2 +1,2 @@
-${SURGSIM_SOURCE_DIR}/Data/
+${PROJECT_BINARY_DIR}/Data/
${CMAKE_CURRENT_BINARY_DIR}/Data/
diff --git a/Examples/InputVtc/CMakeLists.txt b/Examples/InputVtc/CMakeLists.txt
index 4a197c7..c1c93a1 100644
--- a/Examples/InputVtc/CMakeLists.txt
+++ b/Examples/InputVtc/CMakeLists.txt
@@ -19,40 +19,36 @@ link_directories(
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})
+surgsim_add_executable(InputVtc "${SOURCES}" "${HEADERS}")
set(LIBS
- IdentityPoseDevice
SurgSimBlocks
+ SurgSimDevices
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})
+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"
+)
+
# Put InputVtc into folder "Examples"
set_target_properties(InputVtc PROPERTIES FOLDER "Examples")
diff --git a/Examples/InputVtc/Data/Device.yaml b/Examples/InputVtc/Data/Device.yaml
new file mode 100644
index 0000000..b317506
--- /dev/null
+++ b/Examples/InputVtc/Data/Device.yaml
@@ -0,0 +1,13 @@
+- SurgSim::Devices::NovintDevice:
+ Name: Tool_Device
+ InitializationName: 7DofFalcon
+ 7DofDevice: true
+- SurgSim::Devices::PhantomDevice:
+ Name: Tool_Device
+- SurgSim::Devices::NovintDevice:
+ Name: Tool_Device
+ 7DofDevice: false
+- SurgSim::Devices::MultiAxisDevice:
+ Name: Tool_Device
+- SurgSim::Devices::IdentityPoseDevice:
+ Name: Tool_Device
diff --git a/Examples/InputVtc/DeviceFactory.cpp b/Examples/InputVtc/DeviceFactory.cpp
deleted file mode 100644
index 2b48d1c..0000000
--- a/Examples/InputVtc/DeviceFactory.cpp
+++ /dev/null
@@ -1,73 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index e58bac3..0000000
--- a/Examples/InputVtc/DeviceFactory.h
+++ /dev/null
@@ -1,45 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
index 881df9c..28149c5 100644
--- a/Examples/InputVtc/InputVtc.cpp
+++ b/Examples/InputVtc/InputVtc.cpp
@@ -16,46 +16,20 @@
#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"
+#include "SurgSim/Devices/Devices.h"
+#include "SurgSim/Framework/Framework.h"
+#include "SurgSim/Graphics/Graphics.h"
+#include "SurgSim/Input/Input.h"
+#include "SurgSim/Math/Math.h"
+#include "SurgSim/Physics/Physics.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::OsgProgram;
using SurgSim::Graphics::OsgUniform;
using SurgSim::Graphics::OsgViewElement;
using SurgSim::Graphics::ViewElement;
@@ -82,19 +56,19 @@ std::shared_ptr<SceneElement> createPlane(const std::string& name)
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<OsgProgram> program = std::make_shared<OsgProgram>();
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(
+ program->setFragmentShaderSource(
"uniform vec4 color;\n"
"void main(void)\n"
"{\n"
" gl_FragColor = color;\n"
"}");
- material->setShader(shader);
+ material->setProgram(program);
graphicsRepresentation->setMaterial(material);
std::shared_ptr<SceneElement> planeElement = std::make_shared<BasicSceneElement>(name);
@@ -110,7 +84,7 @@ std::shared_ptr<SceneElement> createPlane(const std::string& name)
}
-std::shared_ptr<SceneElement> createBox(const std::string& name, const std::string& toolDeviceName)
+std::shared_ptr<SceneElement> createBox(const std::string& name, const std::string& deviceName)
{
std::shared_ptr<BoxShape> box = std::make_shared<BoxShape>(0.8, 2.0, 0.2); // in meter
@@ -134,39 +108,21 @@ std::shared_ptr<SceneElement> createBox(const std::string& name, const std::stri
// Input Components
std::shared_ptr<SurgSim::Input::InputComponent> inputComponent =
std::make_shared<SurgSim::Input::InputComponent>(name + " Input");
- inputComponent->setDeviceName(toolDeviceName);
+ inputComponent->setDeviceName(deviceName);
// 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.
+ outputComponent->setDeviceName(deviceName);
+
+ // A virtual tool coupler can be used to connect an input/output device with the physics thread.
+ // The physics representation follows the pose provided by the device, and the representation's collisions
+ // generate forces and torques on the device.
std::shared_ptr<VirtualToolCoupler> inputCoupler = std::make_shared<VirtualToolCoupler>(name + " Input Coupler");
inputCoupler->setInput(inputComponent);
inputCoupler->setOutput(outputComponent);
inputCoupler->setRepresentation(physicsRepresentation);
+ inputCoupler->setHapticOutputOnlyWhenColliding(true);
// The SceneElement
std::shared_ptr<BasicSceneElement> boxElement = std::make_shared<BasicSceneElement>(name);
@@ -180,29 +136,29 @@ std::shared_ptr<SceneElement> createBox(const std::string& name, const std::stri
return boxElement;
}
-std::shared_ptr<SceneElement> createBoxForRawInput(const std::string& name, const std::string& toolDeviceName)
+std::shared_ptr<SceneElement> createBoxForRawInput(const std::string& name, const std::string& deviceName)
{
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(
+ std::shared_ptr<OsgProgram> program = std::make_shared<OsgProgram>();
+ program->setVertexShaderSource(
"void main(void)\n"
"{\n"
" gl_Position = ftransform();\n"
"}");
- shader->setFragmentShaderSource(
+ program->setFragmentShaderSource(
"void main(void)\n"
"{\n"
" gl_FragColor = vec4(0.2, 0.2, 0.2, 1.0);\n"
"}");
- material->setShader(shader);
+ material->setProgram(program);
graphicsRepresentation->setMaterial(material);
std::shared_ptr<SurgSim::Input::InputComponent> inputComponent;
inputComponent = std::make_shared<SurgSim::Input::InputComponent>(name + " Input");
- inputComponent->setDeviceName(toolDeviceName);
+ inputComponent->setDeviceName(deviceName);
std::shared_ptr<DriveElementFromInputBehavior> driver;
driver = std::make_shared<DriveElementFromInputBehavior>(name + " Driver");
@@ -219,27 +175,30 @@ std::shared_ptr<SceneElement> createBoxForRawInput(const std::string& name, cons
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());
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
runtime->addManager(physicsManager);
runtime->addManager(graphicsManager);
runtime->addManager(behaviorManager);
runtime->addManager(inputManager);
+ const std::string deviceName = "Tool_Device";
+ std::shared_ptr<SurgSim::Input::DeviceInterface> device =
+ SurgSim::Devices::loadDevice("Device.yaml");
+ SURGSIM_ASSERT(device != nullptr) << "Failed to initialize any device.";
+ SURGSIM_LOG_IF(std::dynamic_pointer_cast<SurgSim::Devices::IdentityPoseDevice>(device) != nullptr,
+ SurgSim::Framework::Logger::getDefaultLogger(), WARNING) << "The InputVtc example was unable to initialize " <<
+ "an input device that provides poses, so it will use a constant pose.";
+ inputManager->addDevice(device);
+
std::shared_ptr<SurgSim::Framework::Scene> scene = runtime->getScene();
- scene->addSceneElement(createBox("VTC Box", toolDeviceName));
- scene->addSceneElement(createBoxForRawInput("Raw Input", toolDeviceName));
+ scene->addSceneElement(createBox("VTC Box", deviceName));
+ scene->addSceneElement(createBoxForRawInput("Raw Input", deviceName));
std::shared_ptr<SceneElement> plane = createPlane("Plane");
plane->setPose(makeRigidTransform(SurgSim::Math::Quaterniond::Identity(), Vector3d(0.0, -1.0, 0.0)));
diff --git a/Examples/InputVtc/config.txt.in b/Examples/InputVtc/config.txt.in
new file mode 100644
index 0000000..11a8132
--- /dev/null
+++ b/Examples/InputVtc/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/Examples/ShowScenery/CMakeLists.txt b/Examples/ShowScenery/CMakeLists.txt
new file mode 100644
index 0000000..aa21cd2
--- /dev/null
+++ b/Examples/ShowScenery/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}"
+ "${OPENTHREADS_INCLUDE_DIR}"
+)
+
+set(SOURCES
+ ShowScenery.cpp
+)
+
+set(HEADERS
+)
+
+# Configure the path for the data files
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+surgsim_add_executable(ShowScenery "${SOURCES}" "${HEADERS}")
+
+SET(LIBS
+ SurgSimBlocks
+ SurgSimGraphics
+ ${Boost_LIBRARIES}
+ ${YAML_CPP_LIBRARIES}
+)
+
+target_link_libraries(ShowScenery ${LIBS})
+
+# Put ShowScenery into folder "Examples"
+set_target_properties(ShowScenery PROPERTIES FOLDER "Examples")
diff --git a/Examples/ShowScenery/ShowScenery.cpp b/Examples/ShowScenery/ShowScenery.cpp
new file mode 100644
index 0000000..4db9b44
--- /dev/null
+++ b/Examples/ShowScenery/ShowScenery.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 <memory>
+
+#include <boost/program_options.hpp>
+#include "SurgSim/Framework/Framework.h"
+#include "SurgSim/Graphics/Graphics.h"
+#include "SurgSim/Math/Math.h"
+
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Framework::Logger;
+using SurgSim::Framework::Runtime;
+using SurgSim::Graphics::OsgAxesRepresentation;
+using SurgSim::Graphics::OsgSceneryRepresentation;
+using SurgSim::Graphics::OsgViewElement;
+using SurgSim::Math::Vector3d;
+
+namespace po = boost::program_options;
+
+namespace
+{
+
+std::shared_ptr<OsgViewElement> createView(const std::string& name, int x, int y, int width, int height)
+{
+ 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);
+
+ return viewElement;
+}
+}
+
+int main(int argc, char* argv[])
+{
+
+ // Parse commandline parameters
+ po::options_description visible("Allowed options");
+ visible.add_options()("help", "produce help message")
+ ("config-file", po::value<std::string>(), "The config file to use");
+
+ po::options_description hidden("Hidden options");
+ hidden.add_options()("input-file", po::value<std::vector<std::string>>(), "input file");
+
+ po::options_description all("All options");
+ all.add(visible).add(hidden);
+
+ po::positional_options_description positional;
+ positional.add("input-file", -1);
+ po::variables_map variables;
+ po::store(po::command_line_parser(argc, argv).options(all).positional(positional).run(), variables);
+
+ if (variables.count("help"))
+ {
+ std::cout << visible << "\n";
+ return 1;
+ }
+
+ if (variables.count("input-file") == 0)
+ {
+ std::cout << "You need to supply one or more input files.\n";
+ return 1;
+ }
+
+ std::shared_ptr<Runtime> runtime;
+
+ if (variables.count("config-file") == 1)
+ {
+ runtime = std::make_shared<Runtime>(variables["config-file"].as<std::string>());
+ }
+ else
+ {
+ runtime = std::make_shared<Runtime>();
+ }
+
+ auto data = runtime->getApplicationData();
+
+ runtime->addManager(std::make_shared<SurgSim::Graphics::OsgManager>());
+ runtime->addManager(std::make_shared<SurgSim::Framework::BehaviorManager>());
+
+ auto scene = runtime->getScene();
+
+ auto viewElement = createView("View", 40, 40, 1024, 768);
+ scene->addSceneElement(viewElement);
+ viewElement->enableManipulator(true);
+ viewElement->setManipulatorParameters(Vector3d(0, 0, 2), Vector3d(0, 0, 0));
+
+ auto element = std::make_shared<BasicSceneElement>("Graphics");
+
+ auto axes = std::make_shared<OsgAxesRepresentation>("axes");
+ element->addComponent(axes);
+
+ auto files = variables["input-file"].as<std::vector<std::string>>();
+ for (auto file : files)
+ {
+ auto appData = runtime->getApplicationData();
+ std::string path;
+
+#ifdef WIN32
+ // Fix windows backslashes coming in from the command-line, these may be absolute paths
+ std::replace_if(file.begin(), file.end(), [](const char& c)
+ {
+ return c == '\\';
+ }, '/');
+#endif
+
+ // In this case we circumvent the assertion that will stop execution if the file can't be found and
+ // try to show as many scenery objects as possible
+ if (appData->tryFindFile(file, &path))
+ {
+ auto graphics = std::make_shared<OsgSceneryRepresentation>(file);
+ graphics->loadModel(file);
+ element->addComponent(graphics);
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(Logger::getDefaultLogger())
+ << "Can't find " << file;
+ }
+ }
+
+ scene->addSceneElement(element);
+
+ runtime->execute();
+
+ return 0;
+}
diff --git a/Examples/ShowScenery/config.txt.in b/Examples/ShowScenery/config.txt.in
new file mode 100644
index 0000000..18ced4c
--- /dev/null
+++ b/Examples/ShowScenery/config.txt.in
@@ -0,0 +1 @@
+${PROJECT_BINARY_DIR}/Data/
diff --git a/Examples/Stapling/CMakeLists.txt b/Examples/Stapling/CMakeLists.txt
index 5823ab3..5b8c872 100644
--- a/Examples/Stapling/CMakeLists.txt
+++ b/Examples/Stapling/CMakeLists.txt
@@ -15,34 +15,29 @@
include_directories(
${CMAKE_CURRENT_SOURCE_DIR}
- ${OSG_INCLUDE_DIR}
)
-set(SOURCES
+set(STAPLING_SOURCES
StapleElement.cpp
StaplerBehavior.cpp
+ Stapling.cpp
)
-
-set(HEADERS
+set(STAPLING_HEADERS
StapleElement.h
StaplerBehavior.h
)
+surgsim_add_executable(Stapling "${STAPLING_SOURCES}" "${STAPLING_HEADERS}")
-add_executable(Stapling
- Stapling.cpp
- ${SOURCES}
- ${HEADERS}
-)
-
-add_executable(SerializedStapling
+set(SERIALIZEDSTAPLING_SOURCES
SerializedStapling.cpp
- ${SOURCES}
- ${HEADERS}
+ StapleElement.cpp
+ StaplerBehavior.cpp
)
-
-add_dependencies(SerializedStapling yaml-cpp)
-
-surgsim_show_ide_folders("Main.cpp SerializedMain.cpp ${SOURCES}" "${HEADERS}")
+set(SERIALIZEDSTAPLING_HEADERS
+ StapleElement.h
+ StaplerBehavior.h
+)
+surgsim_add_executable(SerializedStapling "${SERIALIZEDSTAPLING_SOURCES}" "${SERIALIZEDSTAPLING_HEADERS}")
set(LIBS
IdentityPoseDevice
diff --git a/Examples/Stapling/Data/StaplingDemo.yaml b/Examples/Stapling/Data/StaplingDemo.yaml
index 8f59ade..d3269f4 100644
--- a/Examples/Stapling/Data/StaplingDemo.yaml
+++ b/Examples/Stapling/Data/StaplingDemo.yaml
@@ -2,133 +2,162 @@ SurgSim::Framework::Scene:
SceneElements:
- SurgSim::Framework::BasicSceneElement:
Name: StaplingDemoView
+ IsActive: true
Components:
- SurgSim::Graphics::OsgCamera:
+ IsLocalActive: true
+ RenderGroupReference: __OssDefault__
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:
[]
+ Id: 9ac2cca4-b2df-4ccb-bd6a-d09eed0b1f71
DrawAsWireFrame: false
- AmbientColor: [0, 0, 0, 0]
+ ProjectionMatrix: [[2.414213562373095, 0, 0, 0], [0, 2.414213562373095, 0, 0], [0, 0, -1.002002002002002, -0.02002002002002002], [0, 0, -1, 0]]
+ Name: StaplingDemoView Camera
+ AmbientColor: [0.2, 0.2, 0.2, 1]
- SurgSim::Framework::PoseComponent:
+ Id: 61e00b41-e0bf-4048-9cc9-69d828da8845
+ Name: Pose
+ IsLocalActive: true
Pose:
Quaternion: [0, 0, 0, 1]
Translation: [0, 0, 0]
- Name: Pose
- Id: b7ea8b3d-ef64-4ade-ab89-664390816842
- SurgSim::Graphics::OsgView:
+ IsLocalActive: true
+ FullScreen: false
Position:
- 0
- 0
- FullScreen: false
Dimensions:
- 1024
- 768
- EyeSeparation: 0.06
- ScreenDistance: 1
Camera:
SurgSim::Graphics::OsgCamera:
- Id: 169b057e-e2d7-4f6d-8628-e1b5cf70835d
+ Id: 9ac2cca4-b2df-4ccb-bd6a-d09eed0b1f71
Name: StaplingDemoView Camera
- WindowBorder: true
DisplayType: 0
- TargetScreen: 0
- KeyboardDeviceEnabled: true
StereoMode: -1
- ScreenWidth: 0
+ WindowBorder: true
+ TargetScreen: 0
+ EyeSeparation: 0.06
ScreenHeight: 0
+ ScreenDistance: 1
+ ScreenWidth: 0
CameraManipulatorEnabled: true
CameraPosition: [0, 0.5, 0.5]
CameraLookAt: [0, 0, 0]
OsgMapUniforms: false
+ KeyboardDeviceEnabled: true
+ Id: 4740c5c9-a638-4eec-b9ad-b6117733ec7a
MouseDeviceEnabled: false
- Id: f79e6280-9905-40b2-bb97-887bb689e605
Name: StaplingDemoView View
- SurgSim::Framework::BasicSceneElement:
Name: arm
+ IsActive: true
Components:
- SurgSim::Framework::PoseComponent:
Pose:
Quaternion: [0, 0, 0, 1]
Translation: [0, -0.2, 0]
- Id: 24879435-329d-47c6-a4d5-8f50c0c3e35c
+ IsLocalActive: true
+ Id: 194f4f32-bfc6-4b22-bc42-b80ee0414edd
Name: Pose
- - SurgSim::Physics::RigidCollisionRepresentation:
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ DrawAsWireFrame: true
+ Name: Collision Mesh
+ Id: 5cd67775-3f9c-40c1-b4ae-4d516272fb9c
+ MeshFileName: Geometry/arm_collision.ply
+ UpdateOptions: 1
+ GroupReferences:
+ - __OssDefault__
LocalPose:
Quaternion: [0, 0, 0, 1]
Translation: [0, 0, 0]
- Name: Collision
- Id: 16149947-44fa-404f-bfaa-096e7732ee1d
+ IsLocalActive: false
- SurgSim::Graphics::OsgSceneryRepresentation:
- LocalPose:
- Quaternion: [0, 0, 0, 1]
- Translation: [0, 0, 0]
- Visible: true
+ Id: 201e6e8d-59f9-409c-8688-64192aedad4a
+ DrawAsWireFrame: false
+ Name: Forearm
+ ModelFileName: Geometry/forearm.osgb
+ IsLocalActive: 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:
+ - SurgSim::Physics::RigidCollisionRepresentation:
+ IsLocalActive: true
+ Id: 485750a6-32c7-4487-a362-050fee89f45d
+ Name: Collision
+ Ignore: [wound/Collision]
LocalPose:
Quaternion: [0, 0, 0, 1]
Translation: [0, 0, 0]
- Visible: true
+ Shape:
+ SurgSim::Math::MeshShape:
+ FileName: Geometry/arm_collision.ply
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ IsLocalActive: true
+ ModelFileName: Geometry/upperarm.osgb
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
+ Id: 2a8aba74-66af-4c41-879f-38c54bdc931f
+ DrawAsWireFrame: false
+ Name: Upperarm
+ - SurgSim::Physics::FixedRepresentation:
IsGravityEnabled: true
- IsLocalActive: true
CollisionRepresentation:
SurgSim::Physics::RigidCollisionRepresentation:
- Id: 16149947-44fa-404f-bfaa-096e7732ee1d
+ Id: 485750a6-32c7-4487-a362-050fee89f45d
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
+ IsDrivingSceneElementPose: true
LinearDamping: 0
AngularDamping: 0
Shape:
SurgSim::Math::MeshShape:
FileName: Geometry/arm_collision.ply
- IsDrivingSceneElementPose: true
+ Id: 007741f4-d2d7-4f4f-9d2c-505539aa00bf
+ Name: Physics
+ IsLocalActive: true
+ NumDof: 0
+ Density: 0
+ RigidState:
+ SurgSim::Physics::RigidState:
+ Pose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ LinearVelocity: [0, 0, 0]
+ AngularVelocity: [0, 0, 0]
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
- SurgSim::Framework::BasicSceneElement:
+ Name: stapler
+ IsActive: true
Components:
+ - SurgSim::Blocks::VisualizeContactsBehavior:
+ VectorFieldScale: 200
+ Id: f7cde302-5c80-4f2b-a80b-a74f55bf15be
+ Name: Contacts
+ Source:
+ SurgSim::Physics::RigidCollisionRepresentation:
+ Name: Collision
+ Id: ec7ac2e2-93b5-46e6-9fda-55b125555092
+ IsLocalActive: false
- SurgSim::Physics::VirtualToolCoupler:
+ AttachmentPoint:
+ Value: [0, 0, 0]
+ HasValue: true
+ IsLocalActive: true
+ Input:
+ SurgSim::Input::InputComponent:
+ Id: 94c01580-98ac-49b1-b8c7-626ace6dbbfd
+ Name: InputComponent
LinearStiffness:
HasValue: false
Value: Not set
@@ -138,326 +167,312 @@ SurgSim::Framework::Scene:
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
+ Id: 87eefb88-55d8-48c7-81c9-f8cb24aa65fd
Name: Physics
- Id: 0f65703b-00aa-40d7-82c7-d9e27bc7021b
+ AngularDamping:
+ HasValue: false
+ Value: Not set
+ CalculateInertialTorques: false
+ Id: dd56fed8-aa69-4d77-99ce-60364d8cb122
Name: VTC
- SurgSim::Framework::PoseComponent:
- Pose:
- Quaternion: [0, 0, 0, 1]
- Translation: [0, 0, 0]
- Id: 886ced11-abc7-4c4d-8086-9f53247c5f83
+ IsLocalActive: true
+ Id: f0f6bb9c-7676-450d-9cb6-e97b0fe1b56d
Name: Pose
- - SurgSim::Graphics::OsgMeshRepresentation:
- LocalPose:
+ Pose:
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:
+ NumDof: 6
Density: 8050
- LinearDamping: 0
- AngularDamping: 0
- Shape:
- SurgSim::Math::MeshShape:
- FileName: Geometry/stapler_collision.ply
- IsDrivingSceneElementPose: true
+ IsLocalActive: 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:
+ RigidState:
+ SurgSim::Physics::RigidState:
LinearVelocity: [0, 0, 0]
AngularVelocity: [0, 0, 0]
Pose:
Quaternion: [0, 0, 0, 1]
Translation: [0, 0, 0]
- Id: 4e069c9f-4684-4b31-b08e-9e43c48af422
+ CollisionRepresentation:
+ SurgSim::Physics::RigidCollisionRepresentation:
+ Id: ec7ac2e2-93b5-46e6-9fda-55b125555092
+ Name: Collision
+ IsDrivingSceneElementPose: true
+ LinearDamping: 0
+ AngularDamping: 0
+ Shape:
+ SurgSim::Math::MeshShape:
+ FileName: Geometry/stapler_collision.ply
+ Id: 87eefb88-55d8-48c7-81c9-f8cb24aa65fd
Name: Physics
+ - SurgSim::Input::InputComponent:
+ DeviceName: MultiAxisDevice
+ Id: 94c01580-98ac-49b1-b8c7-626ace6dbbfd
+ IsLocalActive: true
+ Name: InputComponent
+ - SurgSim::Physics::RigidCollisionRepresentation:
+ IsLocalActive: true
+ Shape:
+ SurgSim::Math::MeshShape:
+ FileName: Geometry/stapler_collision.ply
+ Id: ec7ac2e2-93b5-46e6-9fda-55b125555092
+ Name: Collision
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ Name: Collision Mesh
+ DrawAsWireFrame: true
+ Id: e1d65704-5035-4db0-91b7-a722b28f6b27
+ UpdateOptions: 1
+ IsLocalActive: false
+ MeshFileName: Geometry/stapler_collision.ply
+ GroupReferences:
+ - __OssDefault__
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
- StaplerBehavior:
StapleEnabledSceneElements:
- - armSceneElement
+ - wound
InputComponent:
SurgSim::Input::InputComponent:
- Id: a0686eb4-795b-4c66-8ecd-3a8b9b72eba4
+ Id: 94c01580-98ac-49b1-b8c7-626ace6dbbfd
Name: InputComponent
+ IsLocalActive: true
Representation:
SurgSim::Physics::RigidRepresentation:
- Id: 4e069c9f-4684-4b31-b08e-9e43c48af422
+ Id: 87eefb88-55d8-48c7-81c9-f8cb24aa65fd
Name: Physics
+ Id: 34e5245b-9b67-4370-86d8-eddd92d7fde3
VirtualTeeth:
- SurgSim::Collision::ShapeCollisionRepresentation:
- Id: 56a34e38-2875-4f2b-971d-840d2579e902
+ Id: b7f70a7d-a6ab-4719-8fcd-698912de1aa6
Name: VirtualToothCollision0
- SurgSim::Collision::ShapeCollisionRepresentation:
- Id: ae77a6b2-8735-465d-ace0-c7cb0953ddc7
+ Id: c5fae459-35d8-4ded-b478-d04ed66e8694
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
+ ModelFileName: Geometry/stapler_indicator.obj
+ IsLocalActive: 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__
+ Id: 11a4f850-b8e3-43c6-a9d8-6d76bcd24998
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
+ Name: Handle
+ DrawAsWireFrame: false
+ IsLocalActive: true
+ ModelFileName: Geometry/stapler_handle.obj
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
+ Id: 5709c8b8-b12b-42fa-9bd8-37e5fd55773e
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ IsLocalActive: true
+ ModelFileName: Geometry/stapler_markings.obj
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
+ Id: 787b6765-694b-4682-a252-7bfbc8d0e40f
+ DrawAsWireFrame: false
+ Name: Markings
+ - SurgSim::Collision::ShapeCollisionRepresentation:
+ IsLocalActive: true
Shape:
SurgSim::Math::MeshShape:
FileName: Geometry/virtual_staple_1.ply
+ Id: b7f70a7d-a6ab-4719-8fcd-698912de1aa6
Name: VirtualToothCollision0
- - SurgSim::Graphics::OsgMeshRepresentation:
+ Ignore: [stapler/Collision]
LocalPose:
Quaternion: [0, 0, 0, 1]
Translation: [0, 0, 0]
- Visible: true
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ ModelFileName: Geometry/stapler_trigger.obj
+ IsLocalActive: 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]
+ Id: b2aa7537-2c8c-460f-8ec9-3e9f71cc7b5f
+ DrawAsWireFrame: false
+ Name: Trigger
+ - SurgSim::Collision::ShapeCollisionRepresentation:
+ IsLocalActive: true
Shape:
SurgSim::Math::MeshShape:
FileName: Geometry/virtual_staple_2.ply
- Id: ae77a6b2-8735-465d-ace0-c7cb0953ddc7
+ Id: c5fae459-35d8-4ded-b478-d04ed66e8694
Name: VirtualToothCollision1
- - SurgSim::Graphics::OsgMeshRepresentation:
+ Ignore: [stapler/Collision]
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
+ IsActive: true
Components:
- SurgSim::Framework::PoseComponent:
+ IsLocalActive: true
+ Id: e14c0e56-22c1-42af-8fb8-1f5caf32903e
+ Name: Pose
Pose:
Quaternion: [0, 0, 0, 1]
Translation: [0, -0.2, 0]
- Id: b147706a-ea62-4e8b-8ebf-fecd478750d6
- Name: Pose
- SurgSim::Graphics::OsgMeshRepresentation:
+ IsLocalActive: false
+ MeshFileName: Geometry/wound_deformable.ply
+ GroupReferences:
+ - __OssDefault__
LocalPose:
Quaternion: [0, 0, 0, 1]
Translation: [0, 0, 0]
- Visible: true
- GroupReferences:
- - __OssDefault__
- DrawAsWireFrame: false
- Filename: Geometry/wound_deformable.ply
+ DrawAsWireFrame: true
+ Id: 68c8db9d-4b71-4b94-b878-aa7bb594cc10
UpdateOptions: 1
- Id: ef06d974-74a8-4339-8885-7d42d281b6be
- Name: Triangle mesh
+ Name: Wire Frame
+ - SurgSim::Blocks::TransferPhysicsToGraphicsMeshBehavior:
+ Target:
+ SurgSim::Graphics::OsgMeshRepresentation:
+ Id: 21058a78-ddc4-421f-b44f-f35fc918a7da
+ Name: Graphics
+ Id: fd091469-54b7-4dd3-a395-ebd43daa1ad7
+ Name: PhysicsToGraphicalFem
+ Source:
+ SurgSim::Physics::Fem3DRepresentation:
+ Id: 7d447c80-d8c0-4213-a46f-164a800f6d1c
+ Name: Physics
+ IsLocalActive: true
- SurgSim::Physics::Fem3DRepresentation:
- IsDrivingSceneElementPose: true
+ IsLocalActive: true
+ NumDof: 1173
+ IsGravityEnabled: true
LocalPose:
Quaternion: [0, 0, 0, 1]
Translation: [0, 0, 0]
- Filename: Geometry/wound_deformable.ply
- NumDof: 1173
- IsGravityEnabled: true
- IsLocalActive: true
+ IsDrivingSceneElementPose: true
CollisionRepresentation:
SurgSim::Physics::DeformableCollisionRepresentation:
- Id: 46070a73-9e34-4a9a-bd83-7d4dbf4cfa26
+ Id: 2cb010a8-8ac3-4735-9551-1a1c17388e27
Name: Collision
- IntegrationScheme:
- SurgSim::Math::IntegrationScheme: INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER
- Id: 5b21c6c5-9e7a-4b4c-a1dd-521a83bd3658
+ FemFileName: Geometry/wound_deformable.ply
+ Id: 7d447c80-d8c0-4213-a46f-164a800f6d1c
Name: Physics
+ IntegrationScheme:
+ SurgSim::Math::IntegrationScheme: INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ IsLocalActive: true
+ MeshFileName: Geometry/wound_deformable.ply
+ GroupReferences:
+ - __OssDefault__
+ DrawAsWireFrame: false
+ Id: 21058a78-ddc4-421f-b44f-f35fc918a7da
+ UpdateOptions: 1
+ Name: Graphics
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
- SurgSim::Physics::DeformableCollisionRepresentation:
LocalPose:
Quaternion: [0, 0, 0, 1]
Translation: [0, 0, 0]
+ IsLocalActive: true
Shape:
SurgSim::Math::MeshShape:
FileName: Geometry/wound_deformable.ply
- Id: 46070a73-9e34-4a9a-bd83-7d4dbf4cfa26
+ Id: 2cb010a8-8ac3-4735-9551-1a1c17388e27
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
+ Id: 7d447c80-d8c0-4213-a46f-164a800f6d1c
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:
+ IsLocalActive: true
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
+ Id: 68c8db9d-4b71-4b94-b878-aa7bb594cc10
+ Name: Wire Frame
+ Id: 4acc648a-e162-4b90-9072-8ccd4b96ce09
Name: PhysicsToWireFrameFem
- SurgSim::Framework::BasicSceneElement:
Name: SceneElement
+ IsActive: true
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:
+ IsLocalActive: true
InputComponent:
SurgSim::Input::InputComponent:
- Id: 616d866d-cc25-443a-a57f-1fcb43e7f376
+ Id: 5a9f89eb-868b-4842-bb97-590497b7a3dd
Name: KeyboardInputComponent
- Id: 81660851-2655-4891-9606-7462e3447b40
+ Id: 485bffe9-36e8-42b3-96fe-ab95069cd3cc
+ Name: KeyboardBehavior
KeyboardRegistry:
+ 98:
+ - SurgSim::Blocks::VisualizeContactsBehavior:
+ Id: f7cde302-5c80-4f2b-a80b-a74f55bf15be
+ Name: Contacts
+ 99:
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ Id: e1d65704-5035-4db0-91b7-a722b28f6b27
+ Name: Collision Mesh
97:
- SurgSim::Graphics::OsgSceneryRepresentation:
- Id: 81eae131-70c9-4752-82cf-9debaf12bcf2
- Name: Markings
- - SurgSim::Graphics::OsgSceneryRepresentation:
- Id: 18def2b5-a733-4021-b778-311c47ac5d7e
Name: Handle
+ Id: 5709c8b8-b12b-42fa-9bd8-37e5fd55773e
- SurgSim::Graphics::OsgSceneryRepresentation:
- Id: cbc68446-17a6-4d23-aa5f-803b05a779f9
- Name: Trigger
- - SurgSim::Graphics::OsgSceneryRepresentation:
- Id: fde00dc7-50de-433c-b596-8ee80da75aaa
+ Id: 11a4f850-b8e3-43c6-a9d8-6d76bcd24998
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
+ Id: b2aa7537-2c8c-460f-8ec9-3e9f71cc7b5f
+ Name: Trigger
- SurgSim::Graphics::OsgSceneryRepresentation:
- Id: 58b991bb-16dc-4fb0-9f0e-b153da9a641d
- Name: upperarm
- 100:
- - SurgSim::Graphics::OsgMeshRepresentation:
- Id: 4ea6a631-6289-4886-98d2-aa91580e8250
- Name: ArmOsgMesh
+ Id: 787b6765-694b-4682-a252-7bfbc8d0e40f
+ Name: Markings
101:
- SurgSim::Graphics::OsgMeshRepresentation:
- Id: ef06d974-74a8-4339-8885-7d42d281b6be
- Name: Triangle mesh
+ Id: 5cd67775-3f9c-40c1-b4ae-4d516272fb9c
+ Name: Collision Mesh
+ 100:
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ Id: 201e6e8d-59f9-409c-8688-64192aedad4a
+ Name: Forearm
+ - SurgSim::Graphics::OsgSceneryRepresentation:
+ Id: 2a8aba74-66af-4c41-879f-38c54bdc931f
+ Name: Upperarm
102:
- SurgSim::Graphics::OsgMeshRepresentation:
- Id: dbafb504-0f09-4f13-9713-7d83b41d4329
- Name: Wire frame
- Name: KeyboardBehavior
+ Id: 21058a78-ddc4-421f-b44f-f35fc918a7da
+ Name: Graphics
+ 103:
+ - SurgSim::Graphics::OsgMeshRepresentation:
+ Id: 68c8db9d-4b71-4b94-b878-aa7bb594cc10
+ Name: Wire Frame
+ - SurgSim::Framework::PoseComponent:
+ IsLocalActive: true
+ Id: f26ccf32-aeb8-4790-a612-01a2865bfef9
+ Name: Pose
+ Pose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ - SurgSim::Input::InputComponent:
+ DeviceName: Keyboard
+ Id: 5a9f89eb-868b-4842-bb97-590497b7a3dd
+ IsLocalActive: true
+ Name: KeyboardInputComponent
diff --git a/Examples/Stapling/SerializedStapling.cpp b/Examples/Stapling/SerializedStapling.cpp
index 8cf4974..f8ae756 100644
--- a/Examples/Stapling/SerializedStapling.cpp
+++ b/Examples/Stapling/SerializedStapling.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,30 +16,18 @@
#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/Blocks/Blocks.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;
+#include "SurgSim/Devices/MultiAxis/MultiAxisDevice.h"
+#include "SurgSim/Framework/Framework.h"
+#include "SurgSim/Graphics/Graphics.h"
+#include "SurgSim/Input/Input.h"
+#include "SurgSim/Math/Math.h"
+#include "SurgSim/Physics/Physics.h"
+
+using SurgSim::Devices::IdentityPoseDevice;
+using SurgSim::Devices::MultiAxisDevice;
using SurgSim::Framework::BehaviorManager;
using SurgSim::Framework::Runtime;
using SurgSim::Framework::SceneElement;
@@ -53,7 +41,7 @@ using SurgSim::Physics::PhysicsManager;
template <typename Type>
std::shared_ptr<Type> getComponentChecked(std::shared_ptr<SurgSim::Framework::SceneElement> sceneElement,
- const std::string& name)
+ const std::string& name)
{
std::shared_ptr<SurgSim::Framework::Component> component = sceneElement->getComponent(name);
SURGSIM_ASSERT(component != nullptr) << "Failed to get Component named '" << name << "'.";
@@ -84,37 +72,19 @@ int main(int argc, char* argv[])
if (!device->initialize())
{
SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
- << "Could not initialize device " << device->getName() << " for the tool.";
+ << "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);
+ runtime->loadScene("StaplingDemo.yaml");
- 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
index 58936fa..92d6be1 100644
--- a/Examples/Stapling/StapleElement.cpp
+++ b/Examples/Stapling/StapleElement.cpp
@@ -17,6 +17,7 @@
#include "SurgSim/Framework/ApplicationData.h"
#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Graphics/Model.h"
#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
#include "SurgSim/Math/MeshShape.h"
#include "SurgSim/Physics/RigidCollisionRepresentation.h"
@@ -53,7 +54,7 @@ bool StapleElement::doInitialize()
std::shared_ptr<SceneryRepresentation> graphicsRepresentation =
std::make_shared<OsgSceneryRepresentation>("Graphics");
- graphicsRepresentation->setFileName("Geometry/staple.obj");
+ graphicsRepresentation->loadModel("Geometry/staple.obj");
addComponent(physicsRepresentation);
addComponent(graphicsRepresentation);
diff --git a/Examples/Stapling/StapleElement.h b/Examples/Stapling/StapleElement.h
index 6bd8bdd..31ec622 100644
--- a/Examples/Stapling/StapleElement.h
+++ b/Examples/Stapling/StapleElement.h
@@ -34,7 +34,7 @@ public:
protected:
/// Initialize this scene element
/// \return True on success, otherwise false.
- virtual bool doInitialize() override;
+ bool doInitialize() override;
private:
/// Flag to specify if the stapleElement was created with a collision representation.
diff --git a/Examples/Stapling/StaplerBehavior.cpp b/Examples/Stapling/StaplerBehavior.cpp
index 512579b..f52fd98 100644
--- a/Examples/Stapling/StaplerBehavior.cpp
+++ b/Examples/Stapling/StaplerBehavior.cpp
@@ -27,24 +27,24 @@
#include "SurgSim/Framework/Scene.h"
#include "SurgSim/Framework/SceneElement.h"
#include "SurgSim/Graphics/SceneryRepresentation.h"
+#include "SurgSim/Graphics/Model.h"
+#include "SurgSim/Math/MlcpConstraintType.h"
#include "SurgSim/Input/InputComponent.h"
#include "SurgSim/Physics/Constraint.h"
#include "SurgSim/Physics/ConstraintComponent.h"
+#include "SurgSim/Physics/ConstraintImplementation.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/RigidConstraintFixedPoint.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;
+using SurgSim::Physics::RigidConstraintFixedPoint;
+using SurgSim::Framework::checkAndConvert;
SURGSIM_REGISTER(SurgSim::Framework::Component, StaplerBehavior, StaplerBehavior);
@@ -57,22 +57,19 @@ StaplerBehavior::StaplerBehavior(const std::string& name):
{
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);
+ InputComponent, getInputComponent, setInputComponent);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(StaplerBehavior, std::shared_ptr<SurgSim::Framework::Component>,
- Representation, getRepresentation, setRepresentation);
+ Representation, getRepresentation, setRepresentation);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(StaplerBehavior, VirtualTeethArray, VirtualTeeth,
- getVirtualTeeth, setVirtualTeeth);
+ getVirtualTeeth, setVirtualTeeth);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(StaplerBehavior, std::list<std::string>, StapleEnabledSceneElements,
- getStapleEnabledSceneElements, setStapleEnabledSceneElements);
+ 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";
+ m_from = checkAndConvert<SurgSim::Input::InputComponent>(inputComponent, "SurgSim::Input::InputComponent");
}
std::shared_ptr<SurgSim::Input::InputComponent> StaplerBehavior::getInputComponent()
@@ -83,11 +80,8 @@ std::shared_ptr<SurgSim::Input::InputComponent> StaplerBehavior::getInputCompone
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";
+ m_representation = checkAndConvert<SurgSim::Framework::Representation>(
+ staplerRepresentation, "SurgSim::Framework::Representation");
}
std::shared_ptr<SurgSim::Framework::Representation> StaplerBehavior::getRepresentation()
@@ -106,7 +100,7 @@ const std::array<std::shared_ptr<SurgSim::Collision::Representation>, 2>& Staple
return m_virtualTeeth;
}
-void StaplerBehavior::enableStaplingForSceneElement(std::string sceneElementName)
+void StaplerBehavior::enableStaplingForSceneElement(const std::string& sceneElementName)
{
m_stapleEnabledSceneElements.push_back(sceneElementName);
}
@@ -183,62 +177,6 @@ void StaplerBehavior::filterCollisionMapForSupportedRepresentationTypes(ContactM
}
}
-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).
@@ -281,15 +219,19 @@ void StaplerBehavior::createStaple()
// 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(); });
+ [](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; });
+ {
+ 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.
@@ -315,29 +257,18 @@ void StaplerBehavior::createStaple()
// 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()));
+ 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;
- }
+ auto constraint = std::make_shared<SurgSim::Physics::Constraint>(SurgSim::Physics::FIXED_3DPOINT,
+ std::make_shared<SurgSim::Physics::ConstraintData>(), stapleRepresentation,
+ stapleConstraintLocation, targetPhysicsRepresentation, targetContact->penetrationPoints.second);
// 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++));
+ "FixedPointConstraint" + boost::to_string(toothId++));
constraintComponent->setConstraint(constraint);
staple->addComponent(constraintComponent);
@@ -383,8 +314,9 @@ int StaplerBehavior::getTargetManagerType() const
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.";
+ 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;
}
diff --git a/Examples/Stapling/StaplerBehavior.h b/Examples/Stapling/StaplerBehavior.h
index 549ad78..f1e401d 100644
--- a/Examples/Stapling/StaplerBehavior.h
+++ b/Examples/Stapling/StaplerBehavior.h
@@ -85,11 +85,11 @@ public:
/// Update the behavior
/// \param dt The length of time (seconds) between update calls.
- virtual void update(double dt) override;
+ 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;
+ int getTargetManagerType() const override;
/// Sets the virtual teeth for the virtual staple
/// \param virtualTeeth Array of collision representations for the virtual stapler teeth.
@@ -100,7 +100,7 @@ public:
/// 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);
+ void enableStaplingForSceneElement(const 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.
@@ -113,12 +113,12 @@ protected:
/// Initialize this behavior
/// \return True on success, otherwise false.
/// \note: In current implementation, this method always returns "true".
- virtual bool doInitialize() override;
+ 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;
+ bool doWakeUp() override;
private:
/// Given a collision map, remove entries whose representations are not part of
@@ -136,18 +136,6 @@ private:
/// \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.
diff --git a/Examples/Stapling/Stapling.cpp b/Examples/Stapling/Stapling.cpp
index 6b8fcd7..94bc405 100644
--- a/Examples/Stapling/Stapling.cpp
+++ b/Examples/Stapling/Stapling.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -17,51 +17,24 @@
#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/Blocks/Blocks.h"
+#include "SurgSim/Collision/Collision.h"
+#include "SurgSim/DataStructures/DataStructures.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"
+#include "SurgSim/Framework/Framework.h"
+#include "SurgSim/Graphics/Graphics.h"
+#include "SurgSim/Input/Input.h"
+#include "SurgSim/Math/Math.h"
+#include "SurgSim/Physics/Physics.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::Devices::IdentityPoseDevice;
+using SurgSim::Devices::MultiAxisDevice;
using SurgSim::Framework::BasicSceneElement;
using SurgSim::Framework::BehaviorManager;
using SurgSim::Framework::Runtime;
@@ -96,15 +69,17 @@ static std::shared_ptr<SurgSim::Framework::SceneElement> createFemSceneElement(
const std::string& name,
const std::string& filename,
SurgSim::Math::IntegrationScheme integrationScheme,
+ SurgSim::Math::LinearSolver linearSolver,
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.
+ // Load the tetrahedral mesh.
std::shared_ptr<Fem3DRepresentation> physicsRepresentation = std::make_shared<Fem3DRepresentation>("Physics");
- physicsRepresentation->setFilename(filename);
+ physicsRepresentation->loadFem(filename);
physicsRepresentation->setIntegrationScheme(integrationScheme);
+ physicsRepresentation->setLinearSolver(linearSolver);
sceneElement->addComponent(physicsRepresentation);
// Load the surface triangle mesh of the finite element model
@@ -113,7 +88,7 @@ static std::shared_ptr<SurgSim::Framework::SceneElement> createFemSceneElement(
// Create a triangle mesh for visualizing the surface of the finite element model
auto graphicalFem = std::make_shared<OsgMeshRepresentation>("Graphics");
- graphicalFem->setFilename(filename);
+ graphicalFem->loadMesh(filename);
sceneElement->addComponent(graphicalFem);
// Create material to transport the Textures
@@ -135,7 +110,7 @@ static std::shared_ptr<SurgSim::Framework::SceneElement> createFemSceneElement(
// 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->setShape(meshShape);
wireFrameFem->setDrawAsWireFrame(true);
wireFrameFem->setLocalActive(false);
sceneElement->addComponent(wireFrameFem);
@@ -156,7 +131,7 @@ static std::shared_ptr<SurgSim::Framework::SceneElement> createFemSceneElement(
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);
+ sceneryRepresentation->loadModel(fileName);
return sceneryRepresentation;
}
@@ -170,7 +145,7 @@ std::shared_ptr<SceneElement> createStaplerSceneElement(const std::string& stapl
std::shared_ptr<MeshRepresentation> meshShapeVisualization =
std::make_shared<OsgMeshRepresentation>("Collision Mesh");
- meshShapeVisualization->setFilename(filename);
+ meshShapeVisualization->setShape(meshShapeForCollision);
meshShapeVisualization->setDrawAsWireFrame(true);
meshShapeVisualization->setLocalActive(false);
@@ -202,7 +177,7 @@ std::shared_ptr<SceneElement> createStaplerSceneElement(const std::string& stapl
std::shared_ptr<VisualizeContactsBehavior> visualizeContactsBehavior =
std::make_shared<VisualizeContactsBehavior>("Contacts");
- visualizeContactsBehavior->setCollisionRepresentation(collisionRepresentation);
+ visualizeContactsBehavior->setSource(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.
@@ -241,6 +216,7 @@ std::shared_ptr<SceneElement> createStaplerSceneElement(const std::string& stapl
std::make_shared<ShapeCollisionRepresentation>("VirtualToothCollision" + std::to_string(i));
virtualToothCollision->setShape(*it);
virtualToothCollision->setLocalPose(RigidTransform3d::Identity());
+ virtualToothCollision->ignore(collisionRepresentation);
virtualTeeth[i] = virtualToothCollision;
sceneElement->addComponent(virtualToothCollision);
@@ -272,7 +248,7 @@ std::shared_ptr<SceneElement> createArmSceneElement(
// Visualization of arm collision mesh
std::shared_ptr<MeshRepresentation> meshShapeVisualization =
std::make_shared<OsgMeshRepresentation>("Collision Mesh");
- meshShapeVisualization->setFilename(filename);
+ meshShapeVisualization->setShape(meshShape);
meshShapeVisualization->setDrawAsWireFrame(true);
meshShapeVisualization->setLocalActive(false);
@@ -281,6 +257,7 @@ std::shared_ptr<SceneElement> createArmSceneElement(
std::shared_ptr<RigidCollisionRepresentation> collisionRepresentation =
std::make_shared<RigidCollisionRepresentation>("Collision");
+ collisionRepresentation->ignore("wound/Collision");
physicsRepresentation->setCollisionRepresentation(collisionRepresentation);
std::shared_ptr<SceneElement> armSceneElement = std::make_shared<BasicSceneElement>(armName);
@@ -294,19 +271,6 @@ std::shared_ptr<SceneElement> createArmSceneElement(
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");
@@ -326,13 +290,13 @@ std::shared_ptr<OsgViewElement> createViewElement()
std::shared_ptr<SurgSim::Graphics::OsgMaterial> createShinyMaterial(
const SurgSim::Framework::ApplicationData& data,
- std::shared_ptr<SurgSim::Graphics::OsgShader> shader,
+ std::shared_ptr<SurgSim::Graphics::OsgProgram> program,
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);
+ material->setProgram(program);
{
auto uniform = std::make_shared<SurgSim::Graphics::OsgUniform<Vector4f>>("diffuseColor");
@@ -413,7 +377,7 @@ int main(int argc, char* argv[])
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");
+ auto shader = SurgSim::Graphics::loadProgram(*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));
@@ -435,7 +399,8 @@ int main(int argc, char* argv[])
std::shared_ptr<SceneElement> wound =
createFemSceneElement("wound",
woundFilename,
- SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT,
+ SurgSim::Math::LINEARSOLVER_LU,
material);
wound->setPose(armPose);
@@ -445,17 +410,18 @@ int main(int argc, char* argv[])
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"));
+ using SurgSim::Devices::KeyCode;
+ keyboardBehavior->registerKey(KeyCode::KEY_A, stapler->getComponent("Handle"));
+ keyboardBehavior->registerKey(KeyCode::KEY_A, stapler->getComponent("Indicator"));
+ keyboardBehavior->registerKey(KeyCode::KEY_A, stapler->getComponent("Markings"));
+ keyboardBehavior->registerKey(KeyCode::KEY_A, stapler->getComponent("Trigger"));
+ keyboardBehavior->registerKey(KeyCode::KEY_B, stapler->getComponent("Contacts"));
+ keyboardBehavior->registerKey(KeyCode::KEY_C, stapler->getComponent("Collision Mesh"));
+ keyboardBehavior->registerKey(KeyCode::KEY_D, arm->getComponent("Forearm"));
+ keyboardBehavior->registerKey(KeyCode::KEY_D, arm->getComponent("Upperarm"));
+ keyboardBehavior->registerKey(KeyCode::KEY_E, arm->getComponent("Collision Mesh"));
+ keyboardBehavior->registerKey(KeyCode::KEY_F, wound->getComponent("Graphics"));
+ keyboardBehavior->registerKey(KeyCode::KEY_G, wound->getComponent("Wire Frame"));
std::shared_ptr<SceneElement> keyboard = std::make_shared<BasicSceneElement>("SceneElement");
keyboard->addComponent(keyboardComponent);
@@ -468,19 +434,6 @@ int main(int argc, char* argv[])
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/Modules/CMakeLists.txt b/Modules/CMakeLists.txt
new file mode 100644
index 0000000..60a22c3
--- /dev/null
+++ b/Modules/CMakeLists.txt
@@ -0,0 +1,30 @@
+# 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(${CMAKE_CURRENT_SOURCE_DIR})
+
+file(GLOB MODULES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/*)
+foreach(MODULE ${MODULES})
+ if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/${MODULE}/CMakeLists.txt)
+ string(TOUPPER ${MODULE} MODULE_UPPER_CASE)
+ option(BUILD_MODULE_${MODULE_UPPER_CASE} "Build ${MODULE} module." OFF)
+ if(BUILD_MODULE_${MODULE_UPPER_CASE})
+ add_subdirectory(${MODULE})
+ endif(BUILD_MODULE_${MODULE_UPPER_CASE})
+ endif()
+endforeach()
+
+
+
diff --git a/Modules/README b/Modules/README
new file mode 100644
index 0000000..32e8c28
--- /dev/null
+++ b/Modules/README
@@ -0,0 +1,6 @@
+Modules Directory
+
+The Modules directory is a place to put extensions to OpenSurgSim. For
+all subdirectories that contain a CMakeLists.txt file, a
+BUILD_MODULE_<Directory_Name> cmake option will be added. If this
+option is turned on, the module will be added to OpenSurgSim.
diff --git a/NOTICE b/NOTICE
index 8813a1e..1f583a2 100644
--- a/NOTICE
+++ b/NOTICE
@@ -1,5 +1,5 @@
OpenSurgSim simulation framework
-Copyright 2012-2013, SimQuest Solutions Inc.
+Copyright 2012-2016, SimQuest Solutions Inc.
This product includes software developed by SimQuest Solutions
Inc. (http://www.simquest.com/).
@@ -45,4 +45,22 @@ 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
+------------------------------------------------------------
+
+Copyright
+
+Copyright (c) 2003 by Bitstream, Inc. All Rights Reserved. Bitstream Vera is a trademark of Bitstream, Inc.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of the fonts accompanying this license (“Fonts”) and associated documentation files (the “Font Software”), to reproduce and distribute the Font Software, including without limitation the rights to use, copy, merge, publish, distribute, and/or sell copies of the Font Software, and to permit persons to whom the Font Software is furnished to do so, subject to the following conditions:
+
+The above copyright and trademark notices and this permission notice shall be included in all copies of one or more of the Font Software typefaces.
+
+The Font Software may be modified, altered, or added to, and in particular the designs of glyphs or characters in the Fonts may be modified and additional glyphs or characters may be added to the Fonts, only if the fonts are renamed to names not containing either the words “Bitstream” or the word “Vera”.
+
+This License becomes null and void to the extent applicable to Fonts or Font Software that has been modified and is distributed under the “Bitstream Vera” names.
+
+The Font Software may be sold as part of a larger software package but no copy of one or more of the Font Software typefaces may be sold by itself.
+
+THE FONT SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO ANY WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF COPYRIGHT, PATENT, TRADEMARK, OR OTHER RIGHT. IN NO EVENT SHALL BITSTREAM OR THE GNOME FOUNDATION BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, INCLUDING ANY GENERAL, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, [...]
+
+Except as contained in this notice, the names of Gnome, the Gnome Foundation, and Bitstream Inc., shall not be used in advertising or otherwise to promote the sale, use or other dealings in this Font Software without prior written authorization from the Gnome Foundation or Bitstream Inc., respectively. For further information, contact: fonts at gnome dot org.
diff --git a/README b/README
deleted file mode 100644
index 8082eb4..0000000
--- a/README
+++ /dev/null
@@ -1,143 +0,0 @@
-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/README.md b/README.md
new file mode 100644
index 0000000..0a8e973
--- /dev/null
+++ b/README.md
@@ -0,0 +1,154 @@
+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 Unstable (Sid). 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 libeigen3-dev
+ sudo apt-get install google-mock libjs-mathjax libopenscenegraph-dev
+ sudo apt-get install libyaml-cpp-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. When we know of reliable pre-built Windows packages we will refer to them.
+
+Required Dependencies
+---------------------
+
+**Boost**<br>
+Homepage: http://www.boost.org/<br>
+Modules: chrono, date_time, filesystem, program_options, system, thread<br>
+Minimum Version: 1.54<br>
+Windows Binaries: http://sourceforge.net/projects/boost/files/boost-binaries/
+
+**CMake**<br>
+Homepage: http://www.cmake.org/<br>
+Minimum Version: 2.8
+
+**Eigen**<br>
+Homepage: http://eigen.tuxfamily.org/<br>
+Minimum Version: 3.2.1 (GNU/Linux)<br>
+Minimum Version: 3.2.3 (Windows)
+
+**Git**<br>
+Homepage: http://www.git-scm.com/<br>
+Minimum Version: 1.7.9
+
+**OpenSceneGraph**<br>
+Homepage: http://www.openscenegraph.org/<br>
+Modules: osg, osgViewer, osgText, osgUtil, osgDB, osgGA, osgAnimation<br>
+Minimum Version: 3.2.0
+
+**yaml-cpp**<br>
+Homepage: https://github.com/jbeder/yaml-cpp<br>
+Minimum Version: 0.5.2
+
+Optional Dependencies
+---------------------
+
+**Doxygen**<br>
+Homepage: http://doxygen.org/<br>
+Minimum Version: 1.8
+
+**FreeGlut**<br>
+Homepage: http://freeglut.sourceforge.net/<br>
+Minimum Version: 2.6.0
+
+**Google Mock**<br>
+Homepage: https://code.google.com/p/googlemock/<br>
+Minimum Version: 1.7.0
+
+**MathJax**<br>
+Homepage: http://www.mathjax.org/<br>
+Minimum Version: 2.4
+
diff --git a/SurgSim/Blocks/CMakeLists.txt b/SurgSim/Blocks/CMakeLists.txt
index 8d84195..40b08ca 100644
--- a/SurgSim/Blocks/CMakeLists.txt
+++ b/SurgSim/Blocks/CMakeLists.txt
@@ -1,5 +1,5 @@
# This file is a part of the OpenSurgSim project.
-# Copyright 2012-2013, SimQuest Solutions Inc.
+# Copyright 2012-2016, SimQuest Solutions Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -14,47 +14,71 @@
# limitations under the License.
include_directories(
- ${OSG_INCLUDE_DIR}
${OPENTHREADS_INCLUDE_DIR}
)
set(SURGSIM_BLOCKS_SOURCES
+ CompoundShapeToGraphics.cpp
+ DebugDumpBehavior.cpp
DriveElementFromInputBehavior.cpp
+ FunctionBehavior.cpp
+ GraphicsUtilities.cpp
+ ImplicitSurface.cpp
+ KeyBehavior.cpp
+ KeyboardCallbackBehavior.cpp
KeyboardTogglesComponentBehavior.cpp
MassSpring1DRepresentation.cpp
MassSpring2DRepresentation.cpp
MassSpring3DRepresentation.cpp
MassSpringNDRepresentationUtils.cpp
PoseInterpolator.cpp
+ ShadowMapping.cpp
+ SingleKeyBehavior.cpp
SphereElement.cpp
+ TransferParticlesToPointCloudBehavior.cpp
TransferPhysicsToGraphicsMeshBehavior.cpp
TransferPhysicsToPointCloudBehavior.cpp
+ TransferPhysicsToVerticesBehavior.cpp
+ VisualizeConstraints.cpp
VisualizeContactsBehavior.cpp
)
set(SURGSIM_BLOCKS_HEADERS
+ CompoundShapeToGraphics.h
+ DebugDumpBehavior.h
DriveElementFromInputBehavior.h
+ FunctionBehavior.h
+ GraphicsUtilities.h
+ ImplicitSurface.h
+ KeyBehavior.h
+ KeyboardCallbackBehavior.h
KeyboardTogglesComponentBehavior.h
MassSpring1DRepresentation.h
MassSpring2DRepresentation.h
MassSpring3DRepresentation.h
MassSpringNDRepresentationUtils.h
PoseInterpolator.h
+ ShadowMapping.h
+ SingleKeyBehavior.h
SphereElement.h
+ TransferParticlesToPointCloudBehavior.h
TransferPhysicsToGraphicsMeshBehavior.h
TransferPhysicsToPointCloudBehavior.h
+ TransferPhysicsToVerticesBehavior.h
+ VisualizeConstraints.h
VisualizeContactsBehavior.h
)
+surgsim_create_library_header(Blocks.h "${SURGSIM_BLOCKS_HEADERS}")
surgsim_add_library(
SurgSimBlocks
"${SURGSIM_BLOCKS_SOURCES}"
"${SURGSIM_BLOCKS_HEADERS}"
- "SurgSim/Blocks"
)
SET(LIBS
SurgSimFramework
+ SurgSimParticles
SurgSimPhysics
${Boost_LIBRARIES}
)
@@ -67,4 +91,4 @@ if(BUILD_TESTING)
endif()
# Put SurgSimBlocks into folder "Blocks"
-set_target_properties(SurgSimBlocks PROPERTIES FOLDER "Blocks")
+set_target_properties(SurgSimBlocks PROPERTIES FOLDER "Blocks")
diff --git a/SurgSim/Blocks/CompoundShapeToGraphics.cpp b/SurgSim/Blocks/CompoundShapeToGraphics.cpp
new file mode 100644
index 0000000..41828f3
--- /dev/null
+++ b/SurgSim/Blocks/CompoundShapeToGraphics.cpp
@@ -0,0 +1,151 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015dd, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <iterator>
+
+#include "SurgSim/Blocks/CompoundShapeToGraphics.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Math/CompoundShape.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Physics/Representation.h"
+
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Blocks::CompoundShapeToGraphics, CompoundShapeToGraphics);
+
+CompoundShapeToGraphics::CompoundShapeToGraphics(const std::string& name) : Framework::Behavior(name)
+{
+ {
+ typedef std::vector<std::shared_ptr<Framework::Component>> ParamType;
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CompoundShapeToGraphics, ParamType, Targets, getTargets, setTargets);
+ }
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CompoundShapeToGraphics, std::shared_ptr<Component>,
+ Source, getSource, setSource);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CompoundShapeToGraphics, std::shared_ptr<SurgSim::Math::Shape>,
+ Shape, getShape, setShape);
+}
+
+CompoundShapeToGraphics::~CompoundShapeToGraphics()
+{
+
+}
+
+void CompoundShapeToGraphics::update(double dt)
+{
+ size_t i = 0;
+ SURGSIM_ASSERT(m_shape->getNumShapes() >= m_representations.size())
+ << "Not enough shapes for the representations.";
+
+ for (const auto& representation : m_representations)
+ {
+ representation->setLocalPose(m_shape->getPose(i++));
+
+ }
+}
+
+int CompoundShapeToGraphics::getTargetManagerType() const
+{
+ return Framework::MANAGER_TYPE_GRAPHICS;
+}
+
+bool CompoundShapeToGraphics::doInitialize()
+{
+ return true;
+}
+
+bool CompoundShapeToGraphics::doWakeUp()
+{
+ if (m_source != nullptr)
+ {
+ std::shared_ptr<Math::Shape> shape;
+ m_source->getValue<std::shared_ptr<Math::Shape>>("Shape", &shape);
+
+ SURGSIM_ASSERT(shape != nullptr) << "Source " << m_source->getFullName() << " does not contain a shape.";
+ SURGSIM_ASSERT(shape->getType() == Math::SHAPE_TYPE_COMPOUNDSHAPE) << "Source : " << m_source->getFullName()
+ << "Does not contain a compound shape.";
+
+ m_shape = std::dynamic_pointer_cast<Math::CompoundShape>(shape);
+ }
+
+
+ return true;
+}
+
+void CompoundShapeToGraphics::setShape(const std::shared_ptr<Math::Shape>& shape)
+{
+ SURGSIM_ASSERT(m_source == nullptr) << "Can't assign the shape and the source at the same time.";
+ SURGSIM_ASSERT(shape != nullptr) << "Shape should not be nullptr.";
+ auto compoundShape = std::dynamic_pointer_cast<SurgSim::Math::CompoundShape>(shape);
+ SURGSIM_ASSERT(compoundShape != nullptr) << "'shape' is not a SurgSim::Math::CompoundShape.";
+
+ m_shape = compoundShape;
+ m_source = nullptr;
+}
+
+void CompoundShapeToGraphics::setSource(const std::shared_ptr<Framework::Component>& component)
+{
+ SURGSIM_ASSERT(m_shape == nullptr) << "Can't assign the shape and the source at the same time.";
+ SURGSIM_ASSERT(component != nullptr) << "Source should not be nullptr.";
+ SURGSIM_ASSERT(component->isReadable("Shape")) << "Source: " << component->getFullName()
+ << "Does not contain a shape.";
+ m_source = component;
+}
+
+void CompoundShapeToGraphics::setTargets(const std::vector<std::shared_ptr<Framework::Component>> components)
+{
+ m_representations.clear();
+ for (const auto& c : components)
+ {
+ addTarget(c);
+ }
+}
+
+void CompoundShapeToGraphics::addTarget(const std::shared_ptr<Framework::Component>& component)
+{
+ auto graphics = Framework::checkAndConvert<Graphics::Representation>(component,
+ "SurgSim::Graphics::Representation");
+ SURGSIM_ASSERT(graphics != nullptr)
+ << "Component " << component->getFullName() << " not a graphics representation, could not add.";
+ m_representations.push_back(std::move(graphics));
+}
+
+std::vector<std::shared_ptr<Framework::Component>> CompoundShapeToGraphics::getTargets() const
+{
+ std::vector<std::shared_ptr<Framework::Component>> result;
+ std::copy(m_representations.cbegin(), m_representations.cend(), std::back_inserter(result));
+ return result;
+}
+
+std::shared_ptr<Math::CompoundShape> CompoundShapeToGraphics::getShape() const
+{
+ return m_shape;
+}
+
+std::shared_ptr<Framework::Component> CompoundShapeToGraphics::getSource() const
+{
+ return m_source;
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Blocks/CompoundShapeToGraphics.h b/SurgSim/Blocks/CompoundShapeToGraphics.h
new file mode 100644
index 0000000..db0a967
--- /dev/null
+++ b/SurgSim/Blocks/CompoundShapeToGraphics.h
@@ -0,0 +1,115 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_COMPOUNDSHAPETOGRAPHICS_H
+#define SURGSIM_BLOCKS_COMPOUNDSHAPETOGRAPHICS_H
+
+#include <memory>
+#include <vector>
+#include <string>
+
+#include "SurgSim/Framework/Behavior.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+class Representation;
+}
+
+namespace Framework
+{
+class Component;
+class Representation;
+}
+
+namespace Math
+{
+class CompoundShape;
+class Shape;
+}
+
+namespace Blocks
+{
+
+SURGSIM_STATIC_REGISTRATION(CompoundShapeToGraphics);
+
+/// Keep a set of Graphics representations in sync with a CompoundShape, the shape can either be set directly or
+/// pulled from a Physics or Collision Representation. The graphics pieces will be set to coincide with the appropriate
+/// pieces of the compound shape in order.
+class CompoundShapeToGraphics : public Framework::Behavior
+{
+public:
+ /// Constructor
+ explicit CompoundShapeToGraphics(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Blocks::CompoundShapeToGraphics);
+
+ /// Destructor
+ ~CompoundShapeToGraphics();
+
+ void update(double dt) override;
+
+ int getTargetManagerType() const override;
+
+ bool doInitialize() override;
+
+ bool doWakeUp() override;
+
+ /// Sets the shape to be used for synchronization
+ /// \param shape The shape to be used
+ void setShape(const std::shared_ptr<Math::Shape>& shape);
+
+ /// Sets the source component, the components needs to provide a shape and that shape needs to be a compound shape
+ /// \param component The component to be used as a shape source
+ /// \throws SurgSim::AssertionFailure if the component does not meet the requirements
+ void setSource(const std::shared_ptr<Framework::Component>& component);
+
+ /// Sets the graphics targets to be used, each target will be update with the pose of the corresponding sub shape
+ /// in the compound shape. The components need to be graphics representations
+ /// \param components The list of graphics representations to be used as targets
+ /// \throws SurgSim::AssertionFailure if one of the components is not a graphics representation
+ void setTargets(const std::vector<std::shared_ptr<Framework::Component>> components);
+
+ /// Adds a single target to the list of targets, the target needs to be a graphics representation
+ /// \param component Graphics Representation to be added at the end of the list
+ /// \throws SurgSim::AssertionFailure if the component is not a graphics representation
+ void addTarget(const std::shared_ptr<Framework::Component>& component);
+
+ /// \return the registered graphics targets
+ std::vector<std::shared_ptr<Framework::Component>> getTargets() const;
+
+ /// \return the shape that is being used, if set, nullptr otherwise
+ std::shared_ptr<Math::CompoundShape> getShape() const;
+
+ /// \return the component that is the source, if set, nullptr otherwise
+ std::shared_ptr<Framework::Component> getSource() const;
+
+private:
+
+ /// Source shape used for updating
+ std::shared_ptr<Math::CompoundShape> m_shape;
+
+ /// Source representation if known
+ std::shared_ptr<Framework::Component> m_source;
+
+ /// List of graphics targets for updating
+ std::vector<std::shared_ptr<Graphics::Representation>> m_representations;
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Blocks/DebugDumpBehavior.cpp b/SurgSim/Blocks/DebugDumpBehavior.cpp
new file mode 100644
index 0000000..5ba5d25
--- /dev/null
+++ b/SurgSim/Blocks/DebugDumpBehavior.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/Blocks/DebugDumpBehavior.h"
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Devices/Keyboard/KeyCode.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Input/InputComponent.h"
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Blocks::DebugDumpBehavior, DebugDumpBehavior);
+
+DebugDumpBehavior::DebugDumpBehavior(const std::string& name) :
+ Framework::Behavior(name),
+ m_keyPressedLastUpdate(false)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(DebugDumpBehavior, std::shared_ptr<SurgSim::Framework::Component>,
+ InputComponent, getInputComponent, setInputComponent);
+}
+
+DebugDumpBehavior::~DebugDumpBehavior()
+{
+
+}
+
+void DebugDumpBehavior::setInputComponent(std::shared_ptr<SurgSim::Framework::Component> inputComponent)
+{
+ SURGSIM_ASSERT(nullptr != inputComponent) << "'inputComponent' cannot be 'nullptr'";
+
+ m_inputComponent = SurgSim::Framework::checkAndConvert<SurgSim::Input::InputComponent>(
+ inputComponent, "SurgSim::Input::InputComponent");
+}
+
+std::shared_ptr<SurgSim::Input::InputComponent> DebugDumpBehavior::getInputComponent() const
+{
+ return m_inputComponent;
+}
+
+void DebugDumpBehavior::update(double dt)
+{
+ SurgSim::DataStructures::DataGroup dataGroup;
+
+ if (m_inputComponent == nullptr)
+ {
+ return;
+ }
+
+ m_inputComponent->getData(&dataGroup);
+
+ using SurgSim::Devices::KeyCode;
+ int key = KeyCode::NONE;
+ if (dataGroup.integers().get("key", &key))
+ {
+
+ if (!m_keyPressedLastUpdate)
+ {
+ switch (key)
+ {
+ case KeyCode::KEY_F1:
+ {
+ auto manager = m_manager.lock();
+ if (manager != nullptr)
+ {
+ manager->dumpDebugInfo();
+ }
+ }
+ break;
+ case KeyCode::KEY_F2:
+ {
+ auto scene = getScene();
+ for (const auto& element : scene->getSceneElements())
+ {
+ std::cout << element->getName()
+ << ": "
+ << (element->isActive() ? "active" : "inactive") << std::endl;
+ for (const auto& component : element->getComponents())
+ {
+ std::cout << " "
+ << component->getName() << ": "
+ << (component->isLocalActive() ? "active" : "inactive") <<
+ std::endl;
+ }
+ }
+ }
+ break;
+ case KeyCode::KEY_F3:
+ {
+ auto scene = getScene();
+ for (const auto& element : scene->getSceneElements())
+ {
+ std::cout << element->getName() << std::endl;
+ for (const auto& component : element->getComponents<SurgSim::Graphics::Representation>())
+ {
+ std::cout << " " << component->getName()
+ << ": " << (component->isGeneratingTangents() ? "Tangents" : "NoTangents")
+ << std::endl;
+ }
+
+ }
+ }
+ break;
+ default:
+ break;
+ }
+ }
+ m_keyPressedLastUpdate = (KeyCode::NONE != key);
+ }
+}
+
+bool DebugDumpBehavior::doInitialize()
+{
+ auto managers = getRuntime()->getManagers();
+ for (const auto& manager : managers)
+ {
+ auto shared = manager.lock();
+ auto graphics = std::dynamic_pointer_cast<SurgSim::Graphics::OsgManager>(shared);
+ if (graphics != nullptr)
+ {
+ m_manager = graphics;
+ break;
+ }
+ }
+
+ SURGSIM_LOG_IF(m_manager.expired(), Framework::Logger::getDefaultLogger(), WARNING);
+
+ return true;
+}
+
+bool DebugDumpBehavior::doWakeUp()
+{
+ bool result = true;
+ if (nullptr == m_inputComponent)
+ {
+ SURGSIM_ASSERT(m_inputComponent != nullptr)
+ << __FUNCTION__ << getFullName() << " does not have an Input Component.";
+ result = false;
+ }
+ return result;
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Blocks/DebugDumpBehavior.h b/SurgSim/Blocks/DebugDumpBehavior.h
new file mode 100644
index 0000000..7de9c79
--- /dev/null
+++ b/SurgSim/Blocks/DebugDumpBehavior.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_BLOCKS_DEBUGDUMPBEHAVIOR_H
+#define SURGSIM_BLOCKS_DEBUGDUMPBEHAVIOR_H
+
+#include <memory>
+#include "SurgSim/Framework/Behavior.h"
+
+namespace SurgSim
+{
+namespace Input
+{
+
+class InputComponent;
+}
+namespace Graphics
+{
+class OsgManager;
+}
+
+namespace Blocks
+{
+
+SURGSIM_STATIC_REGISTRATION(DebugDumpBehavior)
+
+/// Provides keyboard access to debugging functionality
+/// F1 - call the graphics manager dumpDebugInfo, this will write the current scenegraph to a file in the working
+/// directory.
+/// F2 - Display the whole scene and activity status for sceneelements and components
+/// F3 - Display tangent generation status for graphics objects
+class DebugDumpBehavior : public Framework::Behavior
+{
+
+public:
+
+ /// Constructor
+ /// \param name
+ explicit DebugDumpBehavior(const std::string& name);
+
+ virtual ~DebugDumpBehavior();
+
+ SURGSIM_CLASSNAME(SurgSim::Blocks::DebugDumpBehavior);
+
+ /// Sets the InputComponent, needs to provide keystrokes
+ /// \param inputComponent the input component
+ void setInputComponent(std::shared_ptr<SurgSim::Framework::Component> inputComponent);
+
+ /// \return the input component that is being used
+ std::shared_ptr<SurgSim::Input::InputComponent> getInputComponent() const;
+
+private:
+
+ void update(double dt) override;
+
+ bool doInitialize() override;
+
+ bool doWakeUp() override;
+
+ std::weak_ptr<SurgSim::Graphics::OsgManager> m_manager; ///< Reference to the graphics manager
+ std::shared_ptr<SurgSim::Input::InputComponent> m_inputComponent; ///< The input component for reading
+ bool m_keyPressedLastUpdate; ///< prevent a repeated keystroke
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Blocks/DriveElementFromInputBehavior.cpp b/SurgSim/Blocks/DriveElementFromInputBehavior.cpp
index fdef48a..e7569fd 100644
--- a/SurgSim/Blocks/DriveElementFromInputBehavior.cpp
+++ b/SurgSim/Blocks/DriveElementFromInputBehavior.cpp
@@ -23,6 +23,7 @@
#include "SurgSim/Math/RigidTransform.h"
using SurgSim::Math::RigidTransform3d;
+using SurgSim::Framework::checkAndConvert;
namespace SurgSim
{
@@ -36,15 +37,13 @@ DriveElementFromInputBehavior::DriveElementFromInputBehavior(const std::string&
m_poseName("pose")
{
SURGSIM_ADD_SERIALIZABLE_PROPERTY(DriveElementFromInputBehavior, std::shared_ptr<SurgSim::Framework::Component>,
- Source, getSource, setSource);
+ 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";
+ m_source = checkAndConvert<SurgSim::Input::InputComponent>(source, "SurgSim::Input::InputComponent");
}
std::shared_ptr<SurgSim::Framework::Component> DriveElementFromInputBehavior::getSource()
@@ -69,7 +68,7 @@ void DriveElementFromInputBehavior::update(double dt)
RigidTransform3d pose;
if (dataGroup.poses().get(m_poseName, &pose))
{
- getPoseComponent()->setPose(pose);
+ getPoseComponent()->setPose(m_source->getLocalPose() * pose);
}
}
@@ -84,14 +83,14 @@ bool DriveElementFromInputBehavior::doWakeUp()
if (m_source == nullptr)
{
SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << getClassName() << " named '" +
- getName() + "' must have a source to do anything.";
+ 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.";
+ getName() + "' must belong to a SceneElement with a PoseComponent.";
result = false;
}
diff --git a/SurgSim/Blocks/FunctionBehavior.cpp b/SurgSim/Blocks/FunctionBehavior.cpp
new file mode 100644
index 0000000..a00ab4b
--- /dev/null
+++ b/SurgSim/Blocks/FunctionBehavior.cpp
@@ -0,0 +1,76 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/FunctionBehavior.h"
+
+#include "SurgSim/Framework/Log.h"
+
+
+namespace SurgSim
+{
+namespace Blocks
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Blocks::FunctionBehavior, FunctionBehavior);
+
+FunctionBehavior::FunctionBehavior(const std::string& name) :
+ Framework::Behavior(name),
+ m_targetManager(Framework::MANAGER_TYPE_BEHAVIOR),
+ m_function(nullptr)
+{
+}
+
+void FunctionBehavior::update(double dt)
+{
+ m_function(dt);
+}
+
+void FunctionBehavior::setTargetManagerType(int type)
+{
+ SURGSIM_ASSERT(type >= 0 && type < Framework::MANAGER_TYPE_COUNT) << "Invalid manager type.";
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot change the manager type after initialization.";
+ m_targetManager = type;
+}
+
+int FunctionBehavior::getTargetManagerType() const
+{
+ return m_targetManager;
+}
+
+void FunctionBehavior::setFunction(std::function<void(double)> function)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot change the function after initialization.";
+ m_function = function;
+}
+
+bool FunctionBehavior::doInitialize()
+{
+ if (m_function == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(Framework::Logger::getDefaultLogger())
+ << "FunctionBehavior named " << getFullName() << " missing behavior function.";
+ return false;
+ }
+
+ return true;
+}
+
+bool FunctionBehavior::doWakeUp()
+{
+ return true;
+}
+
+};
+};
diff --git a/SurgSim/Blocks/FunctionBehavior.h b/SurgSim/Blocks/FunctionBehavior.h
new file mode 100644
index 0000000..27e3b86
--- /dev/null
+++ b/SurgSim/Blocks/FunctionBehavior.h
@@ -0,0 +1,107 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_FUNCTIONBEHAVIOR_H
+#define SURGSIM_BLOCKS_FUNCTIONBEHAVIOR_H
+
+#include <functional>
+#include <string>
+
+#include "SurgSim/Framework/Behavior.h"
+
+
+namespace SurgSim
+{
+namespace Blocks
+{
+
+SURGSIM_STATIC_REGISTRATION(FunctionBehavior);
+
+/// A Behavior that can run any callable target in its update function
+///
+/// This behavior has a customizable update function, that can run any callable
+/// target, ie functions, lambda expressions, bind expressions, as well as
+/// pointers to member functions.
+///
+/// Example Usage:
+/// \code{.cpp}
+///
+/// void f1(double dt) { std::cout << dt; }
+///
+/// void f2(double dt, double a) { std::cout << dt + a; }
+///
+/// int main()
+/// {
+/// // A FunctionBehavior that runs a function
+/// auto behavior1 = std::make_shared<SurgSim::Blocks:FunctionBehavior>("Behavior 1");
+/// behavior1->setTargetManagerType(SurgSim::Framework::MANAGER_TYPE_BEHAVIOR);
+/// behavior1->setFunction(f1);
+///
+/// // A FunctionBehavior that runs a std::bind function
+/// auto behavior2 = std::make_shared<SurgSim::Blocks:FunctionBehavior>("Behavior 2");
+/// behavior2->setTargetManagerType(SurgSim::Framework::MANAGER_TYPE_GRAPHICS);
+/// behavior2->setFunction(std::bind(f2, std::placeholders::_1, 2.0));
+///
+/// // A FunctionBehavior that runs a lambda function
+/// auto behavior3 = std::make_shared<SurgSim::Blocks:FunctionBehavior>("Behavior 3");
+/// behavior3->setTargetManagerType(SurgSim::Framework::MANAGER_TYPE_PHYSICS);
+/// behavior3->setFunction([](double dt) { std::cout << dt; });
+///
+/// auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("Element");
+/// element->addComponent(behavior1);
+/// element->addComponent(behavior2);
+/// element->addComponent(behavior3);
+///
+/// return 0;
+/// }
+/// \endcode
+/// \note The FunctionBehavior is incompatible with serialization
+class FunctionBehavior : public SurgSim::Framework::Behavior
+{
+public:
+ /// Constructor
+ /// \param name Name of the behavior
+ explicit FunctionBehavior(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Blocks::FunctionBehavior);
+
+ /// Set the function to run in this behavior's update
+ /// \param function The function
+ void setFunction(std::function<void(double)> function);
+
+ /// Set which manager will handle this behavior
+ /// \param type Type of manager for this behavior
+ void setTargetManagerType(int type);
+
+ int getTargetManagerType() const override;
+
+ void update(double dt) override;
+
+private:
+ bool doInitialize() override;
+
+ bool doWakeUp() override;
+
+ /// The manager type that will handle this behavior
+ int m_targetManager;
+
+ /// The function to run in update
+ std::function<void(double)> m_function;
+};
+
+};
+};
+
+#endif // SURGSIM_BLOCKS_FUNCTIONBEHAVIOR_H
diff --git a/SurgSim/Blocks/GraphicsUtilities.cpp b/SurgSim/Blocks/GraphicsUtilities.cpp
new file mode 100644
index 0000000..b2fd770
--- /dev/null
+++ b/SurgSim/Blocks/GraphicsUtilities.cpp
@@ -0,0 +1,246 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/GraphicsUtilities.h"
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/PoseComponent.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Framework/TransferPropertiesBehavior.h"
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgRenderTarget.h"
+#include "SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h"
+#include "SurgSim/Graphics/OsgTexture2d.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgTextureUniform.h"
+#include "SurgSim/Graphics/OsgProgram.h"
+#include "SurgSim/Graphics/OsgUniformFactory.h"
+#include "SurgSim/Graphics/RenderPass.h"
+
+#include "osg/PolygonMode"
+
+namespace SurgSim
+{
+namespace Blocks
+{
+
+void enable2DTexture(
+ std::shared_ptr<Graphics::OsgMaterial> material,
+ const std::string& uniform, int unit, const std::string& filename, bool repeat)
+{
+ if (!filename.empty())
+ {
+ std::string path;
+ SURGSIM_ASSERT(Framework::Runtime::getApplicationData()->tryFindFile(filename, &path))
+ << "Could not find texture file " << filename;
+ auto texture = std::make_shared<Graphics::OsgTexture2d>();
+ texture->loadImage(path);
+ auto textureUniform = std::make_shared<Graphics::OsgTextureUniform<Graphics::OsgTexture2d>>(uniform);
+
+ if (repeat)
+ {
+ auto osgTexture = texture->getOsgTexture();
+ osgTexture->setWrap(osg::Texture::WRAP_R, osg::Texture::REPEAT);
+ osgTexture->setWrap(osg::Texture::WRAP_S, osg::Texture::REPEAT);
+ osgTexture->setWrap(osg::Texture::WRAP_T, osg::Texture::REPEAT);
+ }
+
+ textureUniform->set(texture);
+ textureUniform->setMinimumTextureUnit(unit);
+ material->addUniform(textureUniform);
+ }
+ else
+ {
+ // If there is no texture provided, just set the texture unit on the material
+ material->addUniform("int", uniform);
+ material->setValue(uniform, unit);
+ }
+}
+
+std::shared_ptr<SurgSim::Graphics::OsgMaterial> createPlainMaterial(
+ const std::string& name,
+ SurgSim::Math::Vector4f diffuseColor,
+ SurgSim::Math::Vector4f specularColor,
+ float shininess)
+{
+ auto material = std::make_shared<Graphics::OsgMaterial>(name);
+
+ auto program = Graphics::loadProgram(*Framework::Runtime::getApplicationData(), "Shaders/s_mapping_material");
+ SURGSIM_ASSERT(program != nullptr) << "Could not load program" << "Shaders/s_mapping_material";
+ material->setProgram(program);
+
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", specularColor);
+
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", diffuseColor);
+
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", shininess);
+
+ return material;
+}
+
+std::shared_ptr<SurgSim::Graphics::OsgMaterial> createTexturedMaterial(
+ const std::string& name,
+ SurgSim::Math::Vector4f diffuseColor,
+ SurgSim::Math::Vector4f specularColor,
+ float shininess,
+ const std::string& diffuseMap)
+{
+ auto material = std::make_shared<Graphics::OsgMaterial>(name);
+
+ auto program = Graphics::loadProgram(*Framework::Runtime::getApplicationData(), "Shaders/ds_mapping_material");
+ SURGSIM_ASSERT(program != nullptr) << "Could not load program" << "Shaders/ds_mapping_material";
+ material->setProgram(program);
+
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", specularColor);
+
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", diffuseColor);
+
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", shininess);
+
+ enable2DTexture(material, "diffuseMap", Graphics::DIFFUSE_TEXTURE_UNIT, diffuseMap, false);
+
+ return material;
+}
+
+std::shared_ptr<Graphics::OsgMaterial> createNormalMappedMaterial(
+ const std::string& name,
+ SurgSim::Math::Vector4f diffuseColor,
+ SurgSim::Math::Vector4f specularColor,
+ float shininess,
+ const std::string& diffuseMap,
+ const std::string& normalMap)
+{
+ auto material = std::make_shared<Graphics::OsgMaterial>(name);
+
+ auto program = Graphics::loadProgram(*Framework::Runtime::getApplicationData(), "Shaders/dns_mapping_material");
+ SURGSIM_ASSERT(program != nullptr) << "Could not load program" << "Shaders/dns_mapping_material";
+
+ // Prepare vertex attributes
+ auto osgProgram = program->getOsgProgram();
+ osgProgram->addBindAttribLocation("tangent", Graphics::TANGENT_VERTEX_ATTRIBUTE_ID);
+ osgProgram->addBindAttribLocation("bitangent", Graphics::BITANGENT_VERTEX_ATTRIBUTE_ID);
+
+ material->setProgram(program);
+
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", specularColor);
+
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", diffuseColor);
+
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", shininess);
+
+ enable2DTexture(material, "diffuseMap", Graphics::DIFFUSE_TEXTURE_UNIT, diffuseMap, false);
+ enable2DTexture(material, "normalMap", Graphics::NORMAL_TEXTURE_UNIT, normalMap, false);
+
+ return material;
+}
+
+void applyMaterials(std::shared_ptr<SurgSim::Framework::Scene> scene, std::string materialFilename,
+ const Materials& materials)
+{
+ static SurgSim::Graphics::OsgUniformFactory uniformFactory;
+ YAML::Node nodes;
+ if (Framework::tryLoadNode(materialFilename, &nodes))
+ {
+ for (auto node = nodes.begin(); node != nodes.end(); ++node)
+ {
+ auto name = node->begin()->first.as<std::string>();
+ auto materialName = node->begin()->second["Material"].as<std::string>();
+ auto found = name.find("/");
+ auto elementName = name.substr(0, found);
+ auto componentName = name.substr(found + 1);
+
+ auto component = std::dynamic_pointer_cast<Graphics::OsgRepresentation>
+ (scene->getComponent(elementName, componentName));
+
+ if (component != nullptr)
+ {
+
+ auto materialIt = materials.find(materialName);
+
+ SURGSIM_ASSERT(materialIt != materials.end())
+ << "Could not find material " << materialName << " in the prebuilt materials.";
+
+ auto material = materialIt->second;
+ component->setMaterial(material);
+
+ auto propertyNodes = node->begin()->second["Properties"];
+ for (auto nodeIt = propertyNodes.begin(); nodeIt != propertyNodes.end(); ++nodeIt)
+ {
+ auto rawUniform = uniformFactory.create(
+ (*nodeIt)[0].as<std::string>(), (*nodeIt)[1].as<std::string>()
+ );
+ SURGSIM_ASSERT(rawUniform != nullptr)
+ << "Could not create uniform " << (*nodeIt)[1].as<std::string>() << " of type "
+ << (*nodeIt)[0].as<std::string>() << ".";
+ auto uniform = std::dynamic_pointer_cast<Graphics::OsgUniformBase>(rawUniform);
+ uniform->set((*nodeIt)[2]);
+ component->addUniform(uniform);
+ }
+ }
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getLogger("GraphicsUtilities"))
+ << "Could not find material definitions, visuals are going to be compromised.";
+ }
+}
+
+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;
+}
+
+std::shared_ptr<SurgSim::Graphics::RenderPass> createPass(
+ std::unordered_map<std::string, std::shared_ptr<Graphics::OsgMaterial>> materials,
+ const std::string& passName,
+ const std::string& materialName)
+{
+ auto pass = std::make_shared<SurgSim::Graphics::RenderPass>(passName);
+ auto renderTarget = std::make_shared<Graphics::OsgRenderTarget2d>(1024, 1024, 1.0, 1, false);
+ pass->setRenderTarget(renderTarget);
+ pass->setRenderOrder(SurgSim::Graphics::Camera::RENDER_ORDER_PRE_RENDER, 0);
+ SURGSIM_ASSERT(materials[materialName] != nullptr);
+ materials[materialName]->getProgram()->setGlobalScope(true);
+ pass->setMaterial(materials[materialName]);
+ return pass;
+}
+
+
+
+}
+}
+
diff --git a/SurgSim/Blocks/GraphicsUtilities.h b/SurgSim/Blocks/GraphicsUtilities.h
new file mode 100644
index 0000000..5cfa12b
--- /dev/null
+++ b/SurgSim/Blocks/GraphicsUtilities.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_BLOCKS_GRAPHICSUTILITIES_H
+#define SURGSIM_BLOCKS_GRAPHICSUTILITIES_H
+
+#include <memory>
+#include <unordered_map>
+#include <string>
+#include "SurgSim/Math/Vector.h"
+
+/// \file GraphicsUtilities.h
+/// The code in here is supposed to help create materials correctly, a lot of it is boilerplate that is repeated over
+/// for each material. It will have to be changed if the shaders being used change.
+namespace SurgSim
+{
+namespace Graphics
+{
+class OsgMaterial;
+}
+namespace Framework
+{
+class Scene;
+}
+
+namespace Blocks
+{
+
+typedef std::unordered_map<std::string, std::shared_ptr<SurgSim::Graphics::OsgMaterial>> Materials;
+
+/// Provide a consistent interface to add texture uniforms on materials, adds the actual texture with a given minimum
+/// unit, or provides a placeholder uniform for the unit
+/// \param material The material for adding the texture
+/// \param uniform The name of the uniform to use
+/// \param unit The texture unit to use
+/// \param filename The file to use for the texture,
+/// \param repeat whether to create the texture as repeating
+/// \note if the texture filename is empty a placeholder uniform will be created using the unit as a value
+/// this for using objects with textures built in without having to assign the texture to the material on creation
+void enable2DTexture(std::shared_ptr<SurgSim::Graphics::OsgMaterial> material,
+ const std::string& uniform,
+ int unit,
+ const std::string& filename = "", bool repeat = false);
+
+/// Create a basic material
+/// \param name name of the material
+/// \param diffuseColor Base diffuse color to use
+/// \param specularColor Base specular color to use
+/// \param shininess Phong shininess exponent
+std::shared_ptr<SurgSim::Graphics::OsgMaterial> createPlainMaterial(
+ const std::string& name,
+ SurgSim::Math::Vector4f diffuseColor,
+ SurgSim::Math::Vector4f specularColor,
+ float shininess);
+
+/// Create a basic textured material
+/// \param name name of the material
+/// \param diffuseColor Base diffuse color to use
+/// \param specularColor Base specular color to use
+/// \param shininess Phong shininess exponent
+/// \param diffuseMap Diffuse texture map name to use, if the texture is embedded in the object
+/// pass an empty string here, it has to occupy the correct texture unit though.
+std::shared_ptr<SurgSim::Graphics::OsgMaterial> createTexturedMaterial(
+ const std::string& name,
+ SurgSim::Math::Vector4f diffuseColor,
+ SurgSim::Math::Vector4f specularColor,
+ float shininess,
+ const std::string& diffuseMap = "");
+
+/// Create a basic textured material
+/// \param name name of the material
+/// \param diffuseColor Base diffuse color to use
+/// \param specularColor Base specular color to use
+/// \param shininess Phong shininess exponent
+/// \param diffuseMap Diffuse texture map name to use, if the texture is embedded in the object
+/// pass an empty string here, it has to occupy the correct texture unit as defined by \sa DIFFUSE_TEXTURE_UNIT.
+/// \param normalMap Normal texture map to use, if the texture is embedded in the object pass an empty string here,
+/// it has to occupy the correct texture unit as defined by \sa NORMAL_TEXTURE_UNIT.
+std::shared_ptr<SurgSim::Graphics::OsgMaterial> createNormalMappedMaterial(
+ const std::string& name,
+ SurgSim::Math::Vector4f diffuseColor,
+ SurgSim::Math::Vector4f specularColor,
+ float shininess,
+ const std::string& diffuseMap = "",
+ const std::string& normalMap = "");
+
+/// Reads a material file, iterates over the components listed up in the material file and applies the materials and
+/// the appropriate material properties (if present) to the component, if the component is not found it will be ignored
+/// The material file is a yaml file with the following format
+///
+/// - SceneElementName/ComponentName>
+/// Material: MaterialName
+/// Properties:
+/// - [GLSLUniformType, UniformName, YamlEncodedValue]
+/// - [GLSLUniformType, UniformName, YamlEncodedValue]
+/// - SceneElementName/ComponentName
+/// Material: ...
+///
+/// The name of the SceneElement and the Component addressed need to be be separated by a '/' character.
+/// For each of the properties a uniform is created with the given GLSL type, name, and the YAML node will be passed
+/// to the uniform setter for conversion. If the type does not match what the appropriate GLSL type is, there will
+/// be an error.
+/// GLSLUniformType is e.g. vec3, vec4, float, mat4 ...
+/// \param scene The scene to traverse for component lookup
+/// \param materialFilename the YAML file that contains the descriptions
+/// \param materials lookup table for all the materials that are available
+void applyMaterials(std::shared_ptr<SurgSim::Framework::Scene> scene,
+ std::string materialFilename,
+ const Materials& materials);
+
+
+}
+}
+#endif
diff --git a/SurgSim/Blocks/ImplicitSurface.cpp b/SurgSim/Blocks/ImplicitSurface.cpp
new file mode 100644
index 0000000..580487a
--- /dev/null
+++ b/SurgSim/Blocks/ImplicitSurface.cpp
@@ -0,0 +1,358 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ImplicitSurface.h"
+
+#include "SurgSim/Blocks/GraphicsUtilities.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/TransferPropertiesBehavior.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgLight.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgProgram.h"
+#include "SurgSim/Graphics/OsgRenderTarget.h"
+#include "SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h"
+#include "SurgSim/Graphics/OsgTexture2d.h"
+#include "SurgSim/Graphics/OsgTextureUniform.h"
+#include "SurgSim/Graphics/RenderPass.h"
+#include "SurgSim/Graphics/View.h"
+
+namespace SurgSim
+{
+namespace Blocks
+{
+std::shared_ptr<Graphics::Camera> createBlurPass(
+ std::shared_ptr<Graphics::RenderPass> depthPass,
+ int textureWidth,
+ int textureHeight,
+ double blurRadius,
+ std::vector<std::shared_ptr<Framework::SceneElement>>* elements,
+ bool debug)
+{
+ float floatRadius = static_cast<float>(blurRadius);
+
+ std::shared_ptr<Graphics::Camera> previousCamera = depthPass->getCamera();
+
+ // Horizontal Pass
+ {
+ auto renderPass = std::make_shared<Graphics::RenderPass>("ImplicitSurfaceHorizontalBlurPass");
+ renderPass->getCamera()->setOrthogonalProjection(0, textureWidth, 0, textureHeight, -1.0, 1.0);
+
+ auto renderTarget = std::make_shared<Graphics::OsgRenderTarget2d>(textureWidth, textureHeight, 1.0, 0, true);
+ renderPass->setRenderTarget(renderTarget);
+ renderPass->getCamera()->setRenderOrder(Graphics::Camera::RENDER_ORDER_PRE_RENDER, 1);
+
+ auto material = Graphics::buildMaterial("Shaders/gauss_blur_horizontal.vert", "Shaders/bilateral_blur.frag");
+ material->addUniform("sampler2D", "texture");
+ material->setValue("texture", previousCamera->getRenderTarget()->getDepthTarget());
+ material->addUniform("float", "width");
+ material->setValue("width", static_cast<float>(textureWidth));
+ material->addUniform("float", "blurRadius");
+ material->setValue("blurRadius", floatRadius);
+ renderPass->setMaterial(material);
+
+ // Quad
+ auto graphics = std::make_shared<Graphics::OsgScreenSpaceQuadRepresentation>("Quad");
+ graphics->setSize(textureWidth, textureHeight);
+ graphics->setLocation(0, 0);
+ graphics->setGroupReference("ImplicitSurfaceHorizontalBlurPass");
+ renderPass->addComponent(graphics);
+
+ if (debug)
+ {
+ renderPass->showDepthTarget(0, 256, 256, 256);
+ }
+ previousCamera = renderPass->getCamera();
+ elements->push_back(renderPass);
+ }
+
+ // Vertical Pass
+ {
+ auto renderPass = std::make_shared<Graphics::RenderPass>("ImplicitSurfaceVerticalBlurPass");
+ renderPass->getCamera()->setOrthogonalProjection(0, textureWidth, 0, textureHeight, -1.0, 1.0);
+
+ auto renderTarget = std::make_shared<Graphics::OsgRenderTarget2d>(textureWidth, textureHeight, 1.0, 0, true);
+ renderPass->setRenderTarget(renderTarget);
+ renderPass->getCamera()->setRenderOrder(Graphics::Camera::RENDER_ORDER_PRE_RENDER, 2);
+
+ auto material = Graphics::buildMaterial("Shaders/gauss_blur_vertical.vert", "Shaders/bilateral_blur.frag");
+ material->addUniform("sampler2D", "texture");
+ material->setValue("texture", previousCamera->getRenderTarget()->getDepthTarget());
+ material->addUniform("float", "height");
+ material->setValue("height", static_cast<float>(textureWidth));
+ material->addUniform("float", "blurRadius");
+ material->setValue("blurRadius", floatRadius);
+ renderPass->setMaterial(material);
+
+ // Quad
+ auto graphics = std::make_shared<Graphics::OsgScreenSpaceQuadRepresentation>("Quad");
+ graphics->setSize(textureWidth, textureHeight);
+ graphics->setLocation(0, 0);
+ graphics->setGroupReference("ImplicitSurfaceVerticalBlurPass");
+ renderPass->addComponent(graphics);
+
+ if (debug)
+ {
+ renderPass->showDepthTarget(256, 256, 256, 256);
+ }
+ previousCamera = renderPass->getCamera();
+ elements->push_back(renderPass);
+ }
+
+ return previousCamera;
+}
+
+std::shared_ptr<Graphics::RenderPass> createDepthPass(
+ float sphereRadius,
+ float sphereScale,
+ int textureWidth,
+ int textureHeight,
+ bool debug)
+{
+ auto renderPass = std::make_shared<Graphics::RenderPass>("ImplicitSurfaceDepthPass");
+ renderPass->getCamera()->setRenderGroupReference(GROUP_IMPLICIT_SURFACE);
+
+ auto renderTarget = std::make_shared<Graphics::OsgRenderTarget2d>(textureWidth, textureHeight, 1.0, 0, true);
+ renderPass->setRenderTarget(renderTarget);
+ renderPass->getCamera()->setRenderOrder(Graphics::Camera::RENDER_ORDER_PRE_RENDER, 0);
+
+ auto material = Graphics::buildMaterial("Shaders/implicit_surface/depth.vert",
+ "Shaders/implicit_surface/depth.frag");
+
+ auto texture = std::make_shared<Graphics::OsgTexture2d>();
+ texture->setIsPointSprite(true);
+ auto pointSpriteUniform = std::make_shared<Graphics::OsgTextureUniform<Graphics::OsgTexture2d>>("PointSpriteDepth");
+ pointSpriteUniform->set(texture);
+ material->addUniform(pointSpriteUniform);
+
+ material->addUniform("float", "sphereRadius");
+ material->setValue("sphereRadius", sphereRadius);
+ material->addUniform("float", "sphereScale");
+ material->setValue("sphereScale", sphereScale);
+ renderPass->setMaterial(material);
+
+ if (debug)
+ {
+ renderPass->showDepthTarget(0, 0, 256, 256);
+ }
+
+ return renderPass;
+}
+
+std::shared_ptr<Graphics::RenderPass> createNormalPass(
+ std::shared_ptr<Graphics::Camera> camera,
+ std::shared_ptr<Graphics::Texture> depthMap,
+ int textureWidth,
+ int textureHeight,
+ bool debug)
+{
+ auto renderPass = std::make_shared<Graphics::RenderPass>("ImplicitSurfaceNormalPass");
+ renderPass->getCamera()->setOrthogonalProjection(0, textureWidth, 0, textureHeight, -1.0, 1.0);
+
+ auto renderTarget = std::make_shared<Graphics::OsgRenderTarget2d>(textureWidth, textureHeight, 1.0, 1, false);
+ renderPass->setRenderTarget(renderTarget);
+ renderPass->getCamera()->setRenderOrder(Graphics::Camera::RENDER_ORDER_PRE_RENDER, 3);
+
+ auto material = Graphics::buildMaterial("Shaders/implicit_surface/normal.vert",
+ "Shaders/implicit_surface/normal.frag");
+
+ material->addUniform("sampler2D", "depthMap");
+ material->setValue("depthMap", depthMap);
+ material->getUniform("depthMap")->setValue("MinimumTextureUnit", static_cast<size_t>(8));
+ material->addUniform("float", "texelSize");
+ material->setValue("texelSize", static_cast<float>(1.0 / textureWidth));
+
+ renderPass->setMaterial(material);
+
+ // Quad
+ auto graphics = std::make_shared<Graphics::OsgScreenSpaceQuadRepresentation>("Quad");
+ graphics->setSize(textureWidth, textureHeight);
+ graphics->setLocation(0, 0);
+ graphics->setGroupReference("ImplicitSurfaceNormalPass");
+ renderPass->addComponent(graphics);
+
+ if (debug)
+ {
+ renderPass->showColorTarget(256, 0, 256, 256);
+ }
+
+ return renderPass;
+}
+
+std::shared_ptr<Graphics::RenderPass> createShadingPass(
+ std::shared_ptr<Graphics::View> view,
+ std::shared_ptr<Graphics::Camera> camera,
+ std::shared_ptr<Graphics::Light> light,
+ std::shared_ptr<Graphics::RenderPass> lightMapPass,
+ std::shared_ptr<Graphics::Texture> depthMap,
+ std::shared_ptr<Graphics::Texture> normalMap,
+ const Math::Vector4f& diffuseColor,
+ const Math::Vector4f& specularColor,
+ const std::string diffuseEnvMap,
+ float diffusePct,
+ const std::string specularEnvMap,
+ float specularPct,
+ float shininess,
+ float shadowBias,
+ float shadowIntensity)
+{
+ std::array<int, 2> dimensions = view->getDimensions();
+ auto copier = std::make_shared<Framework::TransferPropertiesBehavior>("Copier");
+ copier->setTargetManagerType(SurgSim::Framework::MANAGER_TYPE_GRAPHICS);
+
+ auto renderPass = std::make_shared<Graphics::RenderPass>("ImplicitSurfaceShadingPass");
+ renderPass->addComponent(copier);
+
+ auto renderCamera = std::dynamic_pointer_cast<Graphics::OsgCamera>(renderPass->getCamera());
+ renderCamera->setAmbientColor(camera->getAmbientColor());
+ renderCamera->getOsgCamera()->setProjectionMatrixAsOrtho2D(0, dimensions[0], 0, dimensions[1]);
+ renderCamera->getOsgCamera()->setReferenceFrame(osg::Transform::ABSOLUTE_RF);
+ renderCamera->getOsgCamera()->setClearMask(GL_NONE);
+ renderCamera->setRenderOrder(Graphics::Camera::RENDER_ORDER_POST_RENDER, 0);
+
+ auto material = Graphics::buildMaterial("Shaders/implicit_surface/shading.vert",
+ "Shaders/implicit_surface/shading.frag");
+ if (lightMapPass != nullptr)
+ {
+ material->addUniform("sampler2D", "lightMap");
+ material->setValue("lightMap", lightMapPass->getRenderTarget()->getDepthTarget());
+ material->addUniform("mat4", "lightViewMatrix");
+ copier->connect(lightMapPass->getCamera(), "FloatViewMatrix",
+ material, "lightViewMatrix");
+
+ material->addUniform("mat4", "lightProjectionMatrix");
+ copier->connect(lightMapPass->getCamera(), "FloatProjectionMatrix",
+ material, "lightProjectionMatrix");
+ }
+ material->addUniform("sampler2D", "depthMap");
+ material->setValue("depthMap", depthMap);
+ material->addUniform("sampler2D", "normalMap");
+ material->setValue("normalMap", normalMap);
+ material->addUniform("vec3", "light");
+ material->setValue("light", light->getPose().translation().cast<float>().eval());
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", diffuseColor);
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", specularColor);
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", shininess);
+ material->addUniform("float", "bias");
+ material->setValue("bias", shadowBias);
+ material->addUniform("float", "intensity");
+ material->setValue("intensity", shadowIntensity);
+
+ std::string filename;
+ {
+ // The Diffuse environment map
+ SURGSIM_ASSERT(Framework::Runtime::getApplicationData()->tryFindFile(diffuseEnvMap, &filename))
+ << "Could not load diffuse environment map " << diffuseEnvMap;
+ auto texture = std::make_shared<SurgSim::Graphics::OsgTextureCubeMap>();
+ texture->loadImage(filename);
+ material->addUniform("samplerCube", "diffuseEnvMap");
+ material->setValue("diffuseEnvMap", texture);
+ material->addUniform("float", "diffusePercent");
+ material->setValue("diffusePercent", diffusePct);
+ }
+
+ {
+ // The Specular environment map
+ SURGSIM_ASSERT(Framework::Runtime::getApplicationData()->tryFindFile(specularEnvMap, &filename))
+ << "Could not load specular environment map " << specularEnvMap;
+ auto texture = std::make_shared<SurgSim::Graphics::OsgTextureCubeMap>();
+ texture->loadImage(filename);
+ material->addUniform("samplerCube", "specularEnvMap");
+ material->setValue("specularEnvMap", texture);
+ material->addUniform("float", "specularPercent");
+ material->setValue("specularPercent", specularPct);
+ }
+
+ renderPass->setMaterial(material);
+
+ auto graphics = std::make_shared<Graphics::OsgScreenSpaceQuadRepresentation>("Graphics");
+ graphics->setSize(dimensions[0], dimensions[1]);
+ graphics->setLocation(0, 0);
+ graphics->setGroupReference("ImplicitSurfaceShadingPass");
+ renderPass->addComponent(graphics);
+
+ return renderPass;
+}
+
+std::vector<std::shared_ptr<Framework::SceneElement>> createImplicitSurfaceEffect(
+ std::shared_ptr<Framework::Component> view,
+ std::shared_ptr<Framework::Component> light,
+ std::shared_ptr<Framework::SceneElement> lightMapPass,
+ float sphereRadius,
+ float sphereScale,
+ float blurRadius,
+ const Math::Vector4f& diffuseColor,
+ const Math::Vector4f& specularColor,
+ const std::string diffuseEnvMap,
+ float diffusePct,
+ const std::string specularEnvMap,
+ float specularPct,
+ float shininess,
+ float shadowBias,
+ float shadowIntensity,
+ bool showDebug)
+{
+ SURGSIM_ASSERT(view != nullptr) << "View can't be nullptr.";
+ SURGSIM_ASSERT(light != nullptr) << "Light can't be nullptr.";
+ auto graphicsView = Framework::checkAndConvert<Graphics::View>(view, "SurgSim::Graphics::View");
+ auto osgCamera = Framework::checkAndConvert<Graphics::OsgCamera>(graphicsView->getCamera(),
+ "SurgSim::Graphics::OsgCamera");
+ auto osgLight = Framework::checkAndConvert<Graphics::OsgLight>(light, "SurgSim::Graphics::OsgLight");
+
+ auto lightPass = std::dynamic_pointer_cast<Graphics::RenderPass>(lightMapPass);
+
+ std::vector<std::shared_ptr<Framework::SceneElement>> result;
+
+ auto dimensions = graphicsView->getDimensions();
+
+ switch (graphicsView->getStereoMode())
+ {
+ case Graphics::View::STEREO_MODE_VERTICAL_SPLIT:
+ dimensions[0] /= 2;
+ break;
+ case Graphics::View::STEREO_MODE_HORIZONTAL_SPLIT:
+ dimensions[1] /= 2;
+ break;
+ default:
+ break;
+ }
+
+ auto depthPass = createDepthPass(sphereRadius, sphereScale, dimensions[0], dimensions[1], showDebug);
+
+ auto blurPass = createBlurPass(depthPass, dimensions[0], dimensions[1], blurRadius, &result, showDebug);
+
+ auto normalPass = createNormalPass(osgCamera, blurPass->getRenderTarget()->getDepthTarget(),
+ dimensions[0], dimensions[1], showDebug);
+
+ auto shadingPass = createShadingPass(graphicsView, osgCamera, osgLight, lightPass,
+ blurPass->getRenderTarget()->getDepthTarget(),
+ normalPass->getRenderTarget()->getColorTarget(0),
+ diffuseColor, specularColor,
+ diffuseEnvMap, diffusePct, specularEnvMap, specularPct, shininess,
+ shadowBias, shadowIntensity);
+
+
+ result.push_back(depthPass);
+ result.push_back(normalPass);
+ result.push_back(shadingPass);
+
+ return result;
+}
+} // Blocks
+} // SurgSim
diff --git a/SurgSim/Blocks/ImplicitSurface.h b/SurgSim/Blocks/ImplicitSurface.h
new file mode 100644
index 0000000..6b5cbb9
--- /dev/null
+++ b/SurgSim/Blocks/ImplicitSurface.h
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_IMPLICITSURFACE_H
+#define SURGSIM_BLOCKS_IMPLICITSURFACE_H
+
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Graphics/Uniform.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Blocks
+{
+static const std::string GROUP_IMPLICIT_SURFACE = "ImplicitSurface";
+
+/// Builds a series of SceneElements enabling the rendering of a screen-space surface, all graphics objects that
+/// should be rendered as a surface need to be in the render group GROUP_IMPLICIT_SURFACE.
+/// The rest is done by the graphics system.
+/// All of the elements added are \sa RenderPass elements
+/// \param view the view to pull screen space dimensions from
+/// \param light the scene light to be used to shade the surface
+/// \param lightMapPass scene element containing the render pass for shadow casters
+/// \param sphereRadius the radius in meters that each point sprite sphere should have
+/// \param sphereScale the scaling factor for the point sprite sphere based on distance from the camera
+/// \param blurRadius sampling radius for the blur pass
+/// \param diffuseColor the color to use for the final surface shading
+/// \param specularColor the color to use for the specular highlight on the surface
+/// \param diffuseEnvMap the cube map to use for the diffuse environmental mapping
+/// \param diffusePct the amount to blend the environment map with the rest of diffuse lighting
+/// \param specularEnvMap the cube map to use for the specular environmental mapping
+/// \param specularPct the amount to blend the environment map with the rest of specular lighting
+/// \param shininess the shininess factor for the specular highlight
+/// \param shadowBias the bias to be used when determining something is in shadow or not, increasing this can help
+/// remove sharp edges at glancing angles
+/// \param shadowIntensity the general intensity of the shadow, 1 means everything in shadow will be black, 0 means
+/// there won't be any shadows at all
+/// \param showDebug whether to show debug information
+std::vector<std::shared_ptr<Framework::SceneElement>> createImplicitSurfaceEffect(
+ std::shared_ptr<Framework::Component> view,
+ std::shared_ptr<Framework::Component> light,
+ std::shared_ptr<Framework::SceneElement> lightMapPass,
+ float sphereRadius,
+ float sphereScale,
+ float blurRadius,
+ const Math::Vector4f& diffuseColor,
+ const Math::Vector4f& specularColor,
+ const std::string diffuseEnvMap,
+ float diffusePct,
+ const std::string specularEnvMap,
+ float specularPct,
+ float shininess,
+ float shadowBias,
+ float shadowIntensity,
+ bool showDebug);
+
+} // Blocks
+} // SurgSim
+
+#endif // SURGSIM_BLOCKS_IMPLICITSURFACE_H
diff --git a/SurgSim/Blocks/KeyBehavior.cpp b/SurgSim/Blocks/KeyBehavior.cpp
new file mode 100644
index 0000000..df918bc
--- /dev/null
+++ b/SurgSim/Blocks/KeyBehavior.cpp
@@ -0,0 +1,141 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/KeyBehavior.h"
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Devices/Keyboard/KeyCode.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Input/InputComponent.h"
+
+namespace SurgSim
+{
+namespace Blocks
+{
+
+KeyBehavior::KeyBehavior(const std::string& name) :
+ Framework::Behavior(name),
+ m_lastKey(SurgSim::Devices::KeyCode::NONE)
+
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(KeyBehavior, std::shared_ptr<SurgSim::Framework::Component>,
+ InputComponent, getInputComponent, setInputComponent);
+}
+
+KeyBehavior::~KeyBehavior()
+{
+}
+
+void KeyBehavior::setInputComponent(std::shared_ptr<SurgSim::Framework::Component> inputComponent)
+{
+ m_inputComponent = SurgSim::Framework::checkAndConvert<SurgSim::Input::InputComponent>(inputComponent,
+ "SurgSim::Input::InputComponent");
+}
+
+std::shared_ptr<SurgSim::Input::InputComponent> KeyBehavior::getInputComponent() const
+{
+ return m_inputComponent;
+}
+
+bool KeyBehavior::doInitialize()
+{
+ return true;
+}
+
+bool KeyBehavior::doWakeUp()
+{
+ SURGSIM_LOG_IF(m_inputComponent == nullptr,
+ SurgSim::Framework::Logger::getDefaultLogger(),
+ WARNING) << "Input component not present in " << getFullName();
+
+ return m_inputComponent != nullptr;
+}
+
+void KeyBehavior::update(double dt)
+{
+ DataStructures::DataGroup dataGroup;
+ m_inputComponent->getData(&dataGroup);
+
+ int key;
+ if (dataGroup.integers().get("key", &key))
+ {
+ if (key != m_lastKey)
+ {
+ if (m_lastKey != Devices::KeyCode::NONE)
+ {
+ onKeyUp(m_lastKey);
+ }
+
+ if (key != Devices::KeyCode::NONE)
+ {
+ onKeyDown(key);
+ }
+ }
+ m_lastKey = key;
+ }
+}
+
+std::unordered_map<int, std::string> KeyBehavior::m_keyMap;
+boost::mutex KeyBehavior::m_keyMapMutex;
+
+bool KeyBehavior::registerKey(int keycode, const std::string& description)
+{
+ if (keycode == Devices::KeyCode::NONE)
+ {
+ return false;
+ }
+ bool result = false;
+ boost::unique_lock<boost::mutex> lock(m_keyMapMutex);
+ if (m_keyMap.find(keycode) == m_keyMap.end())
+ {
+ m_keyMap[keycode] = description;
+ result = true;
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getLogger("Blocks/KeyBehavior"))
+ << "Key '" << static_cast<char>(keycode) << "' [" << description << "] already registered for function "
+ << m_keyMap[keycode];
+ }
+ return result;
+}
+
+bool KeyBehavior::unregisterKey(int keycode)
+{
+ bool result = false;
+ boost::unique_lock<boost::mutex> lock(m_keyMapMutex);
+ if (m_keyMap.find(keycode) != m_keyMap.end())
+ {
+ m_keyMap.erase(keycode);
+ result = true;
+ }
+ return result;
+}
+
+void KeyBehavior::logMap()
+{
+ std::string message;
+ auto logger = SurgSim::Framework::Logger::getLogger("Blocks/KeyBehavior");
+ boost::unique_lock<boost::mutex> lock(m_keyMapMutex);
+ for (const auto& item : m_keyMap)
+ {
+ SURGSIM_LOG_INFO(logger) << "'" << static_cast<char>(item.first) << "' : " << item.second;
+ }
+}
+
+}; // namespace Blocks
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Blocks/KeyBehavior.h b/SurgSim/Blocks/KeyBehavior.h
new file mode 100644
index 0000000..0e6454a
--- /dev/null
+++ b/SurgSim/Blocks/KeyBehavior.h
@@ -0,0 +1,96 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_KEYBEHAVIOR_H
+#define SURGSIM_BLOCKS_KEYBEHAVIOR_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Framework/Behavior.h"
+
+namespace SurgSim
+{
+namespace Input
+{
+class InputComponent;
+}
+
+namespace Blocks
+{
+
+/// Behavior to abstract the functionality of keyboard driven behaviors, can be programmed to react to a
+/// single keystroke
+class KeyBehavior : public Framework::Behavior
+{
+public:
+ /// Constructor
+ explicit KeyBehavior(const std::string& name);
+
+ /// Destructor
+ ~KeyBehavior();
+
+ void update(double dt) override;
+ bool doInitialize() override;
+ bool doWakeUp() override;
+
+ /// Set the input component from which pressed key comes.
+ /// \param inputComponent The input component which contains the pressed key(s).
+ void setInputComponent(std::shared_ptr<Framework::Component> inputComponent);
+
+ /// Get the input component of this behavior, from which the pressed key comes.
+ /// \return The input component which sends key press to this behavior.
+ std::shared_ptr<Input::InputComponent> getInputComponent() const;
+
+ /// Register the key, this let's the system keep track of keys used in the application
+ /// \param keycode the key that is being used
+ /// \param description description of functionality that the key triggers
+ /// \return true if the key is available, false if another key has already been register
+ static bool registerKey(int keycode, const std::string& description);
+
+ /// Remove a key from the registry
+ /// \param keycode the key to be removed
+ /// \return true if the key was actually removed
+ static bool unregisterKey(int keycode);
+
+ /// Write the keymap out to the logger
+ static void logMap();
+
+protected:
+ /// Implement to execute functionality on key press
+ /// \param key the value of the key hit
+ virtual void onKeyDown(int key) = 0;
+
+ /// Implement to execute functionality on key release
+ /// \param key the value of the key hit
+ virtual void onKeyUp(int key) = 0;
+
+ /// Input component needs to provide key
+ std::shared_ptr<Input::InputComponent> m_inputComponent;
+
+ /// Keep track if the key was pressed the last time around
+ int m_lastKey;
+
+ ///@{
+ /// Handle the key map
+ static boost::mutex m_keyMapMutex;
+ static std::unordered_map<int, std::string> m_keyMap;
+ ///@}
+};
+
+}; // namespace Blocks
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Blocks/KeyboardCallbackBehavior.cpp b/SurgSim/Blocks/KeyboardCallbackBehavior.cpp
new file mode 100644
index 0000000..a34652b
--- /dev/null
+++ b/SurgSim/Blocks/KeyboardCallbackBehavior.cpp
@@ -0,0 +1,101 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/KeyboardCallbackBehavior.h"
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Input/InputComponent.h"
+
+namespace SurgSim
+{
+namespace Blocks
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Blocks::KeyboardCallbackBehavior, KeyboardCallbackBehavior);
+
+KeyboardCallbackBehavior::KeyboardCallbackBehavior(const std::string& name) :
+ Framework::Behavior(name),
+ m_keyPressedLastUpdate(false),
+ m_actionKey(Devices::NONE)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(KeyboardCallbackBehavior, std::shared_ptr<SurgSim::Framework::Component>,
+ InputComponent, getInputComponent, setInputComponent);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(KeyboardCallbackBehavior, int, ActionKey, getKey, registerKey);
+}
+
+void KeyboardCallbackBehavior::setInputComponent(std::shared_ptr<Framework::Component> inputComponent)
+{
+ m_inputComponent = Framework::checkAndConvert<Input::InputComponent>(inputComponent,
+ "SurgSim::Input::InputComponent");
+}
+
+std::shared_ptr<Input::InputComponent> KeyboardCallbackBehavior::getInputComponent() const
+{
+ return m_inputComponent;
+}
+
+void KeyboardCallbackBehavior::registerKey(int key)
+{
+ m_actionKey = key;
+}
+
+void KeyboardCallbackBehavior::update(double dt)
+{
+ DataStructures::DataGroup dataGroup;
+ m_inputComponent->getData(&dataGroup);
+
+ int key;
+ if (dataGroup.integers().get("key", &key))
+ {
+ if (m_actionKey == key && !m_keyPressedLastUpdate)
+ {
+ m_callback();
+ }
+
+ m_keyPressedLastUpdate = (Devices::KeyCode::NONE != key);
+ }
+}
+
+bool KeyboardCallbackBehavior::doInitialize()
+{
+ return true;
+}
+
+bool KeyboardCallbackBehavior::doWakeUp()
+{
+ bool result = true;
+ if (nullptr == m_inputComponent)
+ {
+ SURGSIM_LOG_SEVERE(Framework::Logger::getDefaultLogger()) <<
+ "KeyboardCallbackBehavior '" << getFullName() << "' does not have an Input Component.";
+ result = false;
+ }
+ return result;
+}
+
+int KeyboardCallbackBehavior::getKey() const
+{
+ return m_actionKey;
+}
+
+void KeyboardCallbackBehavior::registerCallback(CallbackType func)
+{
+ m_callback = func;
+}
+
+}; // namespace Blocks
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Blocks/KeyboardCallbackBehavior.h b/SurgSim/Blocks/KeyboardCallbackBehavior.h
new file mode 100644
index 0000000..7e200f7
--- /dev/null
+++ b/SurgSim/Blocks/KeyboardCallbackBehavior.h
@@ -0,0 +1,92 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_KEYBOARDCALLBACKBEHAVIOR_H
+#define SURGSIM_BLOCKS_KEYBOARDCALLBACKBEHAVIOR_H
+
+#include <functional>
+
+#include "SurgSim/Devices/Keyboard/KeyCode.h"
+#include "SurgSim/Framework/Behavior.h"
+
+namespace SurgSim
+{
+namespace Input
+{
+class InputComponent;
+}
+
+namespace Blocks
+{
+SURGSIM_STATIC_REGISTRATION(KeyboardCallbackBehavior);
+
+/// This behavior will call the registered callback function when the registered key is pressed.
+class KeyboardCallbackBehavior : public Framework::Behavior
+{
+public:
+ typedef std::function<void()> CallbackType;
+
+ /// Constructor
+ /// \param name Name of the behavior
+ explicit KeyboardCallbackBehavior(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Blocks::KeyboardCallbackBehavior);
+
+ /// Set the input component from which pressed key comes.
+ /// \param inputComponent The input component which contains the pressed key(s).
+ void setInputComponent(std::shared_ptr<Framework::Component> inputComponent);
+
+ /// Get the input component of this behavior, from which the pressed key comes.
+ /// \return The input component which sends key press to this behavior.
+ std::shared_ptr<Input::InputComponent> getInputComponent() const;
+
+ /// Register a key, so that when such key is pressed, this behavior will call the callback.
+ /// \param key The controlling key.
+ void registerKey(int key);
+
+ /// Register a callback function.
+ /// This function will be called when the registered key is pressed.
+ /// \param func The callback function.
+ void registerCallback(CallbackType func);
+
+ void update(double dt) override;
+
+protected:
+ /// \return The key which will make this behavior to call the callback.
+ /// \note This is used for serialization only.
+ int getKey() const;
+
+ bool doInitialize() override;
+
+ bool doWakeUp() override;
+
+private:
+ /// Record if any key is pressed in last update() call.
+ bool m_keyPressedLastUpdate;
+
+ /// The registered key, when pressed, the registered callback will be called.
+ int m_actionKey;
+
+ /// Callback function.
+ CallbackType m_callback;
+
+ /// Input component from which pressed key comes.
+ std::shared_ptr<Input::InputComponent> m_inputComponent;
+};
+
+}; // namespace Blocks
+}; // namespace SurgSim
+
+#endif //SURGSIM_BLOCKS_KEYBOARDCALLBACKBEHAVIOR_H
diff --git a/SurgSim/Blocks/KeyboardTogglesComponentBehavior.cpp b/SurgSim/Blocks/KeyboardTogglesComponentBehavior.cpp
index 7954c53..da53cc6 100644
--- a/SurgSim/Blocks/KeyboardTogglesComponentBehavior.cpp
+++ b/SurgSim/Blocks/KeyboardTogglesComponentBehavior.cpp
@@ -20,6 +20,8 @@
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Input/InputComponent.h"
+using SurgSim::Framework::checkAndConvert;
+
namespace SurgSim
{
namespace Blocks
@@ -41,9 +43,8 @@ void KeyboardTogglesComponentBehavior::setInputComponent(std::shared_ptr<SurgSim
{
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";
+ m_inputComponent = checkAndConvert<SurgSim::Input::InputComponent>(
+ inputComponent, "SurgSim::Input::InputComponent");
}
std::shared_ptr<SurgSim::Input::InputComponent> KeyboardTogglesComponentBehavior::getInputComponent() const
@@ -51,8 +52,8 @@ std::shared_ptr<SurgSim::Input::InputComponent> KeyboardTogglesComponentBehavior
return m_inputComponent;
}
-void KeyboardTogglesComponentBehavior::registerKey(SurgSim::Device::KeyCode key,
- std::shared_ptr<SurgSim::Framework::Component> component)
+void KeyboardTogglesComponentBehavior::registerKey(SurgSim::Devices::KeyCode key,
+ std::shared_ptr<SurgSim::Framework::Component> component)
{
m_registry[static_cast<int>(key)].insert(component);
}
@@ -73,7 +74,7 @@ void KeyboardTogglesComponentBehavior::update(double dt)
(*it)->setLocalActive(!(*it)->isLocalActive());
};
}
- m_keyPressedLastUpdate = (SurgSim::Device::KeyCode::NONE != key);
+ m_keyPressedLastUpdate = (SurgSim::Devices::KeyCode::NONE != key);
}
}
@@ -88,7 +89,7 @@ bool KeyboardTogglesComponentBehavior::doWakeUp()
if (nullptr == m_inputComponent)
{
SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << __FUNCTION__ <<
- "KeyboardTogglesComponentBehavior " << getName() << " does not have an Input Component.";
+ "KeyboardTogglesComponentBehavior " << getName() << " does not have an Input Component.";
result = false;
}
return result;
@@ -100,7 +101,7 @@ void KeyboardTogglesComponentBehavior::setKeyboardRegistry(const KeyboardRegistr
}
const KeyboardTogglesComponentBehavior::KeyboardRegistryType&
- KeyboardTogglesComponentBehavior::getKeyboardRegistry() const
+KeyboardTogglesComponentBehavior::getKeyboardRegistry() const
{
return m_registry;
}
diff --git a/SurgSim/Blocks/KeyboardTogglesComponentBehavior.h b/SurgSim/Blocks/KeyboardTogglesComponentBehavior.h
index 6017e35..f009719 100644
--- a/SurgSim/Blocks/KeyboardTogglesComponentBehavior.h
+++ b/SurgSim/Blocks/KeyboardTogglesComponentBehavior.h
@@ -40,7 +40,7 @@ namespace Blocks
{
SURGSIM_STATIC_REGISTRATION(KeyboardTogglesComponentBehavior);
-/// This behavior is used to control the visibility of registered graphical representation(s)
+/// This behavior is used to control the activity of registered components.
class KeyboardTogglesComponentBehavior : public SurgSim::Framework::Behavior
{
public:
@@ -65,7 +65,7 @@ public:
/// \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);
+ void registerKey(SurgSim::Devices::KeyCode key, std::shared_ptr<SurgSim::Framework::Component> component);
/// Set the register map of this behavior
/// \param map The register map.
@@ -77,18 +77,18 @@ public:
/// Update the behavior
/// \param dt The length of time (seconds) between update calls.
- virtual void update(double dt) override;
+ 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;
+ 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;
+ bool doWakeUp() override;
private:
/// Record if any key is pressed in last update() call.
diff --git a/SurgSim/Blocks/MassSpring1DRepresentation.cpp b/SurgSim/Blocks/MassSpring1DRepresentation.cpp
index d9bb88d..d09a267 100644
--- a/SurgSim/Blocks/MassSpring1DRepresentation.cpp
+++ b/SurgSim/Blocks/MassSpring1DRepresentation.cpp
@@ -70,8 +70,8 @@ void MassSpring1DRepresentation::init1D(
// Sets the boundary conditions
for (auto boundaryCondition = std::begin(nodeBoundaryConditions);
- boundaryCondition != std::end(nodeBoundaryConditions);
- boundaryCondition++)
+ boundaryCondition != std::end(nodeBoundaryConditions);
+ boundaryCondition++)
{
state->addBoundaryCondition(*boundaryCondition);
}
@@ -80,63 +80,6 @@ void MassSpring1DRepresentation::init1D(
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
index a61d34c..e5bb166 100644
--- a/SurgSim/Blocks/MassSpring1DRepresentation.h
+++ b/SurgSim/Blocks/MassSpring1DRepresentation.h
@@ -52,9 +52,6 @@ public:
double stiffnessStretching, double dampingStretching,
double stiffnessBending, double dampingBending);
-protected:
-
- virtual bool doWakeUp() override;
};
}; // namespace Blocks
diff --git a/SurgSim/Blocks/PoseInterpolator.cpp b/SurgSim/Blocks/PoseInterpolator.cpp
index 952e38c..975493d 100644
--- a/SurgSim/Blocks/PoseInterpolator.cpp
+++ b/SurgSim/Blocks/PoseInterpolator.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -77,6 +77,11 @@ bool PoseInterpolator::doInitialize()
return true;
}
+void PoseInterpolator::doRetire()
+{
+ m_target = nullptr;
+}
+
bool PoseInterpolator::doWakeUp()
{
bool result = false;
@@ -110,11 +115,15 @@ void PoseInterpolator::update(double dt)
else
{
m_currentTime = m_duration;
- getSceneElement()->removeComponent(getName());
+ auto element = getSceneElement();
+ if (element != nullptr)
+ {
+ element->removeComponent(getName());
+ }
}
}
- m_target->setPose(SurgSim::Math::interpolate(m_startingPose, m_endingPose, m_currentTime/m_duration));
+ m_target->setPose(Math::interpolate(m_startingPose, m_endingPose, m_currentTime / m_duration));
}
void PoseInterpolator::setLoop(bool val)
diff --git a/SurgSim/Blocks/PoseInterpolator.h b/SurgSim/Blocks/PoseInterpolator.h
index 8ee4bc1..487c6bd 100644
--- a/SurgSim/Blocks/PoseInterpolator.h
+++ b/SurgSim/Blocks/PoseInterpolator.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -18,15 +18,17 @@
#include <memory>
#include <string>
+
+#include "SurgSim/DataStructures/OptionalValue.h"
#include "SurgSim/Framework/Behavior.h"
#include "SurgSim/Math/RigidTransform.h"
-#include "SurgSim/DataStructures/OptionalValue.h"
+
namespace SurgSim
{
namespace Framework
{
- class SceneElement;
+class SceneElement;
}
}
@@ -82,8 +84,7 @@ public:
/// \return true If the interpolation is doing ping pong.
bool isPingPong() const;
- /// Overridden from Behavior
- virtual void update(double dt) override;
+ void update(double dt) override;
private:
@@ -111,11 +112,11 @@ private:
/// Whether to loop
bool m_loop;
- /// Overridden from Component
- virtual bool doWakeUp() override;
+ bool doWakeUp() override;
+
+ bool doInitialize() override;
- /// Overridden from Component
- virtual bool doInitialize() override;
+ void doRetire() override;
};
diff --git a/SurgSim/Blocks/ShadowMapping.cpp b/SurgSim/Blocks/ShadowMapping.cpp
new file mode 100644
index 0000000..a8f5398
--- /dev/null
+++ b/SurgSim/Blocks/ShadowMapping.cpp
@@ -0,0 +1,280 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/ShadowMapping.h"
+
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgRenderTarget.h"
+#include "SurgSim/Graphics/Program.h"
+#include "SurgSim/Graphics/RenderPass.h"
+#include "SurgSim/Graphics/OsgScreenSpacePass.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/TransferPropertiesBehavior.h"
+#include "SurgSim/Framework/PoseComponent.h"
+#include "SurgSim/Blocks/GraphicsUtilities.h"
+#include "SurgSim/Graphics/OsgLight.h"
+
+#include <osg/PolygonMode>
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+std::shared_ptr<Graphics::Camera> setupBlurPasses(
+ std::shared_ptr<Graphics::RenderPass> shadowMapPass,
+ int textureSize,
+ double blurRadius,
+ bool debug,
+ std::vector<std::shared_ptr<Framework::SceneElement>>* elements)
+{
+ std::vector<std::shared_ptr<Framework::SceneElement>> result;
+ float shadowMapSize = static_cast<float>(textureSize);
+ float floatRadius = static_cast<float>(blurRadius);
+
+ std::shared_ptr<Graphics::Camera> perviousCamera = shadowMapPass->getCamera();
+
+ // Horizontal Pass
+ {
+ auto pass = std::make_shared<Graphics::RenderPass>("HorizontalBlurPass");
+ pass->getCamera()->setOrthogonalProjection(0, textureSize, 0, textureSize, -1.0, 1.0);
+
+ auto renderTarget = std::make_shared<Graphics::OsgRenderTarget2d>(textureSize, textureSize, 1.0, 1, false);
+ pass->setRenderTarget(renderTarget);
+ pass->setRenderOrder(Graphics::Camera::RENDER_ORDER_PRE_RENDER, 2);
+
+ // Material
+ auto material = Graphics::buildMaterial("Shaders/gauss_blur_horizontal.vert",
+ "Shaders/gauss_blur.frag");
+ material->getProgram()->setGlobalScope(true);
+ material->addUniform("float", "width");
+ material->setValue("width", shadowMapSize);
+ material->addUniform("float", "blurRadius");
+ material->setValue("blurRadius", floatRadius);
+ pass->setMaterial(material);
+
+ // Quad
+ auto graphics = std::make_shared<Graphics::OsgScreenSpaceQuadRepresentation>("Quad");
+ graphics->setSize(textureSize, textureSize);
+ graphics->setLocation(0, 0);
+ graphics->setTexture(perviousCamera->getRenderTarget()->getColorTarget(0));
+ graphics->setGroupReference("HorizontalBlurPass");
+ pass->addComponent(graphics);
+
+ if (debug)
+ {
+ pass->showColorTarget(512, 0, 256, 256);
+ }
+ perviousCamera = pass->getCamera();
+ elements->push_back(pass);
+ }
+
+ // Vertical Pass
+ {
+ auto pass = std::make_shared<Graphics::RenderPass>("VerticalBlurPass");
+ pass->getCamera()->setOrthogonalProjection(0, textureSize, 0, textureSize, -1.0, 1.0);
+
+ auto renderTarget = std::make_shared<Graphics::OsgRenderTarget2d>(textureSize, textureSize, 1.0, 1, false);
+ pass->setRenderTarget(renderTarget);
+ pass->setRenderOrder(Graphics::Camera::RENDER_ORDER_PRE_RENDER, 3);
+
+ // Material
+ auto material = Graphics::buildMaterial("Shaders/gauss_blur_vertical.vert",
+ "Shaders/gauss_blur.frag");
+ material->getProgram()->setGlobalScope(true);
+ material->addUniform("float", "height");
+ material->setValue("height", shadowMapSize);
+ material->addUniform("float", "blurRadius");
+ material->setValue("blurRadius", floatRadius);
+ pass->setMaterial(material);
+
+ // Quad
+ auto graphics = std::make_shared<Graphics::OsgScreenSpaceQuadRepresentation>("Quad");
+ graphics->setSize(textureSize, textureSize);
+ graphics->setLocation(0, 0);
+ graphics->setTexture(perviousCamera->getRenderTarget()->getColorTarget(0));
+ graphics->setGroupReference("VerticalBlurPass");
+ pass->addComponent(graphics);
+
+ if (debug)
+ {
+ pass->showColorTarget(756, 0, 256, 256);
+ }
+ perviousCamera = pass->getCamera();
+ elements->push_back(pass);
+ }
+
+ return perviousCamera;
+}
+
+/// Create the pass that renders the scene from the view of the light source
+/// the identifier GROUP_SHADOW_CASTER is used in all graphic objects to mark them as used
+/// in this pass
+std::shared_ptr<Graphics::RenderPass> createLightMapPass(int textureSize, bool debug)
+{
+ auto pass = std::make_shared<Graphics::RenderPass>(GROUP_SHADOW_CASTER);
+ auto renderTarget = std::make_shared<Graphics::OsgRenderTarget2d>(textureSize, textureSize, 1.0, 0, true);
+ pass->setRenderTarget(renderTarget);
+ pass->setRenderOrder(Graphics::Camera::RENDER_ORDER_PRE_RENDER, 0);
+ std::dynamic_pointer_cast<Graphics::OsgCamera>(pass->getCamera())->getOsgCamera()->setReferenceFrame(
+ osg::Transform::ABSOLUTE_RF);
+
+ auto material = Graphics::buildMaterial("Shaders/depth_map.vert", "Shaders/depth_map.frag");
+ material->getProgram()->setGlobalScope(true);
+ pass->setMaterial(material);
+
+ if (debug)
+ {
+ pass->showDepthTarget(0, 0, 256, 256);
+ }
+
+ return pass;
+}
+
+/// Create the pass that renders shadowed pixels into the scene,
+/// the identifier GROUPD_SHADOW_RECEIVER is used in all graphics objects to mark them
+/// as used in this pass
+std::shared_ptr<Graphics::RenderPass> createShadowMapPass(int textureSize, double bias, double intensity, bool debug)
+{
+ auto pass = std::make_shared<Graphics::RenderPass>(GROUP_SHADOW_RECEIVER);
+ auto renderTarget = std::make_shared<Graphics::OsgRenderTarget2d>(textureSize, textureSize, 1.0, 1, false);
+ pass->setRenderTarget(renderTarget);
+ pass->setRenderOrder(Graphics::Camera::RENDER_ORDER_PRE_RENDER, 1);
+ std::dynamic_pointer_cast<Graphics::OsgCamera>(pass->getCamera())->getOsgCamera()->setReferenceFrame(
+ osg::Transform::ABSOLUTE_RF);
+
+
+ auto material = Graphics::buildMaterial("Shaders/shadow_map.vert", "Shaders/shadow_map.frag");
+ material->getProgram()->setGlobalScope(true);
+ pass->setMaterial(material);
+
+ material->addUniform("float", "bias");
+ material->setValue("bias", static_cast<float>(bias));
+
+ material->addUniform("float", "intensity");
+ material->setValue("intensity", static_cast<float>(intensity));
+
+ if (debug)
+ {
+ pass->showColorTarget(256, 0, 256, 256);
+ }
+
+ return pass;
+}
+
+
+std::vector<std::shared_ptr<Framework::SceneElement>> createShadowMapping(
+ std::shared_ptr<Framework::Component> camera,
+ std::shared_ptr<Framework::Component> light,
+ int depthTextureSize,
+ int shadowTextureSize,
+ std::array<double, 6> lightCameraProjection,
+ double bias,
+ double intensity,
+ bool useBlur,
+ double blurRadius,
+ bool showDebug)
+{
+ SURGSIM_ASSERT(camera != nullptr) << "Camera can't be nullptr.";
+ SURGSIM_ASSERT(light != nullptr) << "Light can't be nullptr.";
+
+ std::vector<std::shared_ptr<Framework::SceneElement>> result;
+
+ auto osgCamera = Framework::checkAndConvert<Graphics::OsgCamera>(camera, "SurgSim::Graphics::OsgCamera");
+ auto osgLight = Framework::checkAndConvert<Graphics::OsgLight>(light, "SurgSim::Graphics::OsgLight");
+
+ auto lightMapPass = createLightMapPass(depthTextureSize, showDebug);
+ result.push_back(lightMapPass);
+
+ lightMapPass->getCamera()->setValue("OrthogonalProjection", lightCameraProjection);
+
+ auto cameraNode = std::dynamic_pointer_cast<Graphics::OsgCamera>(lightMapPass->getCamera())->getOsgCamera();
+ cameraNode->getOrCreateStateSet()->setAttributeAndModes(
+ new osg::PolygonMode(osg::PolygonMode::BACK, osg::PolygonMode::FILL), osg::StateAttribute::ON);
+
+ auto copier = std::make_shared<Framework::TransferPropertiesBehavior>("Copier");
+ copier->setTargetManagerType(Framework::MANAGER_TYPE_GRAPHICS);
+
+ // connect the light pose and the light map camera pose, so when the light moves,
+ // this camera will move as well
+ // Stapling has the light and the camera on the same element, the view, take the view element and connect the pose
+ // to the light map pass
+ copier->connect(osgLight, "Pose", lightMapPass->getPoseComponent(), "Pose");
+
+ auto shadowMapPass = createShadowMapPass(shadowTextureSize, bias, intensity, showDebug);
+ result.push_back(shadowMapPass);
+
+ shadowMapPass->getMaterial()->addUniform("mat4", "lightViewMatrix");
+ copier->connect(lightMapPass->getCamera(), "FloatViewMatrix",
+ shadowMapPass->getMaterial(), "lightViewMatrix");
+
+ auto shadowMaterial = shadowMapPass->getMaterial();
+ // The projection matrix of the camera used to render the light map
+ shadowMaterial->addUniform("mat4", "lightProjectionMatrix");
+ copier->connect(lightMapPass->getCamera(), "FloatProjectionMatrix",
+ shadowMaterial, "lightProjectionMatrix");
+
+ // 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
+
+ shadowMaterial->addUniform("sampler2D", "depthMap");
+ shadowMaterial->setValue("depthMap", lightMapPass->getRenderTarget()->getDepthTarget());
+ shadowMaterial->getUniform("depthMap")->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
+ auto shadowCamera = shadowMapPass->getCamera();
+ copier->connect(osgCamera, "ProjectionMatrix", shadowCamera , "ProjectionMatrix");
+ copier->connect(osgCamera, "Pose", shadowCamera, "LocalPose");
+
+ // Put the result of the last pass into the main camera to make it accessible
+ auto material = std::make_shared<Graphics::OsgMaterial>("camera material");
+ material->addUniform("sampler2D", "shadowMap");
+
+ std::shared_ptr<Graphics::Texture> texture;
+
+ if (useBlur)
+ {
+ auto blurrPass = setupBlurPasses(
+ shadowMapPass,
+ shadowTextureSize,
+ static_cast<float>(blurRadius),
+ showDebug,
+ &result);
+ texture = blurrPass->getRenderTarget()->getColorTarget(0);
+ }
+ else
+ {
+ texture = shadowCamera->getRenderTarget()->getColorTarget(0);
+ }
+
+ material->setValue("shadowMap", texture);
+ material->getUniform("shadowMap")->setValue("MinimumTextureUnit", static_cast<size_t>(8));
+
+ osgCamera->getSceneElement()->addComponent(material);
+ osgCamera->setMaterial(material);
+
+ lightMapPass->addComponent(copier);
+
+ return result;
+}
+
+}
+}
diff --git a/SurgSim/Blocks/ShadowMapping.h b/SurgSim/Blocks/ShadowMapping.h
new file mode 100644
index 0000000..f66c511
--- /dev/null
+++ b/SurgSim/Blocks/ShadowMapping.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_BLOCKS_SHADOWMAPPING_H
+#define SURGSIM_BLOCKS_SHADOWMAPPING_H
+
+#include <string>
+#include <memory>
+#include <array>
+#include <vector>
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+class OsgMaterial;
+}
+namespace Framework
+{
+class SceneElement;
+class Component;
+}
+
+
+
+namespace Blocks
+{
+
+///@{
+/// Names to use as RenderGroupReferences
+static const std::string GROUP_SHADOW_CASTER = "Shadowing";
+static const std::string GROUP_SHADOW_RECEIVER = "Shadowed";
+///@}
+
+/// Builds a series of SceneElements enabling the rendering of shadows, all graphics object that should cast shadows
+/// need to be in the render group GROUP_SHADOW_CASTER, all objects that should receive shadows should be in the
+/// render group GROUP_SHADOW_RECEIVER. The rest is done by the graphics system.
+/// All of the elements added are \sa RenderPass elements
+/// \param camera the view camera that is used for this pass
+/// \param light the light that should be used for the shadows
+/// \param depthTextureSize the size of the texture for the depth map, main determinant for the quality of the shadows
+/// \param shadowTextureSize the size of shadowTexture pass and the blur textures
+/// \param lightCameraProjection parameters for an orthogonal projection that will be used to render the scene from
+/// the lights point of view, needs to be set so it encompasses all the shadow casters and receivers
+/// \param bias the bias to be used when determining something is in shadow or not, increasing this can help remove
+/// sharp edges at glancing angles
+/// \param intensity the general intensity of the shadow, 1 means everything in shadow will be black, 0 means there
+/// won't be any shadows at all
+/// \param useBlur whether to blur the output of the light map pass, this will remove some of the blockiness of the
+/// shadows
+/// \param blurRadius sampling radius for the blur pass
+/// \param showDebug whether to show debug information
+std::vector<std::shared_ptr<Framework::SceneElement>> createShadowMapping(
+ std::shared_ptr<Framework::Component> camera,
+ std::shared_ptr<Framework::Component> light,
+ int depthTextureSize,
+ int shadowTextureSize,
+ std::array<double, 6> lightCameraProjection,
+ double bias,
+ double intensity,
+ bool useBlur,
+ double blurRadius,
+ bool showDebug);
+
+}
+}
+
+#endif
diff --git a/SurgSim/Blocks/SingleKeyBehavior.cpp b/SurgSim/Blocks/SingleKeyBehavior.cpp
new file mode 100644
index 0000000..36fcc46
--- /dev/null
+++ b/SurgSim/Blocks/SingleKeyBehavior.cpp
@@ -0,0 +1,88 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "SingleKeyBehavior.h"
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Devices/Keyboard/KeyCode.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Input/InputComponent.h"
+
+namespace SurgSim
+{
+namespace Blocks
+{
+
+SingleKeyBehavior::SingleKeyBehavior(const std::string& name) :
+ KeyBehavior(name),
+ m_actionKey(SurgSim::Devices::KeyCode::NONE)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SingleKeyBehavior, int, ActionKey, getKey, setKey);
+
+}
+
+SingleKeyBehavior::~SingleKeyBehavior()
+{
+}
+
+bool SingleKeyBehavior::doWakeUp()
+{
+ SURGSIM_LOG_IF(m_actionKey == Devices::KeyCode::NONE,
+ SurgSim::Framework::Logger::getDefaultLogger(),
+ WARNING) << "No key set in " << getFullName();
+
+ return KeyBehavior::doWakeUp() && m_actionKey != Devices::KeyCode::NONE;
+}
+
+
+int SingleKeyBehavior::getKey() const
+{
+ return m_actionKey;
+}
+
+void SingleKeyBehavior::setDescription(const std::string& description)
+{
+ m_description = description;
+}
+
+void SingleKeyBehavior::onKeyDown(int actualKey)
+{
+ if (actualKey == m_actionKey)
+ {
+ onKey();
+ }
+}
+
+void SingleKeyBehavior::onKeyUp(int val)
+{
+ return;
+}
+
+void SingleKeyBehavior::setKey(int val)
+{
+ KeyBehavior::unregisterKey(m_actionKey);
+ m_actionKey = val;
+ std::string description = m_description;
+ if (description == "")
+ {
+ description = getClassName();
+ }
+ KeyBehavior::registerKey(val, description);
+}
+
+}; // namespace Blocks
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Blocks/SingleKeyBehavior.h b/SurgSim/Blocks/SingleKeyBehavior.h
new file mode 100644
index 0000000..6ce487c
--- /dev/null
+++ b/SurgSim/Blocks/SingleKeyBehavior.h
@@ -0,0 +1,74 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_SINGLEKEYBEHAVIOR_H
+#define SURGSIM_BLOCKS_SINGLEKEYBEHAVIOR_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Blocks/KeyBehavior.h"
+
+namespace SurgSim
+{
+namespace Input
+{
+class InputComponent;
+}
+
+namespace Blocks
+{
+
+/// Behavior to abstract the functionality of keyboard driven behaviors, can be programmed to react to a
+/// single keystroke
+class SingleKeyBehavior : public KeyBehavior
+{
+public:
+ /// Constructor
+ explicit SingleKeyBehavior(const std::string& name);
+
+ /// Destructor
+ ~SingleKeyBehavior();
+
+ bool doWakeUp() override;
+
+ /// Sets the current key value used to trigger this behavior
+ /// \param val the key code to use to trigger this behavior
+ void setKey(int val);
+
+ /// \return the key code that triggers this behavior
+ int getKey() const;
+
+protected:
+ void setDescription(const std::string& description);
+
+ virtual void onKey() = 0;
+
+ void onKeyDown(int actualKey) override;
+
+ void onKeyUp(int actualKey) override;
+
+ std::shared_ptr<Input::InputComponent> m_inputComponent;
+
+ /// Registered key to trigger action
+ int m_actionKey;
+
+ std::string m_description;
+};
+
+}; // namespace Blocks
+}; // namespace SurgSim
+
+#endif // SURGSIM_BLOCKS_SINGLEKEYBEHAVIOR_H
diff --git a/SurgSim/Blocks/SphereElement.cpp b/SurgSim/Blocks/SphereElement.cpp
index f4d1b70..587bce8 100644
--- a/SurgSim/Blocks/SphereElement.cpp
+++ b/SurgSim/Blocks/SphereElement.cpp
@@ -13,39 +13,37 @@
// 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/OsgProgram.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::OsgProgram;
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)
+
+namespace SurgSim
{
-}
+namespace Blocks
+{
-SphereElement::~SphereElement()
+SphereElement::SphereElement(const std::string& name) :
+ SurgSim::Framework::SceneElement(name)
{
}
bool SphereElement::doInitialize()
{
std::shared_ptr<RigidRepresentation> physicsRepresentation =
- std::make_shared<RigidRepresentation>(m_name + " Physics");
+ std::make_shared<RigidRepresentation>(getName() + " Physics");
physicsRepresentation->setDensity(700.0); // Wood
physicsRepresentation->setLinearDamping(0.1);
@@ -54,13 +52,13 @@ bool SphereElement::doInitialize()
physicsRepresentation->setShape(shape);
std::shared_ptr<OsgSphereRepresentation> graphicsRepresentation =
- std::make_shared<OsgSphereRepresentation>(m_name + " Graphics");
+ std::make_shared<OsgSphereRepresentation>(getName() + " Graphics");
graphicsRepresentation->setRadius(shape->getRadius());
std::shared_ptr<OsgMaterial> material = std::make_shared<OsgMaterial>("material");
- std::shared_ptr<OsgShader> shader = std::make_shared<OsgShader>();
+ std::shared_ptr<OsgProgram> program = std::make_shared<OsgProgram>();
- shader->setVertexShaderSource(
+ program->setVertexShaderSource(
"varying vec4 color;\n"
"void main(void)\n"
"{\n"
@@ -68,13 +66,13 @@ bool SphereElement::doInitialize()
" color.rgb = gl_Normal;\n"
" color.a = 1.0;\n"
"}");
- shader->setFragmentShaderSource(
+ program->setFragmentShaderSource(
"varying vec4 color;\n"
"void main(void)\n"
"{\n"
" gl_FragColor = color;\n"
"}");
- material->setShader(shader);
+ material->setProgram(program);
graphicsRepresentation->setMaterial(material);
addComponent(physicsRepresentation);
@@ -87,7 +85,5 @@ bool SphereElement::doInitialize()
return true;
}
-bool SphereElement::doWakeUp()
-{
- return true;
-}
+};
+};
diff --git a/SurgSim/Blocks/SphereElement.h b/SurgSim/Blocks/SphereElement.h
index ce812cd..3088606 100644
--- a/SurgSim/Blocks/SphereElement.h
+++ b/SurgSim/Blocks/SphereElement.h
@@ -16,8 +16,9 @@
#ifndef SURGSIM_BLOCKS_SPHEREELEMENT_H
#define SURGSIM_BLOCKS_SPHEREELEMENT_H
-#include "SurgSim/Framework/SceneElement.h"
+#include <string>
+#include "SurgSim/Framework/SceneElement.h"
#include "SurgSim/Math/RigidTransform.h"
@@ -30,21 +31,12 @@ namespace Blocks
class SphereElement : public SurgSim::Framework::SceneElement
{
public:
-
+ /// Constructor
+ /// \param name Name of the sphere
explicit SphereElement(const std::string& name);
- ~SphereElement();
-
-
protected:
- virtual bool doInitialize();
-
- virtual bool doWakeUp();
-
-
-private:
- std::string m_name;
-
+ bool doInitialize() override;
};
diff --git a/SurgSim/Blocks/TransferParticlesToPointCloudBehavior.cpp b/SurgSim/Blocks/TransferParticlesToPointCloudBehavior.cpp
new file mode 100644
index 0000000..dfad53c
--- /dev/null
+++ b/SurgSim/Blocks/TransferParticlesToPointCloudBehavior.cpp
@@ -0,0 +1,100 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/TransferParticlesToPointCloudBehavior.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/Particles/Representation.h"
+
+using SurgSim::Framework::checkAndConvert;
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Blocks::TransferParticlesToPointCloudBehavior,
+ TransferParticlesToPointCloudBehavior);
+
+TransferParticlesToPointCloudBehavior::TransferParticlesToPointCloudBehavior(const std::string& name) :
+ SurgSim::Framework::Behavior(name)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(TransferParticlesToPointCloudBehavior,
+ std::shared_ptr<SurgSim::Framework::Component>, Source, getSource, setSource);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(TransferParticlesToPointCloudBehavior,
+ std::shared_ptr<SurgSim::Framework::Component>, Target, getTarget, setTarget);
+}
+
+void TransferParticlesToPointCloudBehavior::setSource(const std::shared_ptr<SurgSim::Framework::Component>& source)
+{
+ SURGSIM_ASSERT(nullptr != source) << "'source' can not be nullptr.";
+ m_source = checkAndConvert<SurgSim::Particles::Representation>(
+ source, "SurgSim::Particles::Representation");
+}
+
+void TransferParticlesToPointCloudBehavior::setTarget(const std::shared_ptr<SurgSim::Framework::Component>& target)
+{
+ SURGSIM_ASSERT(nullptr != target) << "'target' can not be nullptr.";
+ m_target = checkAndConvert<SurgSim::Graphics::PointCloudRepresentation>(
+ target, "SurgSim::Graphics::PointCloudRepresentation");
+}
+
+std::shared_ptr<SurgSim::Particles::Representation> TransferParticlesToPointCloudBehavior::getSource() const
+{
+ return m_source;
+}
+
+std::shared_ptr<SurgSim::Graphics::PointCloudRepresentation>
+ TransferParticlesToPointCloudBehavior::getTarget() const
+{
+ return m_target;
+}
+
+void TransferParticlesToPointCloudBehavior::update(double dt)
+{
+ *m_target->getVertices() = *m_source->getParticles().safeGet();
+}
+
+bool TransferParticlesToPointCloudBehavior::doInitialize()
+{
+ return true;
+}
+
+bool TransferParticlesToPointCloudBehavior::doWakeUp()
+{
+ if (m_source == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << getClassName() << " named '" +
+ getName() + "' must have a source.";
+ return false;
+ }
+ if (m_target == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << getClassName() << " named '" +
+ getName() + "' must have a target.";
+ return false;
+ }
+
+ return true;
+}
+
+}; //namespace Blocks
+}; //namespace SurgSim
diff --git a/SurgSim/Blocks/TransferParticlesToPointCloudBehavior.h b/SurgSim/Blocks/TransferParticlesToPointCloudBehavior.h
new file mode 100644
index 0000000..f49375f
--- /dev/null
+++ b/SurgSim/Blocks/TransferParticlesToPointCloudBehavior.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_TRANSFERPARTICLESTOPOINTCLOUDBEHAVIOR_H
+#define SURGSIM_BLOCKS_TRANSFERPARTICLESTOPOINTCLOUDBEHAVIOR_H
+
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/Macros.h"
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+class Component;
+}
+
+namespace Graphics
+{
+class PointCloudRepresentation;
+}
+
+namespace Particles
+{
+class Representation;
+}
+
+namespace Blocks
+{
+SURGSIM_STATIC_REGISTRATION(TransferParticlesToPointCloudBehavior);
+
+/// Behavior to copy positions of a Particles::Representation to a PointCloud.
+class TransferParticlesToPointCloudBehavior : public SurgSim::Framework::Behavior
+{
+public:
+ /// Constructor
+ /// \param name Name of the behavior
+ explicit TransferParticlesToPointCloudBehavior(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Blocks::TransferParticlesToPointCloudBehavior);
+
+ /// Set the representation from which the positions are from
+ /// \param source The particles 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 particles representation which sends the positions
+ /// \return The Physics representation which produces positions.
+ std::shared_ptr<SurgSim::Particles::Representation> 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;
+
+ void update(double dt) override;
+
+private:
+ bool doInitialize() override;
+ bool doWakeUp() override;
+
+ /// The Particles::Representation from which the positions come.
+ std::shared_ptr<SurgSim::Particles::Representation> m_source;
+
+ /// The Graphics PointCloud Representation to which the vertices's positions are set.
+ std::shared_ptr<SurgSim::Graphics::PointCloudRepresentation> m_target;
+};
+
+}; // namespace Blocks
+}; // namespace SurgSim
+
+#endif // SURGSIM_BLOCKS_TRANSFERPARTICLESTOPOINTCLOUDBEHAVIOR_H
diff --git a/SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.cpp b/SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.cpp
index 24ac92e..97a18d8 100644
--- a/SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.cpp
+++ b/SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.cpp
@@ -15,15 +15,21 @@
#include "SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h"
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
+#include "SurgSim/DataStructures/Grid.h"
+#include "SurgSim/DataStructures/TriangleMesh.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/Aabb.h"
#include "SurgSim/Math/OdeState.h"
#include "SurgSim/Physics/DeformableRepresentation.h"
+using SurgSim::Framework::checkAndConvert;
+
namespace SurgSim
{
@@ -33,40 +39,79 @@ SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Blocks::TransferPhysics
TransferPhysicsToGraphicsMeshBehavior);
TransferPhysicsToGraphicsMeshBehavior::TransferPhysicsToGraphicsMeshBehavior(const std::string& name) :
- SurgSim::Framework::Behavior(name)
+ Framework::Behavior(name)
{
SURGSIM_ADD_SERIALIZABLE_PROPERTY(TransferPhysicsToGraphicsMeshBehavior,
- std::shared_ptr<SurgSim::Framework::Component>, Source, getSource, setSource);
+ std::shared_ptr<Framework::Component>, Source, getSource, setSource);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(TransferPhysicsToGraphicsMeshBehavior,
- std::shared_ptr<SurgSim::Framework::Component>, Target, getTarget, setTarget);
+ std::shared_ptr<Framework::Component>, Target, getTarget, setTarget);
+
+ // Enable full serialization on the index map type, but need to deal with overloaded functions
+ {
+ typedef std::vector<std::pair<size_t, size_t>> ParamType;
+ typedef TransferPhysicsToGraphicsMeshBehavior ClassType;
+
+ // Deal with the overloaded function, by casting to explicit function type
+ auto getter = (ParamType(ClassType::*)(void) const)&ClassType::getIndexMap;
+ auto setter = (void(ClassType::*)(const ParamType&))&ClassType::setIndexMap;
+
+ setAccessors("IndexMap", std::bind(getter, this),
+ std::bind(setter, this, std::bind(Framework::convert<ParamType>, std::placeholders::_1)));
+
+ setSerializable("IndexMap",
+ std::bind(&YAML::convert<ParamType>::encode, std::bind(getter, this)),
+ std::bind(setter, this, std::bind(&YAML::Node::as<ParamType>, std::placeholders::_1)));
+ }
+
+ // Enable a setter to take the names of two mesh files to create the index map
+ {
+ typedef std::pair<std::string, std::string> ParamType;
+ typedef TransferPhysicsToGraphicsMeshBehavior ClassType;
+ auto setter = (void(ClassType::*)(const ParamType&))&ClassType::setIndexMap;
+
+ setSetter("IndexMapMeshNames",
+ std::bind(setter, this, std::bind(Framework::convert<ParamType>, std::placeholders::_1)));
+
+ setDecoder("IndexMapMeshNames",
+ std::bind(setter, this, std::bind(&YAML::Node::as<ParamType>, std::placeholders::_1)));
+ }
+
+ // Enable a setter to take two meshes to create the index map
+ {
+ typedef std::pair<std::shared_ptr<Framework::Asset>, std::shared_ptr<Framework::Asset>> ParamType;
+ typedef TransferPhysicsToGraphicsMeshBehavior ClassType;
+ auto setter = (void(ClassType::*)(const ParamType&))&ClassType::setIndexMap;
+
+ setSetter("IndexMapMeshes",
+ std::bind(setter, this, std::bind(Framework::convert<ParamType>, std::placeholders::_1)));
+
+ setDecoder("IndexMapMeshes",
+ std::bind(setter, this, std::bind(&YAML::Node::as<ParamType>, std::placeholders::_1)));
+ }
+
+
}
-void TransferPhysicsToGraphicsMeshBehavior::setSource(const std::shared_ptr<SurgSim::Framework::Component>& source)
+void TransferPhysicsToGraphicsMeshBehavior::setSource(const std::shared_ptr<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;
+ m_source = checkAndConvert<Physics::DeformableRepresentation>(
+ source, "Physics::DeformableRepresentation");
}
-void TransferPhysicsToGraphicsMeshBehavior::setTarget(const std::shared_ptr<SurgSim::Framework::Component>& target)
+void TransferPhysicsToGraphicsMeshBehavior::setTarget(const std::shared_ptr<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;
+ m_target = checkAndConvert<Graphics::MeshRepresentation>(
+ target, "Graphics::MeshRepresentation");
}
-std::shared_ptr<SurgSim::Physics::DeformableRepresentation> TransferPhysicsToGraphicsMeshBehavior::getSource() const
+std::shared_ptr<Physics::DeformableRepresentation> TransferPhysicsToGraphicsMeshBehavior::getSource() const
{
return m_source;
}
-std::shared_ptr<SurgSim::Graphics::MeshRepresentation> TransferPhysicsToGraphicsMeshBehavior::getTarget() const
+std::shared_ptr<Graphics::MeshRepresentation> TransferPhysicsToGraphicsMeshBehavior::getTarget() const
{
return m_target;
}
@@ -75,10 +120,24 @@ void TransferPhysicsToGraphicsMeshBehavior::update(double dt)
{
auto state = m_source->getFinalState();
- for (size_t nodeId = 0; nodeId < state->getNumNodes(); ++nodeId)
+ if (m_indexMap.empty())
+ {
+ auto mesh = m_target->getMesh();
+ for (size_t nodeId = 0; nodeId < state->getNumNodes(); ++nodeId)
+ {
+ mesh->setVertexPosition(nodeId, state->getPosition(nodeId));
+ }
+ }
+ else
{
- m_target->getMesh()->setVertexPosition(nodeId, state->getPosition(nodeId));
+ auto mesh = m_target->getMesh();
+ for (const auto& mapping : m_indexMap)
+ {
+ mesh->setVertexPosition(mapping.second, state->getPosition(mapping.first));
+ }
}
+
+ m_target->getMesh()->dirty();
}
bool TransferPhysicsToGraphicsMeshBehavior::doInitialize()
@@ -95,7 +154,7 @@ bool TransferPhysicsToGraphicsMeshBehavior::doWakeUp()
{
for (size_t nodeId = 0; nodeId < state->getNumNodes(); ++nodeId)
{
- SurgSim::Graphics::Mesh::VertexType vertex(state->getPosition(nodeId));
+ Graphics::Mesh::VertexType vertex(state->getPosition(nodeId));
target->addVertex(vertex);
}
}
@@ -103,5 +162,117 @@ bool TransferPhysicsToGraphicsMeshBehavior::doWakeUp()
return true;
}
+void TransferPhysicsToGraphicsMeshBehavior::setIndexMap(
+ const std::shared_ptr<DataStructures::TriangleMeshPlain>& source,
+ const std::shared_ptr<DataStructures::TriangleMeshPlain>& target)
+{
+ setIndexMap(generateIndexMap(source, target));
+}
+
+void TransferPhysicsToGraphicsMeshBehavior::setIndexMap(const std::string& sourceName , const std::string& targetName)
+{
+ auto source = std::make_shared<DataStructures::TriangleMeshPlain>();
+ source->load(sourceName);
+
+
+ auto target = std::make_shared<DataStructures::TriangleMeshPlain>();
+ target->load(targetName);
+
+ setIndexMap(source, target);
+}
+
+void TransferPhysicsToGraphicsMeshBehavior::setIndexMap(const std::pair<std::string, std::string>& fileName)
+{
+ setIndexMap(fileName.first, fileName.second);
+}
+
+void TransferPhysicsToGraphicsMeshBehavior::setIndexMap(
+ const std::pair<std::shared_ptr<Framework::Asset>, std::shared_ptr<Framework::Asset>>& meshes)
+{
+ auto first = std::dynamic_pointer_cast<DataStructures::TriangleMeshPlain>(meshes.first);
+ auto second = std::dynamic_pointer_cast<DataStructures::TriangleMeshPlain>(meshes.second);
+
+ SURGSIM_ASSERT(first != nullptr) << "Incoming first asset not TriangleMeshPlain";
+ SURGSIM_ASSERT(second != nullptr) << "Incoming second asset not TriangleMeshPlain";
+
+ setIndexMap(first, second);
+}
+
+void TransferPhysicsToGraphicsMeshBehavior::setIndexMap(const std::vector<std::pair<size_t, size_t>>& indexMap)
+{
+ m_indexMap = indexMap;
+}
+
+const std::vector<std::pair<size_t, size_t>> TransferPhysicsToGraphicsMeshBehavior::getIndexMap() const
+{
+ return m_indexMap;
+}
+
+std::vector<std::pair<size_t, size_t>> generateIndexMap(
+ const std::shared_ptr<DataStructures::TriangleMeshPlain>& source,
+ const std::shared_ptr<DataStructures::TriangleMeshPlain>& target)
+{
+ auto logger = Framework::Logger::getLogger("Blocks/TransferPhysicsToGraphicsMeshBehavior");
+ SURGSIM_LOG_INFO(logger) << "Building map";
+
+ SURGSIM_ASSERT(source->getNumVertices() > 0 && target->getNumVertices() > 0)
+ << "Can't build correspondence map, meshes are missing vertices";
+
+
+ // Caclulate AABB for Mesh
+ const auto& vertices = target->getVertices();
+ Math::Aabbd bounds;
+
+ for (const auto& vertex : vertices)
+ {
+ bounds.extend(vertex.position);
+ }
+
+ bounds.extend(bounds.max() + Math::Vector3d(0.1, 0.1, 0.1));
+ bounds.extend(bounds.min() - Math::Vector3d(0.1, 0.1, 0.1));
+
+ // Add All vertices to grid
+ // 100 Seems like a reasonable value ...
+ Math::Vector3d cellSize(bounds.diagonal() / 100.0);
+ DataStructures::Grid<size_t, 3> grid(cellSize, bounds);
+
+ size_t index = 0;
+ for (const auto& vertex : vertices)
+ {
+ grid.addElement(index++, vertex.position);
+ }
+
+ std::vector<std::pair<size_t, size_t>> indexMap;
+
+ // For each state node query grid for neighbors
+ indexMap.reserve(target->getNumVertices());
+ for (size_t nodeId = 0; nodeId < source->getNumVertices(); ++nodeId)
+ {
+ bool hasNeighbors = false;
+ const auto& position = source->getVertexPosition(nodeId);
+
+ // make a copy
+ const std::vector<size_t>& neighbors = grid.getNeighbors(position);
+ // For each neighbor check if same, if yes add to map
+
+ for (auto neighborIndex : neighbors)
+ {
+ if (position.isApprox(vertices[neighborIndex].position))
+ {
+ indexMap.push_back(std::make_pair(nodeId, neighborIndex));
+ hasNeighbors = true;
+ }
+ }
+
+ if (!hasNeighbors)
+ {
+ SURGSIM_LOG_WARNING(logger) << "No coincident point found for node with index " << index;
+
+ }
+ }
+
+ return indexMap;
+}
+
}; //namespace Blocks
}; //namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h b/SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h
index bf521cb..34a8e00 100644
--- a/SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h
+++ b/SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h
@@ -16,6 +16,7 @@
#ifndef SURGSIM_BLOCKS_TRANSFERPHYSICSTOGRAPHICSMESHBEHAVIOR_H
#define SURGSIM_BLOCKS_TRANSFERPHYSICSTOGRAPHICSMESHBEHAVIOR_H
+#include "SurgSim/DataStructures/TriangleMesh.h"
#include "SurgSim/Framework/Behavior.h"
#include "SurgSim/Framework/Macros.h"
@@ -25,6 +26,7 @@ namespace SurgSim
namespace Framework
{
class Component;
+class Asset;
}
namespace Graphics
@@ -42,7 +44,11 @@ namespace Blocks
SURGSIM_STATIC_REGISTRATION(TransferPhysicsToGraphicsMeshBehavior);
/// Behavior to copy positions of a PhysicsRepresentation to a GraphicsMesh.
-class TransferPhysicsToGraphicsMeshBehavior : public SurgSim::Framework::Behavior
+/// By default the behavior will take each node and copy the position of that node to the vertex with the corresponding
+/// index. If an index map is available, for each pair in the index map it will take the nodeId from the first
+/// member of the pair and copy it to the vertex with the id of the second member of the pair.
+/// The index map can be computed from meshes given to this behavior or precomputed via other means.
+class TransferPhysicsToGraphicsMeshBehavior : public Framework::Behavior
{
public:
/// Constructor
@@ -53,33 +59,72 @@ public:
/// Set the representation from which the positions are from
/// \param source The physics representation
- void setSource(const std::shared_ptr<SurgSim::Framework::Component>& source);
+ void setSource(const std::shared_ptr<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);
+ void setTarget(const std::shared_ptr<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;
+ std::shared_ptr<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;
+ std::shared_ptr<Graphics::MeshRepresentation> getTarget() const;
- virtual void update(double dt) override;
+ /// Generate a mapping, for each point in source find the points target that coincide
+ /// \param source Source of search
+ /// \param target Target of search
+ /// \note source and target need to be triangle meshes
+ void setIndexMap(
+ const std::shared_ptr<DataStructures::TriangleMeshPlain>& source,
+ const std::shared_ptr<DataStructures::TriangleMeshPlain>& target);
+
+ /// Generate a mapping, for each point in source find the points target that coincide
+ /// \param sourceFile filename for source mesh
+ /// \param targetFile filename for target mesh
+ /// \note sourceFile and targetFile need to be valid ply files
+ void setIndexMap(const std::string& sourceFile, const std::string& targetFile);
+
+ /// Set the mapping to be used if not empty. first index is the node, second index is the vertex.
+ /// for all pairs copy node position to given vertex index.
+ /// \param indexMap mapping to be used for this mesh
+ void setIndexMap(const std::vector<std::pair<size_t, size_t>>& indexMap);
+
+ /// \return the current mapping
+ const std::vector<std::pair<size_t, size_t>> getIndexMap() const;
+
+ void update(double dt) override;
private:
- virtual bool doInitialize() override;
- virtual bool doWakeUp() override;
+ bool doInitialize() override;
+ bool doWakeUp() override;
+
+
+ void setIndexMap(const std::pair<std::string, std::string>& fileName);
+
+ void setIndexMap(const std::pair<std::shared_ptr<Framework::Asset>, std::shared_ptr<Framework::Asset>>& meshes);
/// The DeformableRepresentation from which the Ode state comes.
- std::shared_ptr<SurgSim::Physics::DeformableRepresentation> m_source;
+ std::shared_ptr<Physics::DeformableRepresentation> m_source;
/// The Graphics Mesh Representation to which the vertices' positions are set.
- std::shared_ptr<SurgSim::Graphics::MeshRepresentation> m_target;
+ std::shared_ptr<Graphics::MeshRepresentation> m_target;
+
+ /// The mapping to be used if not empty.
+ std::vector<std::pair<size_t, size_t>> m_indexMap;
};
+/// Generate a mapping, for each point in source find the points target that coincide
+/// \param source Source of search
+/// \param target Target of search
+/// \note source and target need to be triangle meshes
+/// \return the map of matching points, pair.first are all points from source, pair.second will be points in target
+std::vector<std::pair<size_t, size_t>> generateIndexMap(
+ const std::shared_ptr<DataStructures::TriangleMeshPlain>& source,
+ const std::shared_ptr<DataStructures::TriangleMeshPlain>& target);
+
}; // namespace Blocks
}; // namespace SurgSim
diff --git a/SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.cpp b/SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.cpp
index cc15390..8b84001 100644
--- a/SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.cpp
+++ b/SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.cpp
@@ -25,6 +25,8 @@
#include "SurgSim/Math/OdeState.h"
#include "SurgSim/Physics/DeformableRepresentation.h"
+using SurgSim::Framework::checkAndConvert;
+
namespace SurgSim
{
@@ -37,29 +39,23 @@ TransferPhysicsToPointCloudBehavior::TransferPhysicsToPointCloudBehavior(const s
SurgSim::Framework::Behavior(name)
{
SURGSIM_ADD_SERIALIZABLE_PROPERTY(TransferPhysicsToPointCloudBehavior,
- std::shared_ptr<SurgSim::Framework::Component>, Source, getSource, setSource);
+ std::shared_ptr<SurgSim::Framework::Component>, Source, getSource, setSource);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(TransferPhysicsToPointCloudBehavior,
- std::shared_ptr<SurgSim::Framework::Component>, Target, getTarget, setTarget);
+ 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;
+ m_source = checkAndConvert<SurgSim::Physics::DeformableRepresentation>(
+ source, "SurgSim::Physics::DeformableRepresentation");
}
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;
+ m_target = checkAndConvert<SurgSim::Graphics::PointCloudRepresentation>(
+ target, "SurgSim::Graphics::PointCloudRepresentation");
}
std::shared_ptr<SurgSim::Physics::DeformableRepresentation> TransferPhysicsToPointCloudBehavior::getSource() const
diff --git a/SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h b/SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h
index b993055..6249f67 100644
--- a/SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h
+++ b/SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h
@@ -67,11 +67,11 @@ public:
/// \return The Graphics PointCloud representation which receives positions.
std::shared_ptr<SurgSim::Graphics::PointCloudRepresentation> getTarget() const;
- virtual void update(double dt) override;
+ void update(double dt) override;
private:
- virtual bool doInitialize() override;
- virtual bool doWakeUp() override;
+ bool doInitialize() override;
+ bool doWakeUp() override;
/// The DeformableRepresentation from which the Ode state comes.
std::shared_ptr<SurgSim::Physics::DeformableRepresentation> m_source;
diff --git a/SurgSim/Blocks/TransferPhysicsToVerticesBehavior.cpp b/SurgSim/Blocks/TransferPhysicsToVerticesBehavior.cpp
new file mode 100644
index 0000000..31e7d70
--- /dev/null
+++ b/SurgSim/Blocks/TransferPhysicsToVerticesBehavior.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/Blocks/TransferPhysicsToVerticesBehavior.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Physics/DeformableRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+
+TransferPhysicsToVerticesBehavior::TransferPhysicsToVerticesBehavior(const std::string& name) :
+ Framework::Behavior(name)
+{
+
+}
+
+void TransferPhysicsToVerticesBehavior::setSource(const std::shared_ptr<Framework::Component>& source)
+{
+ SURGSIM_ASSERT(nullptr != source) << "'source' can not be nullptr.";
+ m_source = Framework::checkAndConvert<Physics::DeformableRepresentation>(
+ source, "SurgSim::Physics::DeformableRepresentation");
+}
+
+void TransferPhysicsToVerticesBehavior::setTarget(const std::shared_ptr<Framework::Component>& target)
+{
+ SURGSIM_ASSERT(nullptr != target) << "'target' can not be nullptr.";
+ SURGSIM_ASSERT(target->isWriteable("Vertices")) << "'target'" << target->getFullName()
+ << "needs to accept 'Vertices'";
+ m_target = target;
+}
+
+std::shared_ptr<Physics::DeformableRepresentation> TransferPhysicsToVerticesBehavior::getSource() const
+{
+ return m_source;
+}
+
+std::shared_ptr<Framework::Component> TransferPhysicsToVerticesBehavior::getTarget() const
+{
+ return m_target;
+}
+
+void TransferPhysicsToVerticesBehavior::update(double dt)
+{
+ auto state = m_source->getFinalState();
+ for (size_t nodeId = 0; nodeId < state->getNumNodes(); ++nodeId)
+ {
+ m_vertices.setVertexPosition(nodeId, state->getPosition(nodeId));
+ }
+ m_target->setValue("Vertices", m_vertices);
+}
+
+bool TransferPhysicsToVerticesBehavior::doInitialize()
+{
+ return true;
+}
+
+bool TransferPhysicsToVerticesBehavior::doWakeUp()
+{
+
+ if (m_vertices.getNumVertices() == 0)
+ {
+ auto state = m_source->getFinalState();
+ for (size_t nodeId = 0; nodeId < state->getNumNodes(); ++nodeId)
+ {
+ DataStructures::VerticesPlain::VertexType vertex(state->getPosition(nodeId));
+ m_vertices.addVertex(std::move(vertex));
+ }
+ }
+ return true;
+}
+
+}
+}
diff --git a/SurgSim/Blocks/TransferPhysicsToVerticesBehavior.h b/SurgSim/Blocks/TransferPhysicsToVerticesBehavior.h
new file mode 100644
index 0000000..59f664d
--- /dev/null
+++ b/SurgSim/Blocks/TransferPhysicsToVerticesBehavior.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_BLOCKS_TRANSFERPHYSICSTOVERTICESBEHAVIOR_H
+#define SURGSIM_BLOCKS_TRANSFERPHYSICSTOVERTICESBEHAVIOR_H
+
+#include "SurgSim/DataStructures/EmptyData.h"
+
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Framework/Behavior.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+class DeformableRepresentation;
+}
+namespace Graphics
+{
+class CurveRepresentation;
+}
+
+namespace Blocks
+{
+
+/// Transfer Physics State to any representation that has a Vertices property, the "Vertices" property on the targets
+/// side needs to accept DataStructures::VerticesPlain for the type.
+class TransferPhysicsToVerticesBehavior : public Framework::Behavior
+{
+public:
+
+ /// Constructor
+ /// \param name Name of the behavior
+ explicit TransferPhysicsToVerticesBehavior(const std::string& name);
+
+ /// \return the source representation of this behavior
+ std::shared_ptr<Physics::DeformableRepresentation> getSource() const;
+
+ /// Sets the source for this behavior, all the positions from the state of the source
+ /// will be copied into a Vertices object and set in the target, using setValue("Vertices")
+ /// \param source the component that is the source
+ void setSource(const std::shared_ptr<Framework::Component>& source);
+
+ /// \return the target representation of this behavior
+ std::shared_ptr<Framework::Component> getTarget() const;
+
+ /// Sets the target for this behavior, it needs to have a "Vertices" property that takes
+ /// DataStructures::VerticesPlain as a parameter
+ /// \throws if target does not have a "Vertices" property
+ /// \param target the representation to be used as a target
+ void setTarget(const std::shared_ptr<Framework::Component>& target);
+
+ /// override update
+ /// \throws if the type of the "Vertices" property on the target is not DataStructures::VerticesPlain
+ void update(double dt) override;
+
+ bool doInitialize() override;
+
+ bool doWakeUp() override;
+
+private:
+
+ std::shared_ptr<Physics::DeformableRepresentation> m_source; ///@< Source representation
+ std::shared_ptr<Framework::Component> m_target; ///@< Target representation
+
+ /// vertices structure that is used for the update
+ DataStructures::VerticesPlain m_vertices;
+};
+}
+}
+
+#endif
diff --git a/SurgSim/Blocks/UnitTests/CMakeLists.txt b/SurgSim/Blocks/UnitTests/CMakeLists.txt
index ef906a1..506da9a 100644
--- a/SurgSim/Blocks/UnitTests/CMakeLists.txt
+++ b/SurgSim/Blocks/UnitTests/CMakeLists.txt
@@ -1,5 +1,5 @@
# This file is a part of the OpenSurgSim project.
-# Copyright 2012-2013, SimQuest Solutions Inc.
+# Copyright 2012-2016, SimQuest Solutions Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -15,18 +15,27 @@
include_directories (
${gtest_SOURCE_DIR}/include
+ ${gmock_SOURCE_DIR}/include
)
set(UNIT_TEST_SOURCES
+ CompoundShapeToGraphicsTests.cpp
+ DriveElementFromInputBehaviorTests.cpp
+ FunctionBehaviorTests.cpp
+ KeyBehaviorTests.cpp
+ KeyboardCallbackBehaviorTests.cpp
KeyboardTogglesComponentBehaviorTests.cpp
MassSpring1DRepresentationTests.cpp
MassSpring2DRepresentationTests.cpp
MassSpring3DRepresentationTests.cpp
MassSpringNDRepresentationUtilsTests.cpp
PoseInterpolatorTests.cpp
+ SingleKeyBehaviorTests.cpp
SpringTestUtils.cpp
+ TransferParticlesToPointCloudBehaviorTests.cpp
TransferPhysicsToGraphicsMeshBehaviorTests.cpp
TransferPhysicsToPointCloudBehaviorTests.cpp
+ VisualizeConstraintsTest.cpp
VisualizeContactsBehaviorTests.cpp
)
@@ -34,8 +43,6 @@ 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"
@@ -43,10 +50,13 @@ configure_file(
)
set(LIBS
+ IdentityPoseDevice
SurgSimBlocks
SurgSimGraphics
)
+file(COPY Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+
surgsim_add_unit_tests(SurgSimBlocksTest)
diff --git a/SurgSim/Blocks/UnitTests/CompoundShapeToGraphicsTests.cpp b/SurgSim/Blocks/UnitTests/CompoundShapeToGraphicsTests.cpp
new file mode 100644
index 0000000..893664e
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/CompoundShapeToGraphicsTests.cpp
@@ -0,0 +1,199 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/CompoundShapeToGraphics.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.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/Graphics/OsgSceneryRepresentation.h"
+#include "SurgSim/Math/CompoundShape.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+
+#include <gtest/gtest.h>
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+TEST(CompoundShapeToGraphicsTests, Init)
+{
+ EXPECT_NO_THROW(std::make_shared<CompoundShapeToGraphics>("Copier"));
+}
+
+TEST(CompoundShapeToGraphicsTests, SetShape)
+{
+ auto shape = std::make_shared<Math::CompoundShape>();
+ auto behavior = std::make_shared<CompoundShapeToGraphics>("Copier");
+
+ EXPECT_EQ(nullptr, behavior->getShape());
+ EXPECT_NO_THROW(behavior->setShape(shape));
+ EXPECT_EQ(shape, behavior->getShape());
+
+ // With shape set, setting Source will fail.
+ auto physics = std::make_shared<Physics::FixedRepresentation>("Physics");
+ EXPECT_ANY_THROW(behavior->setSource(physics));
+}
+
+TEST(CompoundShapeToGraphicsTests, SetSource)
+{
+ auto behavior = std::make_shared<CompoundShapeToGraphics>("Copier");
+ auto physics = std::make_shared<Physics::FixedRepresentation>("Physics");
+ auto collisions = std::make_shared<Collision::ShapeCollisionRepresentation>("Physics");
+ auto graphics = std::make_shared<Graphics::OsgSceneryRepresentation>("Graphics");
+
+ EXPECT_NO_THROW(behavior->setSource(physics));
+ EXPECT_EQ(physics, behavior->getSource());
+ EXPECT_NO_THROW(behavior->setSource(collisions));
+ EXPECT_EQ(collisions, behavior->getSource());
+ EXPECT_ANY_THROW(behavior->setSource(graphics));
+
+ // With Source being set, setting Shape will fail.
+ auto shape = std::make_shared<Math::CompoundShape>();
+ EXPECT_ANY_THROW(behavior->setShape(shape));
+}
+
+TEST(CompoundShapeToGraphicsTests, Serialization)
+{
+ std::shared_ptr<SurgSim::Framework::Component> behavior;
+ EXPECT_NO_THROW(
+ behavior =
+ SurgSim::Framework::Component::getFactory().create("SurgSim::Blocks::CompoundShapeToGraphics", "newCopier"));
+ auto behavior2 = std::make_shared<CompoundShapeToGraphics>("Copier");
+ auto graphics = std::make_shared<Graphics::OsgSceneryRepresentation>("Graphics");
+ std::shared_ptr<Math::Shape> compoundShape = std::make_shared<Math::CompoundShape>();
+ std::shared_ptr<Framework::Component> physics = std::make_shared<Physics::FixedRepresentation>("Physics");
+
+ std::vector<std::shared_ptr<Framework::Component>> targets;
+ targets.push_back(graphics);
+ targets.push_back(graphics);
+ targets.push_back(graphics);
+
+ EXPECT_NO_THROW(behavior->setValue("Targets", targets));
+ EXPECT_NO_THROW(behavior->setValue("Source", physics));
+ EXPECT_NO_THROW(behavior2->setValue("Targets", targets));
+ EXPECT_NO_THROW(behavior2->setValue("Shape", compoundShape));
+
+ EXPECT_EQ(3u, behavior->getValue<std::vector<std::shared_ptr<Framework::Component>>>("Targets").size());
+ EXPECT_EQ(physics, behavior->getValue<std::shared_ptr<Framework::Component>>("Source"));
+ EXPECT_EQ(compoundShape, behavior2->getValue<std::shared_ptr<Math::CompoundShape>>("Shape"));
+
+ YAML::Node node;
+ // Shape is not set on 'behavior', serialization should fail.
+ EXPECT_ANY_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*behavior));
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*behavior2));
+ EXPECT_EQ(6u, node[behavior2->getClassName()].size());
+
+ std::shared_ptr<SurgSim::Blocks::CompoundShapeToGraphics> newBehavior;
+ EXPECT_NO_THROW(newBehavior = std::dynamic_pointer_cast<SurgSim::Blocks::CompoundShapeToGraphics>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ EXPECT_EQ(3u, newBehavior->getValue<std::vector<std::shared_ptr<Framework::Component>>>("Targets").size());
+ EXPECT_NE(nullptr, newBehavior->getValue<std::shared_ptr<Math::CompoundShape>>("Shape"));
+}
+
+TEST(CompoundShapeToGraphicsTests, SetGraphics)
+{
+ auto behavior = std::make_shared<CompoundShapeToGraphics>("Copier");
+
+ EXPECT_EQ(0u, behavior->getTargets().size());
+
+ auto graphics = std::make_shared<Graphics::OsgSceneryRepresentation>("One");
+ auto physics = std::make_shared<Physics::FixedRepresentation>("Invalid");
+
+ EXPECT_NO_THROW(behavior->addTarget(graphics));
+ EXPECT_EQ(1u, behavior->getTargets().size());
+
+ EXPECT_ANY_THROW(behavior->addTarget(physics));
+ EXPECT_EQ(1u, behavior->getTargets().size());
+
+ std::vector<std::shared_ptr<Framework::Component>> targets;
+ targets.push_back(graphics);
+ targets.push_back(graphics);
+ targets.push_back(graphics);
+
+ EXPECT_NO_THROW(behavior->setTargets(targets));
+ EXPECT_EQ(3u, behavior->getTargets().size());
+}
+
+TEST(CompoundShapeToGraphicsTests, SetSourceWithValidShape)
+{
+ auto runtime = std::make_shared<Framework::Runtime>();
+ auto element = std::make_shared<Framework::BasicSceneElement>("Element");
+
+ auto behavior = std::make_shared<CompoundShapeToGraphics>("Copier");
+
+ auto physics = std::make_shared<Physics::FixedRepresentation>("Physics");
+
+ auto shape = std::make_shared<Math::CompoundShape>();
+
+ physics->setShape(shape);
+ behavior->setSource(physics);
+
+ element->addComponent(physics);
+ element->addComponent(behavior);
+ runtime->getScene()->addSceneElement(element);
+
+ EXPECT_NO_THROW(behavior->wakeUp());
+ EXPECT_EQ(shape, behavior->getShape());
+}
+
+TEST(CompoundShapeToGraphicsTests, SetSourceWithInvalidShape)
+{
+ auto runtime = std::make_shared<Framework::Runtime>();
+ auto element = std::make_shared<Framework::BasicSceneElement>("Element");
+
+ auto behavior = std::make_shared<CompoundShapeToGraphics>("Copier");
+
+ auto collision = std::make_shared<Collision::ShapeCollisionRepresentation>("Collisions");
+
+ auto shape = std::make_shared<Math::BoxShape>();
+ EXPECT_ANY_THROW(behavior->setShape(shape));
+
+ collision->setShape(shape);
+ behavior->setSource(collision);
+
+ element->addComponent(collision);
+ element->addComponent(behavior);
+ runtime->getScene()->addSceneElement(element);
+
+ EXPECT_ANY_THROW(behavior->wakeUp());
+}
+
+TEST(CompoundShapeToGraphicsTests, SetSourceWithoutShape)
+{
+ auto runtime = std::make_shared<Framework::Runtime>();
+ auto element = std::make_shared<Framework::BasicSceneElement>("Element");
+
+ auto behavior = std::make_shared<CompoundShapeToGraphics>("Copier");
+
+ auto collision = std::make_shared<Collision::ShapeCollisionRepresentation>("Collisions");
+
+ behavior->setSource(collision);
+
+ element->addComponent(collision);
+ element->addComponent(behavior);
+ runtime->getScene()->addSceneElement(element);
+
+ EXPECT_ANY_THROW(behavior->wakeUp());
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Blocks/UnitTests/Data/Geometry/wound_deformable.ply b/SurgSim/Blocks/UnitTests/Data/Geometry/wound_deformable.ply
deleted file mode 100644
index b4de671..0000000
--- a/SurgSim/Blocks/UnitTests/Data/Geometry/wound_deformable.ply
+++ /dev/null
@@ -1,2389 +0,0 @@
-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/Data/TransferPhysicsToGraphicsMeshBehavior/data.ply b/SurgSim/Blocks/UnitTests/Data/TransferPhysicsToGraphicsMeshBehavior/data.ply
new file mode 100644
index 0000000..00de5c0
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/Data/TransferPhysicsToGraphicsMeshBehavior/data.ply
@@ -0,0 +1,14 @@
+ply
+format ascii 1.0
+comment Created by hand
+element vertex 4
+property float x
+property float y
+property float z
+element face 0
+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
diff --git a/SurgSim/Blocks/UnitTests/Data/TransferPhysicsToGraphicsMeshBehavior/data_more.ply b/SurgSim/Blocks/UnitTests/Data/TransferPhysicsToGraphicsMeshBehavior/data_more.ply
new file mode 100644
index 0000000..6c45a12
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/Data/TransferPhysicsToGraphicsMeshBehavior/data_more.ply
@@ -0,0 +1,27 @@
+ply
+# contains the following points from data.ply
+# 0: 0
+# 1: 2
+# 2: 3
+# 3: 4
+# and one extra point that will not be connected
+format ascii 1.0
+comment Created by hand
+element vertex 10
+property float x
+property float y
+property float z
+element face 0
+property list uchar uint vertex_indices
+end_header
+2.000000 2.000000 2.000000
+-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
+0.050000 0.050000 0.050000
+
diff --git a/SurgSim/Blocks/UnitTests/DriveElementFromInputBehaviorTests.cpp b/SurgSim/Blocks/UnitTests/DriveElementFromInputBehaviorTests.cpp
new file mode 100644
index 0000000..464b8c9
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/DriveElementFromInputBehaviorTests.cpp
@@ -0,0 +1,130 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 DriveElementFromInputBehavior class.
+
+#include <gtest/gtest.h>
+#include <yaml-cpp/yaml.h>
+
+#include "SurgSim/Blocks/DriveElementFromInputBehavior.h"
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Input/InputComponent.h"
+#include "SurgSim/Input/OutputComponent.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Devices::IdentityPoseDevice;
+using SurgSim::Input::InputComponent;
+using SurgSim::Input::OutputComponent;
+using SurgSim::Framework::BasicSceneElement;
+using SurgSim::Math::makeRigidTranslation;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+
+namespace SurgSim
+{
+namespace Blocks
+{
+
+TEST(DriveElementFromInputBehaviorTest, Constructor)
+{
+ EXPECT_NO_THROW(DriveElementFromInputBehavior behavior("DriveElementFromInputBehavior"));
+}
+
+TEST(DriveElementFromInputBehaviorTest, SetGetSource)
+{
+ auto behavior = std::make_shared<DriveElementFromInputBehavior>("DriveElementFromInputBehavior");
+ EXPECT_EQ(nullptr, behavior->getSource());
+
+ auto invalidInputComponent = std::make_shared<OutputComponent>("InvalidInputComponent");
+ EXPECT_THROW(behavior->setSource(invalidInputComponent), SurgSim::Framework::AssertionFailure);
+
+ auto inputComponent = std::make_shared<InputComponent>("InputComponent");
+ EXPECT_NO_THROW(behavior->setSource(inputComponent));
+ EXPECT_EQ(inputComponent, behavior->getSource());
+}
+
+TEST(DriveElementFromInputBehaviorTest, SetGetName)
+{
+ auto behavior = std::make_shared<DriveElementFromInputBehavior>("DriveElementFromInputBehavior");
+ EXPECT_EQ("pose", behavior->getPoseName());
+
+ behavior->setPoseName("new_name");
+ EXPECT_EQ("new_name", behavior->getPoseName());
+}
+
+TEST(DriveElementFromInputBehaviorTest, Update)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ auto scene = runtime->getScene();
+
+ auto behavior = std::make_shared<DriveElementFromInputBehavior>("DriveElementFromInputBehavior");
+ auto element = std::make_shared<BasicSceneElement>("SceneElement");
+ auto device = std::make_shared<IdentityPoseDevice>("IdentityPoseDevice");
+ auto inputComponent = std::make_shared<InputComponent>("InputComponent");
+
+ device->addInputConsumer(inputComponent);
+ behavior->setSource(inputComponent);
+ element->addComponent(behavior);
+ element->addComponent(inputComponent);
+
+ scene->addSceneElement(element);
+
+ behavior->wakeUp();
+ inputComponent->wakeUp();
+
+ element->setPose(makeRigidTranslation(Vector3d(-1.0, -2.0, -3.0)));
+ behavior->update(0.1);
+ EXPECT_TRUE(element->getPose().isApprox(RigidTransform3d::Identity()));
+
+ RigidTransform3d inputOffset = makeRigidTranslation(Vector3d(10.0, 20.0, 30.0));
+ inputComponent->setLocalPose(inputOffset);
+ behavior->update(0.1);
+ EXPECT_TRUE(element->getPose().isApprox(inputOffset));
+}
+
+TEST(DriveElementFromInputBehaviorTest, Serialization)
+{
+ auto behavior = std::make_shared<DriveElementFromInputBehavior>("DriveElementFromInputBehavior");
+ EXPECT_EQ("SurgSim::Blocks::DriveElementFromInputBehavior", behavior->getClassName());
+
+ auto inputComponent = std::make_shared<InputComponent>("InputComponent");
+ EXPECT_NO_THROW(behavior->setValue("Source",
+ std::static_pointer_cast<SurgSim::Framework::Component>(inputComponent)));
+ EXPECT_NO_THROW(behavior->setValue("PoseName", std::string("Test")));
+
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*behavior));
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(5, node[behavior->getClassName()].size());
+
+ std::shared_ptr<DriveElementFromInputBehavior> decodedBehavior;
+ EXPECT_NO_THROW(decodedBehavior = std::dynamic_pointer_cast<DriveElementFromInputBehavior>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+
+ EXPECT_EQ("InputComponent", SurgSim::Framework::convert<std::shared_ptr<SurgSim::Framework::Component>>(
+ decodedBehavior->getValue("Source"))->getName());
+ EXPECT_EQ("Test", SurgSim::Framework::convert<std::string>(decodedBehavior->getValue("PoseName")));
+}
+
+}; //namespace Blocks
+}; //namespace SurgSim
diff --git a/SurgSim/Blocks/UnitTests/FunctionBehaviorTests.cpp b/SurgSim/Blocks/UnitTests/FunctionBehaviorTests.cpp
new file mode 100644
index 0000000..9ef480e
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/FunctionBehaviorTests.cpp
@@ -0,0 +1,106 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 <gtest/gtest.h>
+
+#include "SurgSim/Blocks/FunctionBehavior.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Runtime.h"
+
+namespace {
+
+void f1(double dt) { std::cout << dt; }
+void f2(double dt, double a) { std::cout << dt + a; }
+
+};
+
+namespace SurgSim
+{
+namespace Blocks
+{
+
+TEST(FunctionBehaviorTest, Constructor)
+{
+ EXPECT_NO_THROW(FunctionBehavior behavior("Behavior"));
+}
+
+TEST(FunctionBehaviorTest, GetSetTargetManagerType)
+{
+ auto behavior = std::make_shared<FunctionBehavior>("Behavior");
+ EXPECT_EQ(Framework::MANAGER_TYPE_BEHAVIOR, behavior->getTargetManagerType());
+
+ EXPECT_THROW(behavior->setTargetManagerType(-1), Framework::AssertionFailure);
+ EXPECT_NO_THROW(behavior->setTargetManagerType(Framework::MANAGER_TYPE_PHYSICS));
+ EXPECT_EQ(Framework::MANAGER_TYPE_PHYSICS, behavior->getTargetManagerType());
+
+ auto runtime = std::make_shared<Framework::Runtime>();
+ behavior->setFunction([](double dt) { std::cout << dt; });
+ behavior->initialize(runtime);
+
+ EXPECT_THROW(behavior->setTargetManagerType(Framework::MANAGER_TYPE_GRAPHICS), Framework::AssertionFailure);
+}
+
+TEST(FunctionBehaviorTest, SetFunction)
+{
+ auto runtime = std::make_shared<Framework::Runtime>();
+ {
+ auto behavior = std::make_shared<FunctionBehavior>("Behavior");
+ EXPECT_FALSE(behavior->initialize(runtime));
+ }
+ {
+ auto behavior = std::make_shared<FunctionBehavior>("Behavior");
+ behavior->setFunction([](double dt) { std::cout << dt; });
+ EXPECT_TRUE(behavior->initialize(runtime));
+ }
+}
+
+TEST(FunctionBehaviorTest, Update)
+{
+ size_t numUpdates = 0;
+ auto behavior = std::make_shared<FunctionBehavior>("Behavior");
+ behavior->setFunction([&numUpdates](double dt) { numUpdates++; });
+
+ auto runtime = std::make_shared<Framework::Runtime>();
+ behavior->initialize(runtime);
+ behavior->wakeUp();
+
+ behavior->update(1.0);
+ behavior->update(1.0);
+ EXPECT_EQ(2, numUpdates);
+}
+
+TEST(FunctionBehaviorTest, ExampleUsage)
+{
+ auto behavior1 = std::make_shared<SurgSim::Blocks::FunctionBehavior>("Behavior 1");
+ EXPECT_NO_THROW(behavior1->setTargetManagerType(SurgSim::Framework::MANAGER_TYPE_BEHAVIOR));
+ EXPECT_NO_THROW(behavior1->setFunction(f1));
+
+ auto behavior2 = std::make_shared<SurgSim::Blocks::FunctionBehavior>("Behavior 2");
+ EXPECT_NO_THROW(behavior2->setTargetManagerType(SurgSim::Framework::MANAGER_TYPE_GRAPHICS));
+ EXPECT_NO_THROW(behavior2->setFunction(std::bind(f2, std::placeholders::_1, 2.0)));
+
+ auto behavior3 = std::make_shared<SurgSim::Blocks::FunctionBehavior>("Behavior 3");
+ EXPECT_NO_THROW(behavior3->setTargetManagerType(SurgSim::Framework::MANAGER_TYPE_PHYSICS));
+ EXPECT_NO_THROW(behavior3->setFunction([](double dt) { std::cout << dt; }));
+
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("Element");
+ EXPECT_NO_THROW(element->addComponent(behavior1));
+ EXPECT_NO_THROW(element->addComponent(behavior2));
+ EXPECT_NO_THROW(element->addComponent(behavior3));
+}
+
+};
+};
diff --git a/SurgSim/Blocks/UnitTests/KeyBehaviorTests.cpp b/SurgSim/Blocks/UnitTests/KeyBehaviorTests.cpp
new file mode 100644
index 0000000..d246caa
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/KeyBehaviorTests.cpp
@@ -0,0 +1,152 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013 - 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 <gmock/gmock.h>
+
+#include "SurgSim/Blocks/KeyBehavior.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/DataStructures/NamedData.h"
+#include "SurgSim/Devices/Keyboard/KeyCode.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Testing/MockInputComponent.h"
+
+namespace SurgSim
+{
+namespace Blocks
+{
+class MockKeyBehavior : public KeyBehavior
+{
+public:
+ explicit MockKeyBehavior(const std::string& name) : KeyBehavior(name) {}
+
+ MOCK_METHOD1(onKeyDown, void(int));
+ MOCK_METHOD1(onKeyUp, void(int));
+};
+
+class KeyBehaviorTest: public testing::Test
+{
+public:
+
+ void SetUp()
+ {
+ builder.addInteger(DataStructures::Names::KEY);
+ data = builder.createData();
+ inputComponent = std::make_shared<Testing::MockInputComponent>("Input");
+ inputComponent->setData(data);
+ runtime = std::make_shared<Framework::Runtime>();
+ element = std::make_shared<Framework::BasicSceneElement>("Element");
+ runtime->getScene()->addSceneElement(element);
+ element->addComponent(inputComponent);
+ }
+
+ DataStructures::DataGroupBuilder builder;
+ DataStructures::DataGroup data;
+ std::shared_ptr<Testing::MockInputComponent> inputComponent;
+ std::shared_ptr<Framework::Runtime> runtime;
+ std::shared_ptr<Framework::SceneElement> element;
+};
+
+TEST_F(KeyBehaviorTest, Init)
+{
+ std::shared_ptr<MockKeyBehavior> behavior;
+ std::shared_ptr<Framework::Component> component = inputComponent;
+ ASSERT_NO_THROW(behavior = std::make_shared<MockKeyBehavior>("Behavior"));
+ EXPECT_NO_THROW(behavior->setValue("InputComponent", component));
+ auto input = behavior->getValue<std::shared_ptr<Input::InputComponent>>("InputComponent");
+ EXPECT_EQ(inputComponent.get(), input.get());
+}
+
+TEST_F(KeyBehaviorTest, SetAndGetInputComponent)
+{
+ auto behavior = std::make_shared<MockKeyBehavior>("MockSingleKeyBehavior");
+ auto input = std::make_shared<Input::InputComponent>("input");
+
+ EXPECT_NO_THROW(behavior->setInputComponent(input));
+ std::shared_ptr<Input::InputComponent> retrievedInputComponent;
+ EXPECT_NO_THROW(retrievedInputComponent = behavior->getInputComponent());
+ EXPECT_EQ(input, retrievedInputComponent);
+}
+
+TEST_F(KeyBehaviorTest, SimpleTrigger)
+{
+ auto behavior = std::make_shared<MockKeyBehavior>("Behavior");
+ EXPECT_CALL(*behavior, onKeyDown(Devices::KEY_A));
+ EXPECT_CALL(*behavior, onKeyUp(Devices::KEY_A));
+ behavior->setInputComponent(inputComponent);
+ element->addComponent(behavior);
+ data.integers().set(DataStructures::Names::KEY, Devices::KeyCode::KEY_A);
+ inputComponent->setData(data);
+ behavior->update(0.0);
+
+ // Shouldn't trigger another KeyDown ..
+ data.integers().set(DataStructures::Names::KEY, Devices::KeyCode::KEY_A);
+ inputComponent->setData(data);
+ behavior->update(0.0);
+
+ data.integers().set(DataStructures::Names::KEY, Devices::KeyCode::NONE);
+ inputComponent->setData(data);
+ behavior->update(0.0);
+ // Shouldn't trigger another KeyUp ..
+ behavior->update(0.0);
+}
+
+TEST_F(KeyBehaviorTest, FromOneKeyToAnother)
+{
+ auto behavior = std::make_shared<MockKeyBehavior>("Behavior");
+ EXPECT_CALL(*behavior, onKeyDown(Devices::KEY_A));
+ EXPECT_CALL(*behavior, onKeyDown(Devices::KEY_B));
+ EXPECT_CALL(*behavior, onKeyUp(Devices::KEY_A));
+ EXPECT_CALL(*behavior, onKeyUp(Devices::KEY_B));
+ behavior->setInputComponent(inputComponent);
+ element->addComponent(behavior);
+ data.integers().set(DataStructures::Names::KEY, Devices::KeyCode::KEY_A);
+ inputComponent->setData(data);
+ behavior->update(0.0);
+ data.integers().set(DataStructures::Names::KEY, Devices::KeyCode::KEY_B);
+ inputComponent->setData(data);
+ behavior->update(0.0);
+ data.integers().set(DataStructures::Names::KEY, Devices::KeyCode::NONE);
+ inputComponent->setData(data);
+ behavior->update(0.0);
+ // Shouldn't trigger another KeyUp ..
+ behavior->update(0.0);
+}
+
+TEST_F(KeyBehaviorTest, Registry)
+{
+ EXPECT_TRUE(KeyBehavior::registerKey(Devices::KeyCode::KEY_A, "TestFunctionality"));
+ EXPECT_TRUE(KeyBehavior::registerKey(Devices::KeyCode::KEY_B, "OtherFunctionality"));
+
+ EXPECT_FALSE(KeyBehavior::registerKey(Devices::KeyCode::KEY_A, "TestFunctionality"));
+
+ KeyBehavior::unregisterKey(Devices::KeyCode::KEY_A);
+ EXPECT_TRUE(KeyBehavior::registerKey(Devices::KeyCode::KEY_A, "TestFunctionality"));
+
+ KeyBehavior::logMap();
+
+}
+
+
+
+
+
+
+}
+}
diff --git a/SurgSim/Blocks/UnitTests/KeyboardCallbackBehaviorTests.cpp b/SurgSim/Blocks/UnitTests/KeyboardCallbackBehaviorTests.cpp
new file mode 100644
index 0000000..bc2b2f7
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/KeyboardCallbackBehaviorTests.cpp
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 KeyboardCallbackBehavior class.
+
+#include <gtest/gtest.h>
+#include <yaml-cpp/yaml.h>
+
+#include "SurgSim/Blocks/KeyboardCallbackBehavior.h"
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Input/InputComponent.h"
+#include "SurgSim/Input/OutputComponent.h"
+
+using SurgSim::Blocks::KeyboardCallbackBehavior;
+using SurgSim::Devices::KeyCode;
+using SurgSim::Framework::Component;
+using SurgSim::Input::InputComponent;
+using SurgSim::Input::OutputComponent;
+
+TEST(KeyboardCallbackBehavior, Constructor)
+{
+ EXPECT_NO_THROW(KeyboardCallbackBehavior behavior("KeyboardCallbackBehavior"));
+}
+
+TEST(KeyboardCallbackBehavior, InputComponentTests)
+{
+ auto keyboardCallbackBehavior =
+ std::make_shared<KeyboardCallbackBehavior>("KeyboardCallbackBehavior");
+ {
+ auto invalidInputComponent = std::make_shared<OutputComponent>("InvalidInputComponent");
+ EXPECT_ANY_THROW(keyboardCallbackBehavior->setInputComponent(invalidInputComponent));
+ }
+
+ {
+ auto inputComponent = std::make_shared<InputComponent>("InputComponent");
+
+ EXPECT_NO_THROW(keyboardCallbackBehavior->setInputComponent(inputComponent));
+ EXPECT_EQ(inputComponent, keyboardCallbackBehavior->getInputComponent());
+ }
+}
+
+TEST(KeyboardCallbackBehavior, Serialization)
+{
+ auto keyboardCallbackBehavior = std::make_shared<KeyboardCallbackBehavior>("KeyboardCallbackBehavior");
+ std::shared_ptr<Component> inputComponent = std::make_shared<InputComponent>("InputComponent");
+
+ int keyValue = static_cast<int>(KeyCode::KEY_A);
+ keyboardCallbackBehavior->setValue("ActionKey", keyValue);
+ keyboardCallbackBehavior->setValue("InputComponent", inputComponent);
+
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<Component>::encode(*keyboardCallbackBehavior));
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(5, node[keyboardCallbackBehavior->getClassName()].size());
+
+ std::shared_ptr<KeyboardCallbackBehavior> newKeyboardCallbackBehavior;
+ EXPECT_NO_THROW(newKeyboardCallbackBehavior = std::dynamic_pointer_cast<KeyboardCallbackBehavior>(
+ node.as<std::shared_ptr<Component>>()));
+ ASSERT_NE(nullptr, newKeyboardCallbackBehavior);
+ EXPECT_NE(nullptr, newKeyboardCallbackBehavior->getValue<std::shared_ptr<InputComponent>>("InputComponent"));
+
+ int newKeyValue = newKeyboardCallbackBehavior->getValue<int>("ActionKey");
+ EXPECT_EQ(newKeyValue, keyValue);
+}
diff --git a/SurgSim/Blocks/UnitTests/KeyboardTogglesComponentBehaviorTests.cpp b/SurgSim/Blocks/UnitTests/KeyboardTogglesComponentBehaviorTests.cpp
index 0924781..252c384 100644
--- a/SurgSim/Blocks/UnitTests/KeyboardTogglesComponentBehaviorTests.cpp
+++ b/SurgSim/Blocks/UnitTests/KeyboardTogglesComponentBehaviorTests.cpp
@@ -29,6 +29,7 @@
#include "SurgSim/Input/OutputComponent.h"
using SurgSim::Blocks::KeyboardTogglesComponentBehavior;
+using SurgSim::Devices::KeyCode;
using SurgSim::Framework::Component;
using SurgSim::Graphics::OsgBoxRepresentation;
using SurgSim::Graphics::Representation;
@@ -66,13 +67,13 @@ TEST(KeyboardTogglesComponentBehavior, RegistrationTests)
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);
+ keyboardTogglesComponentBehavior->registerKey(KeyCode::KEY_A, graphics);
+ keyboardTogglesComponentBehavior->registerKey(KeyCode::KEY_A, graphics2);
+ keyboardTogglesComponentBehavior->registerKey(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);
+ auto keyAPair = keyMap.find(KeyCode::KEY_A);
+ auto keyBPair = keyMap.find(KeyCode::KEY_B);
EXPECT_EQ(2u, keyAPair->second.size());
EXPECT_EQ(1u, keyBPair->second.size());
@@ -99,8 +100,8 @@ TEST(KeyboardTogglesComponentBehavior, SetAndGetKeyboardRegisterTypeTest)
set2.insert(graphics3);
KeyboardTogglesComponentBehavior::KeyboardRegistryType keyMap;
- keyMap[SurgSim::Device::KeyCode::KEY_A] = set1;
- keyMap[SurgSim::Device::KeyCode::KEY_B] = set2;
+ keyMap[KeyCode::KEY_A] = set1;
+ keyMap[KeyCode::KEY_B] = set2;
EXPECT_NO_THROW(keyboardTogglesComponentBehavior->setKeyboardRegistry(keyMap));
@@ -138,8 +139,8 @@ TEST(KeyboardTogglesComponentBehavior, Serialization)
set2.insert(graphics2);
KeyboardTogglesComponentBehavior::KeyboardRegistryType keyMap;
- keyMap[SurgSim::Device::KeyCode::KEY_A] = set1;
- keyMap[SurgSim::Device::KeyCode::KEY_B] = set2;
+ keyMap[KeyCode::KEY_A] = set1;
+ keyMap[KeyCode::KEY_B] = set2;
keyboardTogglesComponentBehavior->setValue("KeyboardRegistry", keyMap);
keyboardTogglesComponentBehavior->setValue("InputComponent", inputComponent);
diff --git a/SurgSim/Blocks/UnitTests/PoseInterpolatorTests.cpp b/SurgSim/Blocks/UnitTests/PoseInterpolatorTests.cpp
index f9501d3..f9627c3 100644
--- a/SurgSim/Blocks/UnitTests/PoseInterpolatorTests.cpp
+++ b/SurgSim/Blocks/UnitTests/PoseInterpolatorTests.cpp
@@ -19,9 +19,11 @@
#include "SurgSim/Blocks/PoseInterpolator.h"
#include "SurgSim/Framework/BasicSceneElement.h"
#include "SurgSim/Framework/Representation.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Vector.h"
-#include "SurgSim/Math/Quaternion.h"
using SurgSim::Math::RigidTransform3d;
@@ -32,8 +34,8 @@ 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));
+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
@@ -41,23 +43,48 @@ namespace SurgSim
namespace Blocks
{
-TEST(PoseInterpolatorTests, InitTest)
+class PoseInterpolatorTests : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ runtime = std::make_shared<Framework::Runtime>("");
+ scene = runtime->getScene();
+ element = std::make_shared<SurgSim::Framework::BasicSceneElement>("element");
+ representation = std::make_shared<SurgSim::Framework::Representation>("representation");
+ interpolator = std::make_shared<PoseInterpolator>("interpolator");
+ }
+
+ void TearDown()
+ {
+ representation->retire();
+ interpolator->retire();
+ }
+
+ std::shared_ptr<Framework::Runtime> runtime;
+ std::shared_ptr<Framework::Scene> scene;
+ std::shared_ptr<Framework::BasicSceneElement> element;
+ std::shared_ptr<Framework::Representation> representation;
+ std::shared_ptr<PoseInterpolator> interpolator;
+
+};
+
+TEST_F(PoseInterpolatorTests, InitTest)
{
ASSERT_NO_THROW({auto interpolator = std::make_shared<PoseInterpolator>("test");});
}
-TEST(PoseInterpolatorTests, StartAndEndPose)
+TEST_F(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();
+
+ scene->addSceneElement(element);
+
interpolator->wakeUp();
representation->wakeUp();
@@ -68,22 +95,18 @@ TEST(PoseInterpolatorTests, StartAndEndPose)
RigidTransform3d pose = interpolate(startPose, endPose, 0.5);
EXPECT_TRUE(pose.matrix().isApprox(representation->getPose().matrix())) << pose.matrix() << std::endl
- << representation->getPose().matrix();
+ << representation->getPose().matrix();
}
-TEST(PoseInterpolatorTests, UseOptionalStartPose)
+TEST_F(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();
+ scene->addSceneElement(element);
interpolator->wakeUp();
representation->wakeUp();
@@ -94,21 +117,17 @@ TEST(PoseInterpolatorTests, UseOptionalStartPose)
RigidTransform3d pose = interpolate(startPose, endPose, 0.5);
EXPECT_TRUE(pose.matrix().isApprox(representation->getPose().matrix())) << pose.matrix() << std::endl
- << representation->getPose().matrix();
+ << representation->getPose().matrix();
}
-TEST(PoseInterpolatorTests, UseLoop)
+TEST_F(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();
+ scene->addSceneElement(element);
interpolator->setPingPong(true);
interpolator->setLoop(true);
@@ -126,27 +145,23 @@ TEST(PoseInterpolatorTests, UseLoop)
RigidTransform3d pose = interpolate(startPose, endPose, 0.25);
EXPECT_TRUE(pose.matrix().isApprox(representation->getPose().matrix())) << pose.matrix() << std::endl
- << representation->getPose().matrix();
+ << 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();
+ << representation->getPose().matrix();
}
-TEST(PoseInterpolatorTests, UsePingPong)
+TEST_F(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();
+ scene->addSceneElement(element);
interpolator->setLoop(true);
interpolator->setPingPong(true);
@@ -164,13 +179,13 @@ TEST(PoseInterpolatorTests, UsePingPong)
RigidTransform3d pose = interpolate(startPose, endPose, 0.25);
EXPECT_TRUE(pose.matrix().isApprox(representation->getPose().matrix())) << pose.matrix() << std::endl
- << representation->getPose().matrix();
+ << 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();
+ << representation->getPose().matrix();
}
diff --git a/SurgSim/Blocks/UnitTests/SingleKeyBehaviorTests.cpp b/SurgSim/Blocks/UnitTests/SingleKeyBehaviorTests.cpp
new file mode 100644
index 0000000..8ae2602
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/SingleKeyBehaviorTests.cpp
@@ -0,0 +1,105 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013 - 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 SingleKeyBehavior class.
+
+#include <gtest/gtest.h>
+#include <yaml-cpp/yaml.h>
+
+#include "SurgSim/Blocks/SingleKeyBehavior.h"
+#include "SurgSim/Devices/Keyboard/KeyCode.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Input/InputComponent.h"
+
+class MockSingleKeyBehavior : public SurgSim::Blocks::SingleKeyBehavior
+{
+public:
+ explicit MockSingleKeyBehavior(const std::string& name) : SingleKeyBehavior(name)
+ {
+ }
+
+protected:
+ void onKey() override
+ {
+ }
+};
+
+TEST(SingleKeyBehaviorTest, Constructor)
+{
+ std::shared_ptr<SurgSim::Blocks::SingleKeyBehavior> singleKeyBehavior;
+ ASSERT_NO_THROW(singleKeyBehavior = std::make_shared<MockSingleKeyBehavior>("SingleKeyBehavior"));
+ EXPECT_EQ(nullptr, singleKeyBehavior->getInputComponent());
+ EXPECT_EQ(SurgSim::Devices::KeyCode::NONE, singleKeyBehavior->getKey());
+}
+
+TEST(SingleKeyBehaviorTest, SetAndGetKey)
+{
+ auto behavior = std::make_shared<MockSingleKeyBehavior>("MockSingleKeyBehavior");
+ int key = SurgSim::Devices::KeyCode::KEY_0;
+
+ EXPECT_NO_THROW(behavior->setKey(key));
+ int retrievedKey;
+ EXPECT_NO_THROW(retrievedKey = behavior->getKey());
+ EXPECT_EQ(key, retrievedKey);
+}
+
+TEST(SingleKeyBehaviorTest, WakeUp)
+{
+ auto input = std::make_shared<SurgSim::Input::InputComponent>("input");
+ int key = SurgSim::Devices::KeyCode::KEY_1;
+ {
+ auto behavior = std::make_shared<MockSingleKeyBehavior>("MockSingleKeyBehavior");
+ EXPECT_FALSE(behavior->doWakeUp());
+ }
+
+ {
+ auto behavior = std::make_shared<MockSingleKeyBehavior>("MockSingleKeyBehavior");
+ behavior->setInputComponent(input);
+ EXPECT_FALSE(behavior->doWakeUp());
+ }
+
+ {
+ auto behavior = std::make_shared<MockSingleKeyBehavior>("MockSingleKeyBehavior");
+ behavior->setKey(key);
+ EXPECT_FALSE(behavior->doWakeUp());
+ }
+
+ {
+ auto behavior = std::make_shared<MockSingleKeyBehavior>("MockSingleKeyBehavior");
+ behavior->setKey(key);
+ behavior->setInputComponent(input);
+ EXPECT_TRUE(behavior->doWakeUp());
+ }
+}
+
+TEST(SingleKeyBehaviorTest, AccessibleValues)
+{
+ auto behavior = std::make_shared<MockSingleKeyBehavior>("MockSingleKeyBehavior");
+ std::shared_ptr<SurgSim::Framework::Component> input = std::make_shared<SurgSim::Input::InputComponent>("input");
+ int key = SurgSim::Devices::KeyCode::KEY_0;
+
+ EXPECT_NO_THROW(behavior->setValue("ActionKey", key));
+ EXPECT_NO_THROW(behavior->setValue("InputComponent", input));
+ int retrievedKey;
+ std::shared_ptr<SurgSim::Framework::Component> retrievedInputComponent;
+
+ EXPECT_NO_THROW(retrievedKey = behavior->getValue<int>("ActionKey"));
+ EXPECT_NO_THROW(retrievedInputComponent =
+ behavior->getValue<std::shared_ptr<SurgSim::Input::InputComponent>>("InputComponent"));
+
+ EXPECT_EQ(key, retrievedKey);
+ EXPECT_EQ(input, retrievedInputComponent);
+}
diff --git a/SurgSim/Blocks/UnitTests/TransferParticlesToPointCloudBehaviorTests.cpp b/SurgSim/Blocks/UnitTests/TransferParticlesToPointCloudBehaviorTests.cpp
new file mode 100644
index 0000000..4c4dbcb
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/TransferParticlesToPointCloudBehaviorTests.cpp
@@ -0,0 +1,200 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 TransferParticlesToGraphicsBehavior class.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Blocks/TransferParticlesToPointCloudBehavior.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/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Particles/SphRepresentation.h"
+#include "SurgSim/Physics/PhysicsManager.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+using SurgSim::Math::Vector3d;
+
+
+namespace SurgSim
+{
+namespace Blocks
+{
+
+TEST(TransferParticlesToPointCloudBehaviorTests, ConstructorTest)
+{
+ ASSERT_NO_THROW(TransferParticlesToPointCloudBehavior("TestBehavior"));
+}
+
+TEST(TransferParticlesToPointCloudBehaviorTests, SetGetSourceTest)
+{
+ auto particles = std::make_shared<Particles::SphRepresentation>("Particles");
+ auto rigid = std::make_shared<Physics::RigidRepresentation>("Rigid");
+ auto behavior = std::make_shared<TransferParticlesToPointCloudBehavior>("Behavior");
+
+ EXPECT_THROW(behavior->setSource(nullptr), Framework::AssertionFailure);
+ EXPECT_THROW(behavior->setSource(rigid), Framework::AssertionFailure);
+ EXPECT_NO_THROW(behavior->setSource(particles));
+ EXPECT_EQ(particles, behavior->getSource());
+}
+
+TEST(TransferParticlesToPointCloudBehaviorTests, SetGetTargetTest)
+{
+ auto pointCloud = std::make_shared<Graphics::OsgPointCloudRepresentation>("OsgMesh");
+ auto graphicsBox = std::make_shared<Graphics::OsgBoxRepresentation>("OsgBox");
+ auto behavior = std::make_shared<TransferParticlesToPointCloudBehavior>("Behavior");
+
+ EXPECT_THROW(behavior->setTarget(nullptr), Framework::AssertionFailure);
+ EXPECT_THROW(behavior->setTarget(graphicsBox), Framework::AssertionFailure);
+ EXPECT_NO_THROW(behavior->setTarget(pointCloud));
+ EXPECT_EQ(pointCloud, behavior->getTarget());
+}
+
+TEST(TransferParticlesToPointCloudBehaviorTests, WakeUpTest)
+{
+ auto runtime = std::make_shared<Framework::Runtime>("config.txt");
+
+ auto particles = std::make_shared<Particles::SphRepresentation>("Particles");
+ particles->setMassPerParticle(1.0);
+ particles->setDensity(1.0);
+ particles->setGasStiffness(1.0);
+ particles->setKernelSupport(1.0);
+
+ auto pointCloud = std::make_shared<Graphics::OsgPointCloudRepresentation>("Graphics");
+
+ {
+ auto behavior = std::make_shared<TransferParticlesToPointCloudBehavior>("Behavior");
+ EXPECT_TRUE(behavior->initialize(runtime));
+ EXPECT_FALSE(behavior->wakeUp());
+ }
+
+ {
+ auto behavior = std::make_shared<TransferParticlesToPointCloudBehavior>("Behavior");
+ behavior->setTarget(pointCloud);
+ EXPECT_TRUE(behavior->initialize(runtime));
+ EXPECT_FALSE(behavior->wakeUp());
+ }
+
+ {
+ auto behavior = std::make_shared<TransferParticlesToPointCloudBehavior>("Behavior");
+ behavior->setSource(particles);
+ EXPECT_TRUE(behavior->initialize(runtime));
+ EXPECT_FALSE(behavior->wakeUp());
+ }
+
+ {
+ auto behavior = std::make_shared<TransferParticlesToPointCloudBehavior>("Behavior");
+ behavior->setSource(particles);
+ behavior->setTarget(pointCloud);
+ EXPECT_TRUE(behavior->initialize(runtime));
+ EXPECT_TRUE(behavior->wakeUp());
+ }
+}
+
+TEST(TransferParticlesToPointCloudBehaviorTests, UpdateTest)
+{
+ auto runtime = std::make_shared<Framework::Runtime>();
+ runtime->addManager(std::make_shared<Framework::BehaviorManager>());
+ runtime->addManager(std::make_shared<Physics::PhysicsManager>());
+
+ auto sceneElement = std::make_shared<Framework::BasicSceneElement>("Element");
+
+ auto particles = std::make_shared<Particles::SphRepresentation>("Particles");
+ particles->setMaxParticles(10);
+ particles->setMassPerParticle(1.0);
+ particles->setDensity(1.0);
+ particles->setGasStiffness(1.0);
+ particles->setKernelSupport(1.0);
+ for (size_t particleId = 0; particleId < 10; particleId++)
+ {
+ particles->addParticle(Vector3d(static_cast<double>(particleId), 0.0, 0.0), Vector3d::Zero(), 100000);
+ }
+ sceneElement->addComponent(particles);
+
+ auto pointCloud = std::make_shared<Graphics::OsgPointCloudRepresentation>("Graphics");
+ sceneElement->addComponent(pointCloud);
+
+ auto behavior = std::make_shared<TransferParticlesToPointCloudBehavior>("Behavior");
+ behavior->setSource(particles);
+ behavior->setTarget(pointCloud);
+ sceneElement->addComponent(behavior);
+
+ auto scene = runtime->getScene();
+ scene->addSceneElement(sceneElement);
+
+ particles->update(0.1);
+ behavior->update(0.1);
+ auto sourceVertices = particles->getParticles().safeGet()->getVertices();
+ auto targetVertices = pointCloud->getVertices()->getVertices();
+ ASSERT_EQ(sourceVertices.size(), targetVertices.size());
+
+ auto sourceVertex = sourceVertices.begin();
+ auto targetVertex = targetVertices.begin();
+ for (; sourceVertex != sourceVertices.end(); ++sourceVertex, ++targetVertex)
+ {
+ EXPECT_TRUE(sourceVertex->position.isApprox(targetVertex->position));
+ }
+
+ particles->removeParticle(0);
+ particles->removeParticle(1);
+ particles->update(0.1);
+ behavior->update(0.1);
+
+ sourceVertices = particles->getParticles().safeGet()->getVertices();
+ targetVertices = pointCloud->getVertices()->getVertices();
+ ASSERT_EQ(sourceVertices.size(), targetVertices.size());
+
+ sourceVertex = sourceVertices.begin();
+ targetVertex = targetVertices.begin();
+ for (; sourceVertex != sourceVertices.end(); ++sourceVertex, ++targetVertex)
+ {
+ EXPECT_TRUE(sourceVertex->position.isApprox(targetVertex->position));
+ }
+}
+
+TEST(TransferParticlesToPointCloudBehaviorTests, SerializationTest)
+{
+ std::shared_ptr<Framework::Component> particles = std::make_shared<Particles::SphRepresentation>("Particles");
+ std::shared_ptr<Framework::Component> pointCloud =
+ std::make_shared<Graphics::OsgPointCloudRepresentation>("Graphics");
+
+ auto behavior = std::make_shared<TransferParticlesToPointCloudBehavior>("Behavior");
+
+ EXPECT_NO_THROW(behavior->setValue("Source", particles));
+ EXPECT_NO_THROW(behavior->setValue("Target", pointCloud));
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<Framework::Component>::encode(*behavior));
+ EXPECT_EQ(1u, node.size());
+
+ YAML::Node data = node["SurgSim::Blocks::TransferParticlesToPointCloudBehavior"];
+ EXPECT_EQ(5u, data.size());
+
+ std::shared_ptr<TransferParticlesToPointCloudBehavior> newBehavior;
+ std::shared_ptr<Framework::Component> nodeAsComponent = node.as<std::shared_ptr<Framework::Component>>();
+ ASSERT_NO_THROW(newBehavior = std::dynamic_pointer_cast<TransferParticlesToPointCloudBehavior>(nodeAsComponent));
+
+ EXPECT_EQ("SurgSim::Blocks::TransferParticlesToPointCloudBehavior", newBehavior->getClassName());
+ EXPECT_NE(nullptr, newBehavior->getValue<std::shared_ptr<Particles::Representation>>("Source"));
+ EXPECT_NE(nullptr, newBehavior->getValue<std::shared_ptr<Graphics::PointCloudRepresentation>>("Target"));
+}
+
+}; // namespace Blocks
+}; // namespace SurgSim
diff --git a/SurgSim/Blocks/UnitTests/TransferPhysicsToGraphicsMeshBehaviorTests.cpp b/SurgSim/Blocks/UnitTests/TransferPhysicsToGraphicsMeshBehaviorTests.cpp
index 38873cd..5270e19 100644
--- a/SurgSim/Blocks/UnitTests/TransferPhysicsToGraphicsMeshBehaviorTests.cpp
+++ b/SurgSim/Blocks/UnitTests/TransferPhysicsToGraphicsMeshBehaviorTests.cpp
@@ -19,6 +19,8 @@
#include <gtest/gtest.h>
#include "SurgSim/Blocks/TransferPhysicsToGraphicsMeshBehavior.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/Framework/ApplicationData.h"
#include "SurgSim/Framework/BasicSceneElement.h"
#include "SurgSim/Framework/BehaviorManager.h"
#include "SurgSim/Framework/Runtime.h"
@@ -28,6 +30,7 @@
#include "SurgSim/Graphics/OsgMeshRepresentation.h"
#include "SurgSim/Math/OdeState.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
#include "SurgSim/Physics/Fem3DRepresentation.h"
#include "SurgSim/Physics/RigidRepresentation.h"
@@ -41,12 +44,17 @@ using SurgSim::Math::Vector3d;
using SurgSim::Physics::Fem3DRepresentation;
using SurgSim::Physics::RigidRepresentation;
-TEST(TransferPhysicsToGraphicsMeshBehaviorTests, ConstructorTest)
+namespace SurgSim
+{
+namespace Blocks
+{
+
+TEST(TransferPhysicsToGraphicsMeshBehaviorTests, Constructor)
{
ASSERT_NO_THROW(TransferPhysicsToGraphicsMeshBehavior("TestBehavior"));
}
-TEST(TransferPhysicsToGraphicsMeshBehaviorTests, SetGetSourceTest)
+TEST(TransferPhysicsToGraphicsMeshBehaviorTests, SetGetSource)
{
auto physics = std::make_shared<Fem3DRepresentation>("PhysicsDeformable");
auto rigid = std::make_shared<RigidRepresentation>("PhysicsRigid");
@@ -58,7 +66,7 @@ TEST(TransferPhysicsToGraphicsMeshBehaviorTests, SetGetSourceTest)
EXPECT_EQ(physics, behavior->getSource());
}
-TEST(TransferPhysicsToGraphicsMeshBehaviorTests, SetGetTargetTest)
+TEST(TransferPhysicsToGraphicsMeshBehaviorTests, SetGetTarget)
{
auto graphics = std::make_shared<OsgMeshRepresentation>("OsgMesh");
auto graphicsBox = std::make_shared<OsgBoxRepresentation>("OsgBox");
@@ -70,7 +78,7 @@ TEST(TransferPhysicsToGraphicsMeshBehaviorTests, SetGetTargetTest)
EXPECT_EQ(graphics, behavior->getTarget());
}
-TEST(TransferPhysicsToGraphicsMeshBehaviorTests, UpdateTest)
+TEST(TransferPhysicsToGraphicsMeshBehaviorTests, Update)
{
auto runtime = std::make_shared<Runtime>("config.txt");
auto behaviorManager = std::make_shared<BehaviorManager>();
@@ -80,7 +88,7 @@ TEST(TransferPhysicsToGraphicsMeshBehaviorTests, UpdateTest)
auto sceneElement = std::make_shared<BasicSceneElement>("scene element");
auto physics = std::make_shared<Fem3DRepresentation>("Fem3D");
- physics->setFilename("Geometry/wound_deformable.ply");
+ physics->loadFem("Geometry/wound_deformable.ply");
auto graphics = std::make_shared<OsgMeshRepresentation>("GraphicsMesh");
auto behavior = std::make_shared<TransferPhysicsToGraphicsMeshBehavior>("Behavior");
@@ -120,13 +128,14 @@ TEST(TransferPhysicsToGraphicsMeshBehaviorTests, UpdateTest)
runtime->stop();
}
-TEST(TransferPhysicsToGraphicsMeshBehaviorTests, SerializationTest)
+TEST(TransferPhysicsToGraphicsMeshBehaviorTests, Serialization)
{
- std::string filename = std::string("Data/Geometry/wound_deformable.ply");
+ std::string filename = std::string("Geometry/wound_deformable_with_texture.ply");
std::shared_ptr<SurgSim::Framework::Component> physics = std::make_shared<Fem3DRepresentation>("Fem3D");
auto fem3d = std::dynamic_pointer_cast<Fem3DRepresentation>(physics);
- fem3d->setFilename(filename);
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ fem3d->loadFem(filename);
std::shared_ptr<SurgSim::Framework::Component> graphics =
std::make_shared<OsgMeshRepresentation>("GraphicsMesh");
@@ -140,13 +149,97 @@ TEST(TransferPhysicsToGraphicsMeshBehaviorTests, SerializationTest)
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>>()));
+ 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"));
+
+
+}
+
+TEST(TransferPhysicsToGraphicsMeshBehaviorTests, MappingAccessors)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ auto behavior = std::make_shared<TransferPhysicsToGraphicsMeshBehavior>("Behavior");
+
+ auto names = std::make_pair(std::string("TransferPhysicsToGraphicsMeshBehavior/data.ply"),
+ std::string("TransferPhysicsToGraphicsMeshBehavior/data.ply"));
+
+ // Plain Setter
+ auto indices = behavior->getIndexMap();
+ std::vector<std::pair<size_t, size_t>> empty;
+ EXPECT_TRUE(indices.empty());
+
+ indices.push_back(std::make_pair(0, 0));
+ behavior->setValue("IndexMap", indices);
+ EXPECT_EQ(1u, behavior->getIndexMap().size());
+
+ // SetValue with pair
+ ASSERT_NO_THROW(behavior->setValue("IndexMapMeshNames", names));
+ indices = behavior->getValue<std::vector<std::pair<size_t, size_t>>>("IndexMap");
+ EXPECT_EQ(4, indices.size());
+
+ // SetValue with pair of meshes
+ std::shared_ptr<Framework::Asset> mesh1 = std::make_shared<DataStructures::TriangleMeshPlain>();
+ mesh1->load("TransferPhysicsToGraphicsMeshBehavior/data.ply");
+
+ std::shared_ptr<Framework::Asset> mesh2 = std::make_shared<DataStructures::TriangleMeshPlain>();
+ mesh2->load("TransferPhysicsToGraphicsMeshBehavior/data_more.ply");
+
+ auto meshes = std::make_pair(mesh1, mesh2);
+ ASSERT_NO_THROW(behavior->setValue("IndexMapMeshes", meshes));
+ EXPECT_EQ(9u, behavior->getIndexMap().size());
+
+ 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"];
+ std::shared_ptr<TransferPhysicsToGraphicsMeshBehavior> newBehavior;
+ ASSERT_NO_THROW(newBehavior = std::dynamic_pointer_cast<TransferPhysicsToGraphicsMeshBehavior>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+
+ EXPECT_EQ(9u, newBehavior->getIndexMap().size());
+}
+
+TEST(TransferPhysicsToGraphicsMeshBehaviorTests, Mapping)
+{
+ Framework::ApplicationData data("config.txt");
+ auto mesh1 = std::make_shared<DataStructures::TriangleMeshPlain>();
+ mesh1->load("TransferPhysicsToGraphicsMeshBehavior/data.ply", data);
+
+ auto indices = generateIndexMap(mesh1, mesh1);
+
+ EXPECT_EQ(mesh1->getNumVertices(), indices.size());
+ for (const auto& pair : indices)
+ {
+ EXPECT_EQ(pair.first, pair.second);
+ }
+
+ auto mesh2 = std::make_shared<DataStructures::TriangleMeshPlain>();
+ mesh2->load("TransferPhysicsToGraphicsMeshBehavior/data_more.ply", data);
+
+ indices = generateIndexMap(mesh1, mesh2);
+ EXPECT_EQ(9u, indices.size());
+
+ std::array<size_t, 4> counts = {0, 0, 0, 0};
+
+ for (const auto& pair : indices)
+ {
+ ++counts[pair.first];
+ // Nothing should refer to the first vertex
+ EXPECT_NE(0, pair.second);
+ }
+
+ EXPECT_EQ(0u, counts[0]);
+ EXPECT_EQ(2u, counts[1]);
+ EXPECT_EQ(3u, counts[2]);
+ EXPECT_EQ(4u, counts[3]);
+}
+
}
+}
+
diff --git a/SurgSim/Blocks/UnitTests/TransferPhysicsToPointCloudBehaviorTests.cpp b/SurgSim/Blocks/UnitTests/TransferPhysicsToPointCloudBehaviorTests.cpp
index 8b5d8f9..d84be4b 100644
--- a/SurgSim/Blocks/UnitTests/TransferPhysicsToPointCloudBehaviorTests.cpp
+++ b/SurgSim/Blocks/UnitTests/TransferPhysicsToPointCloudBehaviorTests.cpp
@@ -27,6 +27,7 @@
#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
#include "SurgSim/Math/OdeState.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
#include "SurgSim/Physics/Fem3DRepresentation.h"
#include "SurgSim/Physics/RigidRepresentation.h"
@@ -79,7 +80,7 @@ TEST(TransferPhysicsToPointCloudBehaviorTests, UpdateTest)
auto sceneElement = std::make_shared<BasicSceneElement>("scene element");
auto physics = std::make_shared<Fem3DRepresentation>("Fem3D");
- physics->setFilename("Geometry/wound_deformable.ply");
+ physics->loadFem("Geometry/wound_deformable.ply");
auto pointCloud = std::make_shared<OsgPointCloudRepresentation>("GraphicsMesh");
auto behavior = std::make_shared<TransferPhysicsToPointCloudBehavior>("Behavior");
@@ -121,11 +122,12 @@ TEST(TransferPhysicsToPointCloudBehaviorTests, UpdateTest)
TEST(TransferPhysicsToPointCloudBehaviorTests, SerializationTest)
{
- std::string filename = std::string("Data/Geometry/wound_deformable.ply");
+ std::string filename = std::string("Geometry/wound_deformable_with_texture.ply");
std::shared_ptr<SurgSim::Framework::Component> physics = std::make_shared<Fem3DRepresentation>("Fem3D");
auto fem3d = std::dynamic_pointer_cast<Fem3DRepresentation>(physics);
- fem3d->setFilename(filename);
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ fem3d->loadFem(filename);
std::shared_ptr<SurgSim::Framework::Component> pointCloud =
std::make_shared<OsgPointCloudRepresentation>("GraphicsMesh");
diff --git a/SurgSim/Blocks/UnitTests/VisualizeConstraintsTest.cpp b/SurgSim/Blocks/UnitTests/VisualizeConstraintsTest.cpp
new file mode 100644
index 0000000..0fb56d6
--- /dev/null
+++ b/SurgSim/Blocks/UnitTests/VisualizeConstraintsTest.cpp
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/VisualizeConstraints.h"
+#include "SurgSim/Graphics/OsgVectorFieldRepresentation.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+
+#include <gtest/gtest.h>
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+TEST(VisualizeConstraintsTest, Init)
+{
+ EXPECT_NO_THROW({ auto element = std::make_shared<VisualizeConstraints>(); });
+ EXPECT_NO_THROW({ auto component = std::make_shared<VisualizeConstraintsBehavior>("Behavior"); });
+}
+
+TEST(VisualizeConstraintsTest, SingleSetter)
+{
+ auto visualizer = std::make_shared<VisualizeConstraintsBehavior>("Behavior");
+ std::shared_ptr<Framework::Component> field =
+ std::make_shared<Graphics::OsgVectorFieldRepresentation>("VectorField");
+ EXPECT_NO_THROW(visualizer->setVectorField(Physics::CONSTRAINT_GROUP_TYPE_CONTACT, field));
+ EXPECT_ANY_THROW(visualizer->setVectorField(Physics::CONSTRAINT_GROUP_TYPE_COUNT, field));
+ EXPECT_ANY_THROW(visualizer->setVectorField(Physics::CONSTRAINT_GROUP_TYPE_CONTACT, nullptr));
+}
+
+TEST(VisualizeConstraintsTest, Serialization)
+{
+ auto behavior = std::make_shared<VisualizeConstraintsBehavior>("VisualizeConstraints");
+ std::shared_ptr<Framework::Component> field =
+ std::make_shared<Graphics::OsgVectorFieldRepresentation>("VectorField");
+
+ VisualizeConstraintsBehavior::FieldsType fields;
+ fields.push_back(std::make_pair(Physics::CONSTRAINT_GROUP_TYPE_CONTACT, field));
+ EXPECT_NO_THROW(behavior->setValue("VectorFields", fields));
+
+ EXPECT_EQ("SurgSim::Blocks::VisualizeConstraintsBehavior", behavior->getClassName());
+
+ // Encode
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*behavior));
+ EXPECT_TRUE(node.IsMap());
+
+ // Decode
+ std::shared_ptr<VisualizeConstraintsBehavior> newBehavior;
+ EXPECT_NO_THROW(newBehavior = std::dynamic_pointer_cast<VisualizeConstraintsBehavior>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+
+ EXPECT_NE(nullptr, newBehavior);
+ // Verify
+
+ auto newFields = newBehavior->getValue<VisualizeConstraintsBehavior::FieldsType>("VectorFields");
+ EXPECT_EQ(1u, newFields.size());
+ EXPECT_EQ("VectorField", newFields[0].second->getName());
+}
+
+
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Blocks/UnitTests/VisualizeContactsBehaviorTests.cpp b/SurgSim/Blocks/UnitTests/VisualizeContactsBehaviorTests.cpp
index 716c488..f85184a 100644
--- a/SurgSim/Blocks/UnitTests/VisualizeContactsBehaviorTests.cpp
+++ b/SurgSim/Blocks/UnitTests/VisualizeContactsBehaviorTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -20,8 +20,12 @@
#include <yaml-cpp/yaml.h>
#include "SurgSim/Blocks/VisualizeContactsBehavior.h"
-#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
using SurgSim::Blocks::VisualizeContactsBehavior;
using SurgSim::Physics::RigidCollisionRepresentation;
@@ -41,8 +45,8 @@ TEST(VisualizeContactsBehaviorTests, SettersGetters)
std::string name = "CollisionRepresentation";
auto collisionRepresentaiton = std::make_shared<RigidCollisionRepresentation>(name);
- EXPECT_NO_THROW(visualizeContactsBehavior->setCollisionRepresentation(collisionRepresentaiton));
- EXPECT_EQ(name, visualizeContactsBehavior->getCollisionRepresentation()->getName());
+ EXPECT_NO_THROW(visualizeContactsBehavior->setSource(collisionRepresentaiton));
+ EXPECT_EQ(name, visualizeContactsBehavior->getSource()->getName());
// Test vector field scale.
EXPECT_ANY_THROW(visualizeContactsBehavior->setVectorFieldScale(-1.023));
@@ -57,7 +61,7 @@ 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",
+ EXPECT_NO_THROW(visualizeContactsBehavior->setValue("Source",
std::static_pointer_cast<SurgSim::Framework::Component>(collisionRepresentation)););
double scale = 1.234;
EXPECT_NO_THROW(visualizeContactsBehavior->setValue("VectorFieldScale", scale));
@@ -76,6 +80,31 @@ TEST(VisualizeContactsBehaviorTests, Serialization)
// Verify
EXPECT_EQ(name, SurgSim::Framework::convert<std::shared_ptr<SurgSim::Framework::Component>>(
- newVisualizeContactsBehavior->getValue("CollisionRepresentation"))->getName());
+ newVisualizeContactsBehavior->getValue("Source"))->getName());
EXPECT_EQ(scale, SurgSim::Framework::convert<double>(newVisualizeContactsBehavior->getValue("VectorFieldScale")));
-}
\ No newline at end of file
+}
+
+TEST(VisualizeContactsBehaviorTests, MultipleInstances)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ auto manager = std::make_shared<SurgSim::Graphics::OsgManager>();
+ runtime->addManager(manager);
+
+ auto visualizeContactsBehavior1 = std::make_shared<VisualizeContactsBehavior>("VisualizeContactsBehavior1");
+ auto visualizeContactsBehavior2 = std::make_shared<VisualizeContactsBehavior>("VisualizeContactsBehavior2");
+ auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("SceneElement");
+ sceneElement->addComponent(visualizeContactsBehavior1);
+ sceneElement->addComponent(visualizeContactsBehavior2);
+
+ auto collisionRepresentation = std::make_shared<RigidCollisionRepresentation>("CollisionRepresentation");
+
+ EXPECT_NO_THROW(visualizeContactsBehavior1->setSource(collisionRepresentation));
+ EXPECT_NO_THROW(visualizeContactsBehavior2->setSource(collisionRepresentation));
+
+ runtime->getScene()->addSceneElement(sceneElement);
+ EXPECT_TRUE(visualizeContactsBehavior1->isInitialized());
+ EXPECT_TRUE(visualizeContactsBehavior2->isInitialized());
+
+ EXPECT_TRUE(visualizeContactsBehavior1->wakeUp());
+ EXPECT_TRUE(visualizeContactsBehavior2->wakeUp());
+}
diff --git a/SurgSim/Blocks/UnitTests/config.txt.in b/SurgSim/Blocks/UnitTests/config.txt.in
index 9c1cf05..1f35931 100644
--- a/SurgSim/Blocks/UnitTests/config.txt.in
+++ b/SurgSim/Blocks/UnitTests/config.txt.in
@@ -1 +1,3 @@
-${CMAKE_CURRENT_BINARY_DIR}/Data/
\ No newline at end of file
+${PROJECT_BINARY_DIR}/Data/
+${CMAKE_CURRENT_BINARY_DIR}/Data/
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data/
diff --git a/SurgSim/Blocks/VisualizeConstraints.cpp b/SurgSim/Blocks/VisualizeConstraints.cpp
new file mode 100644
index 0000000..402312e
--- /dev/null
+++ b/SurgSim/Blocks/VisualizeConstraints.cpp
@@ -0,0 +1,211 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/VisualizeConstraints.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Graphics/OsgVectorFieldRepresentation.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/PhysicsManager.h"
+#include "SurgSim/Physics/SlidingConstraintData.h"
+
+namespace SurgSim
+{
+
+namespace Blocks
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Blocks::VisualizeConstraintsBehavior,
+ VisualizeConstraintsBehavior);
+
+VisualizeConstraintsBehavior::VisualizeConstraintsBehavior(const std::string& name) :
+ Behavior(name),
+ m_logger(SurgSim::Framework::Logger::getLogger("Block/VisualizeConstraintsBehavior"))
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(VisualizeConstraintsBehavior, FieldsType, VectorFields,
+ getVectorFields, setVectorFields);
+}
+
+void VisualizeConstraintsBehavior::setVectorField(Physics::ConstraintGroupType constraintType,
+ const std::shared_ptr<Framework::Component>& vectorField)
+{
+ SURGSIM_ASSERT(constraintType < Physics::CONSTRAINT_GROUP_TYPE_COUNT) << "Invalid constraint type";
+
+ auto converted = Framework::checkAndConvert<Graphics::VectorFieldRepresentation>(
+ vectorField, "SurgSim::Graphics::VectorFieldRepresentation");
+
+ boost::lock_guard<boost::mutex> lock(m_graphicsMutex);
+ m_graphics[constraintType] = converted;
+}
+
+void VisualizeConstraintsBehavior::update(double dt)
+{
+ using DataStructures::Vertices;
+ using Graphics::VectorFieldData;
+ using Physics::ConstraintGroupType;
+ using Physics::ContactConstraintData;
+
+ Physics::PhysicsManagerState state;
+ auto manager = m_manager.lock();
+ if (manager == nullptr)
+ {
+ return;
+ }
+
+ manager->getFinalState(&state);
+
+ if (state.getMlcpProblem().getSize() == 0)
+ {
+ return;
+ }
+
+ Math::MlcpSolution::Vector& x = state.getMlcpSolution().x;
+ if (state.getMlcpProblem().getSize() != static_cast<size_t>(x.size()))
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "mlcp solution size = " << x.size() << " while mlcp problem size = " <<
+ state.getMlcpProblem().getSize() << std::endl;
+ return;
+ }
+
+ if (x.size() < 1)
+ {
+ return;
+ }
+
+ // For each constraint type...
+ size_t constraintTypeEnd = static_cast<int>(Physics::CONSTRAINT_GROUP_TYPE_COUNT);
+
+ std::map<int, Graphics::VectorField> vectorFields;
+
+ for (size_t constraintType = 0; constraintType < constraintTypeEnd; constraintType++)
+ {
+ ConstraintGroupType constraintGroupType = static_cast<ConstraintGroupType>(constraintType);
+ const std::vector<std::shared_ptr<Physics::Constraint>>& constraints =
+ state.getConstraintGroup(constraintType);
+
+ // For each constraint of that type...
+ for (auto it = constraints.begin(); it != constraints.end(); it++)
+ {
+ // Let's gather the application point and the force vector...
+ Math::Vector3d force = Math::Vector3d::Zero();
+ Math::Vector3d point = (*it)->getLocalizations().first->calculatePosition();
+
+ int mlcpConstraintIndex = state.getConstraintsMapping().getValue((*it).get());
+ Math::Vector4d color = Math::Vector4d::Zero();
+
+ switch ((*it)->getType())
+ {
+ case SurgSim::Physics::FIXED_3DPOINT:
+ {
+ force = x.segment(mlcpConstraintIndex, 3);
+ color = SurgSim::Math::Vector4d(0.9, 0.9, 0.9, 1);
+ }
+ break;
+ case SurgSim::Physics::FIXED_3DROTATION_VECTOR:
+ {
+ force = x.segment(mlcpConstraintIndex, 3);
+ color = SurgSim::Math::Vector4d(0.0, 0.0, 0.9, 1);
+ }
+ break;
+ case SurgSim::Physics::FRICTIONLESS_3DCONTACT:
+ {
+ const SurgSim::Math::Vector3d& forceNormal =
+ std::static_pointer_cast<ContactConstraintData>((*it)->getData())->getNormal();
+ double forceNormalIntensity = x[mlcpConstraintIndex];
+ force = forceNormal * forceNormalIntensity;
+ color = SurgSim::Math::Vector4d(0.9, 0.0, 0.0, 1);
+ }
+ break;
+ case SurgSim::Physics::FRICTIONLESS_SLIDING:
+ {
+ auto data = std::static_pointer_cast<SurgSim::Physics::SlidingConstraintData>((*it)->getData());
+ auto normals = data->getNormals();
+ force = normals[0].cross(normals[1]) * 0.1;
+ color = SurgSim::Math::Vector4d(0.0, 0.9, 0.0, 1);
+ }
+ break;
+ default:
+ break;
+ }
+
+ // Build the data
+ VectorFieldData data;
+ data.direction = force;
+ data.color.setValue(color);
+ Vertices<VectorFieldData>::VertexType vertex(point, data);
+ vectorFields[constraintGroupType].addVertex(vertex);
+ }
+ }
+
+ boost::lock_guard<boost::mutex> lock(m_graphicsMutex);
+ for (const auto& graphics : m_graphics)
+ {
+ graphics.second->updateVectorField(vectorFields[graphics.first]);
+ }
+}
+
+int VisualizeConstraintsBehavior::getTargetManagerType() const
+{
+ return Framework::MANAGER_TYPE_GRAPHICS;
+}
+
+bool VisualizeConstraintsBehavior::doInitialize()
+{
+ m_manager = getRuntime()->getManager<Physics::PhysicsManager>();
+ return !m_manager.expired();
+}
+
+bool VisualizeConstraintsBehavior::doWakeUp()
+{
+ return true;
+}
+
+void VisualizeConstraintsBehavior::setVectorFields(const FieldsType& fields)
+{
+ for (const auto& field : fields)
+ {
+ setVectorField(static_cast<Physics::ConstraintGroupType>(field.first), field.second);
+ }
+}
+
+SurgSim::Blocks::VisualizeConstraintsBehavior::FieldsType VisualizeConstraintsBehavior::getVectorFields() const
+{
+ FieldsType result;
+ for (const auto& field : m_graphics)
+ {
+ result.push_back(field);
+ }
+ return result;
+}
+
+VisualizeConstraints::VisualizeConstraints(const std::string& name) : BasicSceneElement(name)
+{
+ auto visualizer = std::make_shared<VisualizeConstraintsBehavior>("Visualizer");
+ addComponent(visualizer);
+ {
+ auto vectors = std::make_shared<Graphics::OsgVectorFieldRepresentation>("Contact");
+ visualizer->setVectorField(Physics::CONSTRAINT_GROUP_TYPE_CONTACT, vectors);
+ addComponent(vectors);
+ }
+ {
+ auto vectors = std::make_shared<Graphics::OsgVectorFieldRepresentation>("Scene");
+ visualizer->setVectorField(Physics::CONSTRAINT_GROUP_TYPE_SCENE, vectors);
+ addComponent(vectors);
+ }
+}
+
+}
+}
diff --git a/SurgSim/Blocks/VisualizeConstraints.h b/SurgSim/Blocks/VisualizeConstraints.h
new file mode 100644
index 0000000..fe854a3
--- /dev/null
+++ b/SurgSim/Blocks/VisualizeConstraints.h
@@ -0,0 +1,103 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_VISUALIZECONSTRAINTS_H
+#define SURGSIM_BLOCKS_VISUALIZECONSTRAINTS_H
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+class PhysicsManager;
+}
+
+namespace Framework
+{
+class Logger;
+}
+
+namespace Graphics
+{
+class VectorFieldRepresentation;
+}
+
+namespace Blocks
+{
+SURGSIM_STATIC_REGISTRATION(VisualizeConstraintsBehavior);
+/// Behavior to visualize information about the constraints as they are in the physics manager
+/// this will show the constraint location and the force from the last iteration of the physics manager
+/// \note currently only two types of constraint are being visualized MLCP_BILATERAL_3D_CONSTRAINT and
+/// MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT
+class VisualizeConstraintsBehavior : public SurgSim::Framework::Behavior
+{
+public:
+ explicit VisualizeConstraintsBehavior(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Blocks::VisualizeConstraintsBehavior);
+
+ /// Set the graphics Vectorfield to be used to display the constraint of the given physics manager group
+ /// \param constraintType internal group type e.g. CONSTRAINT_GROUP_TYPE_CONTACT
+ /// \param vectorField vectorField to be used for visualization of this constraint group
+ void setVectorField(Physics::ConstraintGroupType constraintType,
+ const std::shared_ptr<Framework::Component>& vectorField);
+
+ typedef std::vector<std::pair<int, std::shared_ptr<Framework::Component>>> FieldsType;
+
+ /// Sets all of the fields in one swoop
+ /// \param fields list of all the fields and their constraint group
+ void setVectorFields(const FieldsType& fields);
+
+ /// \return all the vector fields used in this behavior
+ FieldsType getVectorFields() const;
+
+ void update(double dt) override;
+
+ int getTargetManagerType() const override;
+
+private:
+
+
+ bool doInitialize() override;
+
+ bool doWakeUp() override;
+
+ /// Need reference to physics manager for introspection into the state
+ std::weak_ptr<SurgSim::Physics::PhysicsManager> m_manager;
+
+ std::map<SurgSim::Physics::ConstraintGroupType, std::shared_ptr<SurgSim::Graphics::VectorFieldRepresentation>>
+ m_graphics;
+
+ boost::mutex m_graphicsMutex;
+
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+};
+
+/// SceneElement that generates the VisualizeConstraintBehavior and the appropriate graphics Vectorfield
+class VisualizeConstraints : public SurgSim::Framework::BasicSceneElement
+{
+public:
+ explicit VisualizeConstraints(const std::string& name = "ConstraintVisualization");
+};
+
+}; // namespace Blocks
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_BLOCKS_VISUALIZECONSTRAINTS_H
diff --git a/SurgSim/Blocks/VisualizeContactsBehavior.cpp b/SurgSim/Blocks/VisualizeContactsBehavior.cpp
index 9e91f1e..8d171cd 100644
--- a/SurgSim/Blocks/VisualizeContactsBehavior.cpp
+++ b/SurgSim/Blocks/VisualizeContactsBehavior.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -21,6 +21,7 @@
#include "SurgSim/DataStructures/BufferedValue.h"
#include "SurgSim/DataStructures/Vertex.h"
#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/SceneElement.h"
#include "SurgSim/Graphics/OsgVectorFieldRepresentation.h"
#include "SurgSim/Graphics/VectorField.h"
@@ -29,43 +30,45 @@
using SurgSim::Collision::Contact;
using SurgSim::Collision::Representation;
using SurgSim::DataStructures::Vertex;
+using SurgSim::Framework::checkAndConvert;
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"))
+ m_vectorField(std::make_shared<OsgVectorFieldRepresentation>(name + std::string("_VectorField")))
{
SURGSIM_ADD_SERIALIZABLE_PROPERTY(VisualizeContactsBehavior, std::shared_ptr<SurgSim::Framework::Component>,
- CollisionRepresentation, getCollisionRepresentation, setCollisionRepresentation);
+ Source, getSource, setSource);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(VisualizeContactsBehavior, double, VectorFieldScale,
- getVectorFieldScale, setVectorFieldScale);
+ getVectorFieldScale, setVectorFieldScale);
}
-std::shared_ptr<SurgSim::Framework::Component> VisualizeContactsBehavior::getCollisionRepresentation()
+std::shared_ptr<SurgSim::Framework::Component> VisualizeContactsBehavior::getSource()
{
- return m_collisionRepresentation;
+ return m_source;
}
-void VisualizeContactsBehavior::setCollisionRepresentation(
- std::shared_ptr<SurgSim::Framework::Component> collisionRepresentation)
+void VisualizeContactsBehavior::setSource(std::shared_ptr<SurgSim::Framework::Component> source)
{
- m_collisionRepresentation = std::dynamic_pointer_cast<Representation>(collisionRepresentation);
+ m_source = checkAndConvert<Representation>(source, "SurgSim::Collision::Representation");
}
void VisualizeContactsBehavior::update(double dt)
{
std::shared_ptr<const SurgSim::Collision::ContactMapType> collisions =
- m_collisionRepresentation->getCollisions().safeGet();
+ m_source->getCollisions().safeGet();
if (!collisions->empty())
{
size_t totalContacts = 0;
@@ -78,8 +81,20 @@ void VisualizeContactsBehavior::update(double dt)
vectorField->clear();
vectorField->getVertices().reserve(2 * totalContacts);
- SurgSim::Math::RigidTransform3d inverseElementPose = getSceneElement()->getPose().inverse();
- auto representationPoseFirst = m_collisionRepresentation->getPose();
+ Math::RigidTransform3d inverseElementPose;
+ auto element = getSceneElement();
+ if (element != nullptr)
+ {
+ inverseElementPose = element->getPose().inverse();
+ }
+ else
+ {
+ inverseElementPose = Math::RigidTransform3d::Identity();
+ SURGSIM_LOG_ONCE(Framework::Logger::getDefaultLogger(), WARNING) << getClassName() << " named '"
+ << getFullName() << "' must be in a SceneElement.";
+ }
+
+ auto representationPoseFirst = m_source->getPose();
for (auto it = collisions->cbegin(); it != collisions->cend(); ++it)
{
auto representationPoseSecond = (*it).first->getPose();
@@ -88,12 +103,12 @@ void VisualizeContactsBehavior::update(double dt)
VectorFieldData vectorData1;
VectorFieldData vectorData2;
vectorData1.direction = -(*iter)->normal * (*iter)->depth;
- vectorData2.direction = (*iter)->normal * (*iter)->depth;
+ vectorData2.direction = (*iter)->normal * (*iter)->depth;
Vertex<VectorFieldData> vertex1 = Vertex<VectorFieldData>(
- (*iter)->penetrationPoints.first.rigidLocalPosition.getValue(), vectorData1);
+ (*iter)->penetrationPoints.first.rigidLocalPosition.getValue(), vectorData1);
Vertex<VectorFieldData> vertex2 = Vertex<VectorFieldData>(
- (*iter)->penetrationPoints.second.rigidLocalPosition.getValue(), vectorData2);
+ (*iter)->penetrationPoints.second.rigidLocalPosition.getValue(), vectorData2);
vertex1.position = inverseElementPose * representationPoseFirst * vertex1.position;
vertex2.position = inverseElementPose * representationPoseSecond * vertex2.position;
@@ -101,11 +116,11 @@ void VisualizeContactsBehavior::update(double dt)
vectorField->addVertex(vertex2);
}
}
- m_vectorField->setVisible(true);
+ m_vectorField->setLocalActive(true);
}
else
{
- m_vectorField->setVisible(false);
+ m_vectorField->setLocalActive(false);
}
}
@@ -116,14 +131,27 @@ int VisualizeContactsBehavior::getTargetManagerType() const
bool VisualizeContactsBehavior::doInitialize()
{
- SURGSIM_ASSERT(m_collisionRepresentation) << "VisualizeContactsBehavior: no collision representation held.";
return true;
}
bool VisualizeContactsBehavior::doWakeUp()
{
- getSceneElement()->addComponent(m_vectorField);
- return true;
+ if (m_source == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << getClassName() << " named '"
+ << getName() << "' must have a source.";
+ return false;
+ }
+
+ auto element = getSceneElement();
+ if (element == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << getClassName() << " named '"
+ << getFullName() << "' must be in a SceneElement.";
+ return false;
+ }
+
+ return element->addComponent(m_vectorField);
}
double VisualizeContactsBehavior::getVectorFieldScale()
@@ -138,4 +166,4 @@ void VisualizeContactsBehavior::setVectorFieldScale(double scale)
}
} // namespace Blocks
-} // namespace SurgSim
\ No newline at end of file
+} // namespace SurgSim
diff --git a/SurgSim/Blocks/VisualizeContactsBehavior.h b/SurgSim/Blocks/VisualizeContactsBehavior.h
index bca0911..93ed713 100644
--- a/SurgSim/Blocks/VisualizeContactsBehavior.h
+++ b/SurgSim/Blocks/VisualizeContactsBehavior.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -51,21 +51,17 @@ public:
SURGSIM_CLASSNAME(SurgSim::Blocks::VisualizeContactsBehavior);
- /// Used for serialization.
+ /// Get the source of the contacts
/// \return The collision representation whose contacts will be visualized.
- std::shared_ptr<SurgSim::Framework::Component> getCollisionRepresentation();
+ std::shared_ptr<SurgSim::Framework::Component> getSource();
- /// Used for serialization.
- /// \param collisionRepresentation The collision representation whose contacts will be visualized.
- void setCollisionRepresentation(std::shared_ptr<SurgSim::Framework::Component> collisionRepresentation);
+ /// Set the source of the contacts
+ /// \param source The collision representation whose contacts will be visualized.
+ void setSource(std::shared_ptr<SurgSim::Framework::Component> source);
- /// 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;
+ 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;
+ int getTargetManagerType() const override;
/// \return The scale of the vector field.
double getVectorFieldScale();
@@ -75,19 +71,13 @@ public:
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;
+ 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;
+ bool doWakeUp() override;
private:
/// The collision representation to get contacts for visualizing.
- std::shared_ptr<SurgSim::Collision::Representation> m_collisionRepresentation;
+ std::shared_ptr<SurgSim::Collision::Representation> m_source;
/// The osg vector field for visualizing contacts on collision representation
std::shared_ptr<SurgSim::Graphics::VectorFieldRepresentation> m_vectorField;
diff --git a/SurgSim/CMakeLists.txt b/SurgSim/CMakeLists.txt
index 6012d83..2c8e342 100644
--- a/SurgSim/CMakeLists.txt
+++ b/SurgSim/CMakeLists.txt
@@ -24,5 +24,8 @@ add_subdirectory(Framework)
add_subdirectory(Graphics)
add_subdirectory(Input)
add_subdirectory(Math)
+add_subdirectory(Particles)
add_subdirectory(Physics)
add_subdirectory(Testing)
+
+set(SURGSIM_DEVICES_LIBRARIES ${SURGSIM_DEVICES_LIBRARIES} PARENT_SCOPE)
diff --git a/SurgSim/Collision/BoxCapsuleContact.cpp b/SurgSim/Collision/BoxCapsuleContact.cpp
new file mode 100644
index 0000000..30440b9
--- /dev/null
+++ b/SurgSim/Collision/BoxCapsuleContact.cpp
@@ -0,0 +1,189 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/BoxCapsuleContact.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/Geometry.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
+{
+
+std::pair<int, int> BoxCapsuleContact::getShapeTypes()
+{
+ return std::pair<int, int>(SurgSim::Math::SHAPE_TYPE_BOX, SurgSim::Math::SHAPE_TYPE_CAPSULE);
+}
+
+std::list<std::shared_ptr<Contact>> BoxCapsuleContact::calculateDcdContact(
+ const SurgSim::Math::BoxShape& boxShape,
+ const SurgSim::Math::RigidTransform3d& boxPose,
+ const SurgSim::Math::CapsuleShape& capsuleShape,
+ const SurgSim::Math::RigidTransform3d& capsulePose) const
+{
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ RigidTransform3d capsuleToBoxTransform = boxPose.inverse() * capsulePose;
+ 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);
+ normal = boxPose.linear() * normal;
+ std::pair<Location, Location> penetrationPoints = std::make_pair(Location(deepestBoxPoint),
+ Location(capsulePose.inverse() * boxPose * deepestCapsulePoint));
+ contacts.emplace_back(std::make_shared<Contact>(COLLISION_DETECTION_TYPE_DISCRETE, distance, 1.0,
+ Vector3d::Zero(), normal, penetrationPoints));
+ }
+ return contacts;
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/BoxCapsuleContact.h b/SurgSim/Collision/BoxCapsuleContact.h
new file mode 100644
index 0000000..d36faf3
--- /dev/null
+++ b/SurgSim/Collision/BoxCapsuleContact.h
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_BOXCAPSULECONTACT_H
+#define SURGSIM_COLLISION_BOXCAPSULECONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/CapsuleShape.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+/// Class to calculate intersections between Boxes and Capsules
+class BoxCapsuleContact : public ShapeShapeContactCalculation<Math::BoxShape, Math::CapsuleShape>
+{
+public:
+ using ContactCalculation::calculateDcdContact;
+
+ std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const SurgSim::Math::BoxShape& boxShape,
+ const SurgSim::Math::RigidTransform3d& boxPose,
+ const SurgSim::Math::CapsuleShape& capsuleShape,
+ const SurgSim::Math::RigidTransform3d& capsulePose) const override;
+
+ std::pair<int, int> getShapeTypes() override;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/BoxCapsuleDcdContact.cpp b/SurgSim/Collision/BoxCapsuleDcdContact.cpp
deleted file mode 100644
index cfdaa15..0000000
--- a/SurgSim/Collision/BoxCapsuleDcdContact.cpp
+++ /dev/null
@@ -1,194 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 3805ed6..0000000
--- a/SurgSim/Collision/BoxCapsuleDcdContact.h
+++ /dev/null
@@ -1,53 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/BoxDoubleSidedPlaneContact.cpp b/SurgSim/Collision/BoxDoubleSidedPlaneContact.cpp
new file mode 100644
index 0000000..349a695
--- /dev/null
+++ b/SurgSim/Collision/BoxDoubleSidedPlaneContact.cpp
@@ -0,0 +1,152 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/BoxDoubleSidedPlaneContact.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::DoubleSidedPlaneShape;
+using SurgSim::Math::Geometry::DistanceEpsilon;
+using SurgSim::Math::Vector3d;
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+std::pair<int, int> BoxDoubleSidedPlaneContact::getShapeTypes()
+{
+ return std::pair<int, int>(SurgSim::Math::SHAPE_TYPE_BOX, SurgSim::Math::SHAPE_TYPE_DOUBLESIDEDPLANE);
+}
+
+std::list<std::shared_ptr<Contact>> BoxDoubleSidedPlaneContact::calculateDcdContact(
+ const SurgSim::Math::BoxShape& boxShape,
+ const SurgSim::Math::RigidTransform3d& boxPose,
+ const SurgSim::Math::DoubleSidedPlaneShape& planeShape,
+ const SurgSim::Math::RigidTransform3d& planePose) const
+{
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ // Transform the plane normal to box co-ordinate system.
+ SurgSim::Math::RigidTransform3d planeLocalToBoxLocal = boxPose.inverse() * planePose;
+ Vector3d planeNormal = planeLocalToBoxLocal.linear() * planeShape.getNormal();
+ Vector3d planeNormalScaled = planeShape.getNormal() * -planeShape.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] = boxShape.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;
+
+ enum BoxPlaneIntersectionType
+ {
+ BoxPlaneIntersectionTypeEqualsZero,
+ BoxPlaneIntersectionTypeLessThanZero,
+ BoxPlaneIntersectionTypeGreaterThanZero
+ } boxPlaneIntersectionType;
+
+ if (std::abs(maxD) < DistanceEpsilon)
+ {
+ // Box is touching the "back side" of plane.
+ normal = -(planePose.linear() * planeShape.getNormal());
+ boxPlaneIntersectionType = BoxPlaneIntersectionTypeEqualsZero;
+ }
+ else if (std::abs(minD) < DistanceEpsilon)
+ {
+ // Box is touching the "front side" of plane.
+ normal = planePose.linear() * planeShape.getNormal();
+ boxPlaneIntersectionType = BoxPlaneIntersectionTypeEqualsZero;
+ }
+ else
+ {
+ if (std::abs(maxD) >= std::abs(minD))
+ {
+ // Box is penetrating through the "front side" of plane.
+ normal = planePose.linear() * planeShape.getNormal();
+ boxPlaneIntersectionType = BoxPlaneIntersectionTypeLessThanZero;
+ }
+ else
+ {
+ // Box is penetrating through the "back side" of plane.
+ normal = -(planePose.linear() * planeShape.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 = std::make_pair(Location(boxVertices[i]),
+ Location(planePose.inverse() * (boxPose * boxVertices[i] + normal * std::abs(d[i]))));
+ contacts.emplace_back(std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE,
+ std::abs(d[i]), 1.0, Vector3d::Zero(), normal,
+ penetrationPoints));
+
+ generateContact = false;
+ }
+ }
+ }
+ return contacts;
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/BoxDoubleSidedPlaneContact.h b/SurgSim/Collision/BoxDoubleSidedPlaneContact.h
new file mode 100644
index 0000000..5f6ad54
--- /dev/null
+++ b/SurgSim/Collision/BoxDoubleSidedPlaneContact.h
@@ -0,0 +1,51 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_BOXDOUBLESIDEDPLANECONTACT_H
+#define SURGSIM_COLLISION_BOXDOUBLESIDEDPLANECONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between Boxes and Planes
+class BoxDoubleSidedPlaneContact : public ShapeShapeContactCalculation<Math::BoxShape, Math::DoubleSidedPlaneShape>
+{
+public:
+ using ContactCalculation::calculateDcdContact;
+
+ std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const SurgSim::Math::BoxShape& boxShape,
+ const SurgSim::Math::RigidTransform3d& boxPose,
+ const SurgSim::Math::DoubleSidedPlaneShape& planeShape,
+ const SurgSim::Math::RigidTransform3d& planePose) const override;
+
+ std::pair<int, int> getShapeTypes() override;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.cpp b/SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.cpp
deleted file mode 100644
index e2d8d16..0000000
--- a/SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.cpp
+++ /dev/null
@@ -1,166 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index d8cf457..0000000
--- a/SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.h
+++ /dev/null
@@ -1,52 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/BoxPlaneContact.cpp b/SurgSim/Collision/BoxPlaneContact.cpp
new file mode 100644
index 0000000..47d9009
--- /dev/null
+++ b/SurgSim/Collision/BoxPlaneContact.cpp
@@ -0,0 +1,76 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/BoxPlaneContact.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::Geometry::DistanceEpsilon;
+using SurgSim::Math::PlaneShape;
+using SurgSim::Math::Vector3d;
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+std::pair<int, int> BoxPlaneContact::getShapeTypes()
+{
+ return std::pair<int, int>(SurgSim::Math::SHAPE_TYPE_BOX, SurgSim::Math::SHAPE_TYPE_PLANE);
+}
+
+std::list<std::shared_ptr<Contact>> BoxPlaneContact::calculateDcdContact(
+ const SurgSim::Math::BoxShape& boxShape,
+ const SurgSim::Math::RigidTransform3d& boxPose,
+ const SurgSim::Math::PlaneShape& planeShape,
+ const SurgSim::Math::RigidTransform3d& planePose) const
+{
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ // Transform the plane normal to box co-ordinate system.
+ SurgSim::Math::RigidTransform3d planeLocalToBoxLocal = boxPose.inverse() * planePose;
+ SurgSim::Math::RigidTransform3d boxLocalToPlaneLocal = planeLocalToBoxLocal.inverse();
+ Vector3d planeNormal = planeLocalToBoxLocal.linear() * planeShape.getNormal();
+ Vector3d planeNormalScaled = planeShape.getNormal() * -planeShape.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 = boxShape.getVertex(i);
+ d = planeNormal.dot(boxVertex) + planeD;
+ if (d < DistanceEpsilon)
+ {
+ std::pair<Location, Location> penetrationPoints = std::make_pair(Location(boxVertex),
+ Location(boxLocalToPlaneLocal * (boxVertex - planeNormal * d)));
+ contacts.emplace_back(std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, -d, 1.0,
+ Vector3d::Zero(), planePose.linear() * planeShape.getNormal(),
+ penetrationPoints));
+ }
+ }
+ return contacts;
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/BoxPlaneContact.h b/SurgSim/Collision/BoxPlaneContact.h
new file mode 100644
index 0000000..0b37aa8
--- /dev/null
+++ b/SurgSim/Collision/BoxPlaneContact.h
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_BOXPLANECONTACT_H
+#define SURGSIM_COLLISION_BOXPLANECONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/PlaneShape.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between Boxes and Planes
+class BoxPlaneContact : public ShapeShapeContactCalculation<Math::BoxShape, Math::PlaneShape>
+{
+public:
+ using ContactCalculation::calculateDcdContact;
+
+ std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const SurgSim::Math::BoxShape& boxShape,
+ const SurgSim::Math::RigidTransform3d& boxPose,
+ const SurgSim::Math::PlaneShape& planeShape,
+ const SurgSim::Math::RigidTransform3d& planePose) const override;
+
+ std::pair<int, int> getShapeTypes() override;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/BoxPlaneDcdContact.cpp b/SurgSim/Collision/BoxPlaneDcdContact.cpp
deleted file mode 100644
index 2ddeac7..0000000
--- a/SurgSim/Collision/BoxPlaneDcdContact.cpp
+++ /dev/null
@@ -1,88 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 392dafb..0000000
--- a/SurgSim/Collision/BoxPlaneDcdContact.h
+++ /dev/null
@@ -1,53 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/BoxSphereContact.cpp b/SurgSim/Collision/BoxSphereContact.cpp
new file mode 100644
index 0000000..214a4d1
--- /dev/null
+++ b/SurgSim/Collision/BoxSphereContact.cpp
@@ -0,0 +1,121 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/BoxSphereContact.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::Geometry::DistanceEpsilon;
+using SurgSim::Math::Geometry::SquaredDistanceEpsilon;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+std::pair<int, int> BoxSphereContact::getShapeTypes()
+{
+ return std::pair<int, int>(SurgSim::Math::SHAPE_TYPE_BOX, SurgSim::Math::SHAPE_TYPE_SPHERE);
+}
+
+std::list<std::shared_ptr<Contact>> BoxSphereContact::calculateDcdContact(
+ const SurgSim::Math::BoxShape& boxShape,
+ const SurgSim::Math::RigidTransform3d& boxPose,
+ const SurgSim::Math::SphereShape& sphereShape,
+ const SurgSim::Math::RigidTransform3d& spherePose) const
+{
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ // Sphere center...
+ Vector3d sphereCenter = spherePose.translation();
+ // ... in Box coordinate system.
+ Vector3d boxLocalSphereCenter = boxPose.inverse() * sphereCenter;
+
+ // Box half size.
+ Vector3d boxSize = boxShape.getSize() * 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 - (sphereShape.getRadius() * sphereShape.getRadius()) > SquaredDistanceEpsilon)
+ {
+ // There is no collision.
+ return contacts;
+ }
+
+ 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;
+ }
+
+ double depth = std::abs(distance - sphereShape.getRadius());
+ normal = boxPose.linear() * normal;
+ std::pair<Location, Location> penetrationPoints = std::make_pair(Location(closestPoint),
+ Location(spherePose.inverse() * (sphereCenter + (normal * sphereShape.getRadius()))));
+
+ // Create the contact.
+ contacts.emplace_back(std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, depth, 1.0,
+ Vector3d::Zero(), normal, penetrationPoints));
+ return contacts;
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/BoxSphereContact.h b/SurgSim/Collision/BoxSphereContact.h
new file mode 100644
index 0000000..c5fa49e
--- /dev/null
+++ b/SurgSim/Collision/BoxSphereContact.h
@@ -0,0 +1,53 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_BOXSPHERECONTACT_H
+#define SURGSIM_COLLISION_BOXSPHERECONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between a box and a sphere
+class BoxSphereContact : public ShapeShapeContactCalculation<Math::BoxShape, Math::SphereShape>
+{
+public:
+
+ using ContactCalculation::calculateDcdContact;
+
+ std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const Math::BoxShape& boxShape,
+ const Math::RigidTransform3d& boxPose,
+ const Math::SphereShape& sphereShape,
+ const Math::RigidTransform3d& spherePose) const override;
+
+ std::pair<int, int> getShapeTypes() override;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/BoxSphereDcdContact.cpp b/SurgSim/Collision/BoxSphereDcdContact.cpp
deleted file mode 100644
index d050850..0000000
--- a/SurgSim/Collision/BoxSphereDcdContact.cpp
+++ /dev/null
@@ -1,131 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index d029cbb..0000000
--- a/SurgSim/Collision/BoxSphereDcdContact.h
+++ /dev/null
@@ -1,55 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
index cdf20ab..7b8ff3c 100644
--- a/SurgSim/Collision/CMakeLists.txt
+++ b/SurgSim/Collision/CMakeLists.txt
@@ -1,5 +1,5 @@
# This file is a part of the OpenSurgSim project.
-# Copyright 2012-2013, SimQuest Solutions Inc.
+# Copyright 2012-2015, SimQuest Solutions Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -14,49 +14,78 @@
# limitations under the License.
set(SURGSIM_COLLISION_SOURCES
- BoxCapsuleDcdContact.cpp
- BoxDoubleSidedPlaneDcdContact.cpp
- BoxPlaneDcdContact.cpp
- BoxSphereDcdContact.cpp
- CapsuleSphereDcdContact.cpp
+ BoxCapsuleContact.cpp
+ BoxDoubleSidedPlaneContact.cpp
+ BoxPlaneContact.cpp
+ BoxSphereContact.cpp
+ CapsuleSphereContact.cpp
CollisionPair.cpp
+ CompoundShapeContact.cpp
ContactCalculation.cpp
+ ContactFilter.cpp
DefaultContactCalculation.cpp
- OctreeDcdContact.cpp
+ ElementContactFilter.cpp
+ OctreeCapsuleContact.cpp
+ OctreeContact.cpp
+ OctreeDoubleSidedPlaneContact.cpp
+ OctreePlaneContact.cpp
+ OctreeSphereContact.cpp
Representation.cpp
+ SegmentMeshTriangleMeshContact.cpp
+ SegmentSegmentCcdIntervalCheck.cpp
+ SegmentSegmentCcdMovingContact.cpp
+ SegmentSegmentCcdStaticContact.cpp
+ SegmentSelfContact.cpp
ShapeCollisionRepresentation.cpp
- SphereDoubleSidedPlaneDcdContact.cpp
- SpherePlaneDcdContact.cpp
- SphereSphereDcdContact.cpp
- TriangleMeshPlaneDcdContact.cpp
- TriangleMeshTriangleMeshDcdContact.cpp
+ SphereDoubleSidedPlaneContact.cpp
+ SpherePlaneContact.cpp
+ SphereSphereContact.cpp
+ TriangleMeshParticlesContact.cpp
+ TriangleMeshPlaneContact.cpp
+ TriangleMeshSurfaceMeshContact.cpp
+ TriangleMeshTriangleMeshContact.cpp
)
set(SURGSIM_COLLISION_HEADERS
- BoxCapsuleDcdContact.h
- BoxDoubleSidedPlaneDcdContact.h
- BoxPlaneDcdContact.h
- BoxSphereDcdContact.h
- CapsuleSphereDcdContact.h
+ BoxCapsuleContact.h
+ BoxDoubleSidedPlaneContact.h
+ BoxPlaneContact.h
+ BoxSphereContact.h
+ CapsuleSphereContact.h
+ CcdDcdCollision.h
CollisionPair.h
+ CompoundShapeContact.h
ContactCalculation.h
- DcdCollision.h
+ ContactFilter.h
DefaultContactCalculation.h
- OctreeDcdContact.h
+ ElementContactFilter.h
+ OctreeCapsuleContact.h
+ OctreeContact.h
+ OctreeDoubleSidedPlaneContact.h
+ OctreePlaneContact.h
+ OctreeSphereContact.h
Representation.h
+ SegmentMeshTriangleMeshContact.h
+ SegmentSegmentCcdIntervalCheck.h
+ SegmentSegmentCcdMovingContact.h
+ SegmentSegmentCcdStaticContact.h
+ SegmentSelfContact.h
ShapeCollisionRepresentation.h
- SphereDoubleSidedPlaneDcdContact.h
- SpherePlaneDcdContact.h
- SphereSphereDcdContact.h
- TriangleMeshPlaneDcdContact.h
- TriangleMeshTriangleMeshDcdContact.h
+ ShapeShapeContactCalculation.h
+ SphereDoubleSidedPlaneContact.h
+ SpherePlaneContact.h
+ SphereSphereContact.h
+ TriangleMeshParticlesContact.h
+ TriangleMeshPlaneContact.h
+ TriangleMeshSurfaceMeshContact.h
+ TriangleMeshTriangleMeshContact.h
)
+surgsim_create_library_header(Collision.h "${SURGSIM_COLLISION_HEADERS}")
surgsim_add_library(
SurgSimCollision
"${SURGSIM_COLLISION_SOURCES}"
"${SURGSIM_COLLISION_HEADERS}"
- "SurgSim/Collision"
)
SET(LIBS
@@ -70,6 +99,10 @@ target_link_libraries(SurgSimCollision ${LIBS}
if(BUILD_TESTING)
add_subdirectory(UnitTests)
+
+ if(BUILD_PERFORMANCE_TESTING)
+ add_subdirectory(PerformanceTests)
+ endif()
endif()
# Put SurgSimCollision into folder "Collision"
diff --git a/SurgSim/Collision/CapsuleSphereContact.cpp b/SurgSim/Collision/CapsuleSphereContact.cpp
new file mode 100644
index 0000000..afb3abf
--- /dev/null
+++ b/SurgSim/Collision/CapsuleSphereContact.cpp
@@ -0,0 +1,76 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/CapsuleSphereContact.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
+{
+
+std::pair<int, int> CapsuleSphereContact::getShapeTypes()
+{
+ return std::pair<int, int>(SurgSim::Math::SHAPE_TYPE_CAPSULE, SurgSim::Math::SHAPE_TYPE_SPHERE);
+}
+
+std::list<std::shared_ptr<Contact>> CapsuleSphereContact::calculateDcdContact(
+ const Math::CapsuleShape& capsule, const Math::RigidTransform3d& capsulePose,
+ const Math::SphereShape& sphere, const Math::RigidTransform3d& spherePose) const
+{
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ Vector3d sphereCenter(spherePose.translation());
+ Vector3d globalTop(capsulePose * capsule.topCenter());
+ Vector3d globalBottom(capsulePose * 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(
+ capsulePose.inverse() * (result - normal * capsule.getRadius()));
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ spherePose.inverse() * (sphereCenter + normal * sphere.getRadius()));
+
+ contacts.emplace_back(std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, depth, 1.0,
+ Vector3d::Zero(), normal, penetrationPoints));
+ }
+
+ return contacts;
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/CapsuleSphereContact.h b/SurgSim/Collision/CapsuleSphereContact.h
new file mode 100644
index 0000000..52e3970
--- /dev/null
+++ b/SurgSim/Collision/CapsuleSphereContact.h
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_CAPSULESPHERECONTACT_H
+#define SURGSIM_COLLISION_CAPSULESPHERECONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Math/CapsuleShape.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between a capsule and a sphere
+class CapsuleSphereContact : public ShapeShapeContactCalculation<Math::CapsuleShape, Math::SphereShape>
+{
+public:
+ using ContactCalculation::calculateDcdContact;
+
+ std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const Math::CapsuleShape& capsule,
+ const Math::RigidTransform3d& capsulePose,
+ const Math::SphereShape& sphere,
+ const Math::RigidTransform3d& spherePose) const override;
+
+ std::pair<int, int> getShapeTypes() override;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/CapsuleSphereDcdContact.cpp b/SurgSim/Collision/CapsuleSphereDcdContact.cpp
deleted file mode 100644
index cc7b1cf..0000000
--- a/SurgSim/Collision/CapsuleSphereDcdContact.cpp
+++ /dev/null
@@ -1,78 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index bb19058..0000000
--- a/SurgSim/Collision/CapsuleSphereDcdContact.h
+++ /dev/null
@@ -1,52 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/CcdDcdCollision.h b/SurgSim/Collision/CcdDcdCollision.h
new file mode 100644
index 0000000..54cd2c8
--- /dev/null
+++ b/SurgSim/Collision/CcdDcdCollision.h
@@ -0,0 +1,41 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_CCDDCDCOLLISION_H
+#define SURGSIM_COLLISION_CCDDCDCOLLISION_H
+
+#include "SurgSim/Collision/BoxCapsuleContact.h"
+#include "SurgSim/Collision/BoxDoubleSidedPlaneContact.h"
+#include "SurgSim/Collision/BoxPlaneContact.h"
+#include "SurgSim/Collision/BoxSphereContact.h"
+#include "SurgSim/Collision/CapsuleSphereContact.h"
+#include "SurgSim/Collision/CompoundShapeContact.h"
+#include "SurgSim/Collision/ContactCalculation.h"
+#include "SurgSim/Collision/DefaultContactCalculation.h"
+#include "SurgSim/Collision/OctreeCapsuleContact.h"
+#include "SurgSim/Collision/OctreeDoubleSidedPlaneContact.h"
+#include "SurgSim/Collision/OctreePlaneContact.h"
+#include "SurgSim/Collision/OctreeSphereContact.h"
+#include "SurgSim/Collision/SegmentMeshTriangleMeshContact.h"
+#include "SurgSim/Collision/SegmentSelfContact.h"
+#include "SurgSim/Collision/SphereDoubleSidedPlaneContact.h"
+#include "SurgSim/Collision/SpherePlaneContact.h"
+#include "SurgSim/Collision/SphereSphereContact.h"
+#include "SurgSim/Collision/TriangleMeshParticlesContact.h"
+#include "SurgSim/Collision/TriangleMeshPlaneContact.h"
+#include "SurgSim/Collision/TriangleMeshSurfaceMeshContact.h"
+#include "SurgSim/Collision/TriangleMeshTriangleMeshContact.h"
+
+#endif // SURGSIM_COLLISION_CCDDCDCOLLISION_H
diff --git a/SurgSim/Collision/CollisionPair.cpp b/SurgSim/Collision/CollisionPair.cpp
index 92c7dd8..43123fd 100644
--- a/SurgSim/Collision/CollisionPair.cpp
+++ b/SurgSim/Collision/CollisionPair.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -13,15 +13,13 @@
// 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
@@ -32,11 +30,9 @@ CollisionPair::CollisionPair()
}
CollisionPair::CollisionPair(const std::shared_ptr<Representation>& first,
- const std::shared_ptr<Representation>& second) :
- m_representations(first, second), m_isSwapped(false)
+ const std::shared_ptr<Representation>& second)
{
- SURGSIM_ASSERT(first != second) << "Collision Representation cannot collide with itself";
- SURGSIM_ASSERT(first != nullptr && second != nullptr) << "Collision Representation cannot be null";
+ setRepresentations(first, second);
}
CollisionPair::~CollisionPair()
@@ -45,9 +41,8 @@ CollisionPair::~CollisionPair()
}
void CollisionPair::setRepresentations(const std::shared_ptr<Representation>& first,
- const std::shared_ptr<Representation>& second)
+ 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
@@ -55,14 +50,38 @@ void CollisionPair::setRepresentations(const std::shared_ptr<Representation>& fi
m_representations.first = first;
m_representations.second = second;
m_isSwapped = false;
+
+ if (m_representations.first == m_representations.second)
+ {
+ m_type = m_representations.first->getSelfCollisionDetectionType();
+ }
+ else if (m_representations.first->getCollisionDetectionType() == COLLISION_DETECTION_TYPE_NONE ||
+ m_representations.second->getCollisionDetectionType() == COLLISION_DETECTION_TYPE_NONE)
+ {
+ m_type = COLLISION_DETECTION_TYPE_NONE;
+ }
+ else if (m_representations.first->getCollisionDetectionType() == COLLISION_DETECTION_TYPE_CONTINUOUS &&
+ m_representations.second->getCollisionDetectionType() == COLLISION_DETECTION_TYPE_CONTINUOUS)
+ {
+ m_type = COLLISION_DETECTION_TYPE_CONTINUOUS;
+ }
+ else
+ {
+ m_type = COLLISION_DETECTION_TYPE_DISCRETE;
+ }
}
const std::pair<std::shared_ptr<Representation>, std::shared_ptr<Representation>>&
- CollisionPair::getRepresentations() const
+ CollisionPair::getRepresentations() const
{
return m_representations;
}
+CollisionDetectionType CollisionPair::getType() const
+{
+ return m_type;
+}
+
std::shared_ptr<Representation> CollisionPair::getFirst() const
{
return m_representations.first;
@@ -78,34 +97,41 @@ 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)
+void CollisionPair::addCcdContact(const double& depth, const double& time, 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));
+ SURGSIM_ASSERT(getType() == COLLISION_DETECTION_TYPE_CONTINUOUS)
+ << "Can only add CCD contacts to a CollisionPair that is COLLISION_DETECTION_TYPE_CONTINUOUS";
+ addContact(std::make_shared<Contact>(COLLISION_DETECTION_TYPE_CONTINUOUS, depth, time, contactPoint, normal,
+ penetrationPoints));
}
-void CollisionPair::addContact(const double& depth,
- const SurgSim::Math::Vector3d& normal,
- const std::pair<Location, Location>& penetrationPoints)
+void CollisionPair::addDcdContact(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));
+ SURGSIM_ASSERT(getType() == COLLISION_DETECTION_TYPE_DISCRETE)
+ << "Can only add DCD contacts to a CollisionPair that is COLLISION_DETECTION_TYPE_DISCRETE";
+ addContact(std::make_shared<Contact>(COLLISION_DETECTION_TYPE_DISCRETE, depth, 1.0, Math::Vector3d::Zero(), normal,
+ penetrationPoints));
}
void CollisionPair::addContact(const std::shared_ptr<Contact>& contact)
{
+ SURGSIM_ASSERT(contact->type == getType())
+ << "Only contacts with the same CollisionDetectionType can be added to this CollisionPair.";
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
+void CollisionPair::updateRepresentations()
+{
+ for (auto& contact : m_contacts)
+ {
+ m_representations.first->addContact(m_representations.second, contact);
+ m_representations.second->addContact(m_representations.first, contact->makeComplimentary());
+ }
+}
+
+std::list<std::shared_ptr<Contact>>& CollisionPair::getContacts()
{
return m_contacts;
}
@@ -117,7 +143,7 @@ void CollisionPair::clearContacts()
void CollisionPair::swapRepresentations()
{
- SURGSIM_ASSERT(! hasContacts()) << "Can't swap representations after contacts have already been calculated";
+ 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);
}
@@ -127,6 +153,14 @@ bool CollisionPair::isSwapped() const
return m_isSwapped;
}
+bool CollisionPair::mayIntersect() const
+{
+ const auto& one = m_representations.first->getBoundingBox();
+ const auto& two = m_representations.second->getBoundingBox();
+
+ return (one.isEmpty() || two.isEmpty() || SurgSim::Math::doAabbIntersect(one, two));
+}
+
}; // namespace Collision
}; // namespace SurgSim
diff --git a/SurgSim/Collision/CollisionPair.h b/SurgSim/Collision/CollisionPair.h
index 15751b9..d5f2ff9 100644
--- a/SurgSim/Collision/CollisionPair.h
+++ b/SurgSim/Collision/CollisionPair.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,37 +16,60 @@
#ifndef SURGSIM_COLLISION_COLLISIONPAIR_H
#define SURGSIM_COLLISION_COLLISIONPAIR_H
-#include <memory>
#include <list>
+#include <memory>
+
+#include "SurgSim/Collision/Representation.h"
#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,
+struct Contact
+{
+ Contact(const CollisionDetectionType& newType,
+ const double& newDepth,
+ const double& newTime,
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)
- {
- };
+ SurgSim::DataStructures::Location>& newPenetrationPoints) :
+ type(newType), depth(newDepth), time(newTime), contact(newContact),
+ normal(newNormal), penetrationPoints(newPenetrationPoints), force(SurgSim::Math::Vector3d::Zero())
+ {
+ }
+ std::shared_ptr<Contact> makeComplimentary()
+ {
+ auto complimentary = std::make_shared<Contact>(type, depth, time, contact,
+ -normal, std::make_pair(penetrationPoints.second, penetrationPoints.first));
+ complimentary->force = -force;
+ return complimentary;
+ }
+ bool operator==(const Contact& contact) const
+ {
+ return type == contact.type &&
+ std::abs(time - contact.time) < 1e-8 &&
+ penetrationPoints.first.isApprox(contact.penetrationPoints.first) &&
+ penetrationPoints.second.isApprox(contact.penetrationPoints.second) &&
+ normal.isApprox(contact.normal);
+ }
+ CollisionDetectionType type; ///< What collision algorithm class was used to get the contact
double depth; ///< What is the penetration depth for the representation
+ double time; ///< What is the time of the collision, CCD only
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
+ SurgSim::DataStructures::Location> penetrationPoints; ///< The deepest point inside the opposing object
+ SurgSim::Math::Vector3d force; ///< The reaction force to correct this contact.
};
/// Collision Pair class, it signifies a pair of items that should be checked with the
@@ -74,7 +97,11 @@ public:
/// 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;
+ getRepresentations() const;
+
+ /// Get the collision detection type for this pair
+ /// \return The collision detection type
+ CollisionDetectionType getType() const;
/// \return The representation considered to be the first
std::shared_ptr<Representation> getFirst() const;
@@ -85,32 +112,37 @@ public:
/// \return true if there are any contacts assigned to the pair, false otherwise
bool hasContacts() const;
- /// Adds a contact to the collision pair.
+ /// Adds a CCD contact to the collision pair.
/// \param depth The depth of the intersection.
- /// \param contactPoint The contact point, between the two bodies.
+ /// \param time The actual time of contact as determined by the CCD algorithm.
+ /// \param contactPoint The contact point, between the two bodies at time "time"
/// \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.
+ void addCcdContact(const double& depth,
+ const double& time,
+ const SurgSim::Math::Vector3d& contactPoint,
+ const SurgSim::Math::Vector3d& normal,
+ const std::pair<SurgSim::DataStructures::Location,
+ SurgSim::DataStructures::Location>& penetrationPoints);
+
+ /// Adds a DCD 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);
+ void addDcdContact(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);
+ /// Update the representations by adding the contacts to them.
+ void updateRepresentations();
+
/// \return All the contacts.
- const std::list<std::shared_ptr<Contact>>& getContacts() const;
+ std::list<std::shared_ptr<Contact>>& getContacts();
/// Reset clear the list of contacts, invalidating all the contacts
void clearContacts();
@@ -122,11 +154,16 @@ public:
/// \return true if swapped, false if not.
bool isSwapped() const;
-private:
+ /// \return whether the two represenations might have an intersection
+ /// \note The bounding boxes are taken, if the bounding box is empty it is always considered for collision
+ bool mayIntersect() const;
+private:
/// Pair of objects that are colliding
- std::pair<std::shared_ptr<Representation>,
- std::shared_ptr<Representation>> m_representations;
+ std::pair<std::shared_ptr<Representation>, std::shared_ptr<Representation>> m_representations;
+
+ /// Collision detection type for this pair
+ CollisionDetectionType m_type;
/// List of current contacts
std::list<std::shared_ptr<Contact>> m_contacts;
@@ -138,4 +175,16 @@ private:
}; // namespace Collision
}; // namespace SurgSim
+template <typename charT, typename traits>
+std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& out,
+ const SurgSim::Collision::Contact& contact)
+{
+ out << "Normal: " << contact.normal.transpose() << std::endl;
+ out << "Depth: " << contact.depth << std::endl;
+ out << "Time: " << contact.time << std::endl;
+ out << "Penetration Point 1 :" << contact.penetrationPoints.first << std::endl;
+ out << "Penetration Point 2 :" << contact.penetrationPoints.second << std::endl;
+
+ return out;
+}
#endif
diff --git a/SurgSim/Collision/CompoundShapeContact.cpp b/SurgSim/Collision/CompoundShapeContact.cpp
new file mode 100644
index 0000000..a9aac00
--- /dev/null
+++ b/SurgSim/Collision/CompoundShapeContact.cpp
@@ -0,0 +1,140 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Collision/CollisionPair.h"
+#include "SurgSim/Collision/CompoundShapeContact.h"
+#include "SurgSim/Math/CompoundShape.h"
+
+namespace SurgSim
+{
+
+namespace Collision
+{
+
+CompoundShapeContact::CompoundShapeContact(const std::pair<int, int>& types) : m_types(types)
+{
+
+}
+
+std::pair<int, int> CompoundShapeContact::getShapeTypes()
+{
+ return m_types;
+}
+
+std::list<std::shared_ptr<Contact>> CompoundShapeContact::doCalculateDcdContact(
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape1,
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape2)
+{
+ typedef Math::PosedShape<std::shared_ptr<Math::Shape>> PosedShape;
+
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ const auto& calculations = ContactCalculation::getDcdContactTable();
+
+ // Shape1 is compound shape
+ const auto& compoundShape = std::static_pointer_cast<Math::CompoundShape>(posedShape1.getShape());
+
+ SURGSIM_ASSERT(compoundShape->getType() == posedShape1.getShape()->getType()) <<
+ "Invalid static cast to compound shape";
+
+ Math::RigidTransform3d inversePose = posedShape1.getPose().inverse();
+ size_t index = 0;
+ for (const Math::CompoundShape::SubShape& subShape : compoundShape->getShapes())
+ {
+ const auto& calculation = calculations[subShape.first->getType()][posedShape2.getShape()->getType()];
+ auto relativePose = inversePose * subShape.second;
+ std::list<std::shared_ptr<Contact>> localContacts = calculation->calculateDcdContact(
+ PosedShape(subShape.first, subShape.second), posedShape2);
+
+ for (auto& contact : localContacts)
+ {
+ auto& locations = contact->penetrationPoints.first;
+ locations.index = index;
+ if (locations.rigidLocalPosition.hasValue())
+ {
+ locations.rigidLocalPosition.setValue(relativePose * locations.rigidLocalPosition.getValue());
+ }
+ }
+
+ contacts.splice(contacts.end(), localContacts);
+ index++;
+ }
+ return contacts;
+}
+
+std::list<std::shared_ptr<SurgSim::Collision::Contact>> CompoundShapeContact::doCalculateCcdContact(
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& shape1,
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& shape2)
+{
+ typedef Math::PosedShape<std::shared_ptr<Math::Shape>> PosedShape;
+ typedef Math::PosedShapeMotion<std::shared_ptr<Math::Shape>> Motionshape;
+
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ const auto& calculations = ContactCalculation::getCcdContactTable();
+
+ // Shape1 is compound shape
+ const auto& startShape = std::static_pointer_cast<Math::CompoundShape>(shape1.first.getShape());
+ const auto& endShape = std::static_pointer_cast<Math::CompoundShape>(shape1.second.getShape());
+
+
+ // Get Motion Shapes from left hand side
+ // For each Motion Shape run that shape and the opposing side through the CCD contact calc
+ SURGSIM_ASSERT(startShape->getType() == shape1.first.getShape()->getType()) <<
+ "Invalid static cast to compound shape";
+ SURGSIM_ASSERT(endShape->getType() == shape1.second.getShape()->getType()) <<
+ "Invalid static cast to compound shape";
+
+
+ size_t numShapes = startShape->getNumShapes();
+ for (size_t index = 0; index < numShapes; ++index)
+ {
+ auto posedSubshape1 = PosedShape(startShape->getShape(index), startShape->getPose(index));
+ auto type = posedSubshape1.getShape()->getType();
+ auto posedSubshape2 = PosedShape(endShape->getShape(index), endShape->getPose(index));
+
+ auto motionSubShape = Motionshape(posedSubshape1, posedSubshape2);
+
+ const auto& calculation = calculations[type][shape2.second.getShape()->getType()];
+
+ std::list<std::shared_ptr<Contact>> localContacts = calculation->calculateCcdContact(
+ motionSubShape, shape2);
+ for (auto& contact : localContacts)
+ {
+ auto t = contact->time;
+ Math::RigidTransform3d inverseGlobal =
+ Math::interpolate(shape1.first.getPose(), shape1.second.getPose(), t).inverse();
+
+ Math::RigidTransform3d relativePose =
+ inverseGlobal * Math::interpolate(motionSubShape.first.getPose(), motionSubShape.second.getPose(), t);
+
+ auto& locations = contact->penetrationPoints.first;
+ locations.index = index;
+ if (locations.rigidLocalPosition.hasValue())
+ {
+ locations.rigidLocalPosition.setValue(relativePose * locations.rigidLocalPosition.getValue());
+ }
+ }
+
+ contacts.splice(contacts.end(), localContacts);
+ }
+
+ return contacts;
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Collision/CompoundShapeContact.h b/SurgSim/Collision/CompoundShapeContact.h
new file mode 100644
index 0000000..83afb18
--- /dev/null
+++ b/SurgSim/Collision/CompoundShapeContact.h
@@ -0,0 +1,51 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_COMPOUNDSHAPECONTACT_H
+#define SURGSIM_COLLISION_COMPOUNDSHAPECONTACT_H
+
+#include "SurgSim/Collision/ContactCalculation.h"
+
+namespace SurgSim
+{
+
+namespace Collision
+{
+
+class CompoundShapeContact : public ContactCalculation
+{
+public:
+ /// Constructor
+ explicit CompoundShapeContact(const std::pair<int, int>& types);
+
+protected:
+ std::pair<int, int> getShapeTypes() override;
+
+ std::list<std::shared_ptr<Contact>> doCalculateDcdContact(
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape1,
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape2) override;
+
+ std::list<std::shared_ptr<SurgSim::Collision::Contact>> doCalculateCcdContact(
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& shape1,
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& shape2) override;
+
+ /// Local shape types for this instance, these can be set to match the expected shapes
+ std::pair<int, int> m_types;
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Collision/ContactCalculation.cpp b/SurgSim/Collision/ContactCalculation.cpp
index 2de9ec0..945326d 100644
--- a/SurgSim/Collision/ContactCalculation.cpp
+++ b/SurgSim/Collision/ContactCalculation.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -13,16 +13,23 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "SurgSim/Collision/ContactCalculation.h"
+#include <thread>
+#include "SurgSim/Collision/CcdDcdCollision.h"
+#include "SurgSim/Collision/ContactCalculation.h"
+#include "SurgSim/Collision/DefaultContactCalculation.h"
#include "SurgSim/Collision/Representation.h"
-#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Framework/Log.h"
namespace SurgSim
{
namespace Collision
{
+ContactCalculation::TableType ContactCalculation::m_contactDcdCalculations;
+ContactCalculation::TableType ContactCalculation::m_contactCcdCalculations;
+std::once_flag ContactCalculation::m_initializationFlag;
+
ContactCalculation::ContactCalculation()
{
}
@@ -31,32 +38,261 @@ ContactCalculation::~ContactCalculation()
{
}
+void ContactCalculation::registerDcdContactCalculation(const std::shared_ptr<ContactCalculation>& calculation)
+{
+ std::call_once(m_initializationFlag, ContactCalculation::initializeTables);
+
+ privateDcdRegister(calculation, calculation->getShapeTypes());
+}
+
+void ContactCalculation::registerCcdContactCalculation(const std::shared_ptr<ContactCalculation>& calculation)
+{
+ std::call_once(m_initializationFlag, ContactCalculation::initializeTables);
+
+ privateCcdRegister(calculation, calculation->getShapeTypes());
+}
+
+const ContactCalculation::TableType& ContactCalculation::getDcdContactTable()
+{
+ std::call_once(m_initializationFlag, ContactCalculation::initializeTables);
+
+ return m_contactDcdCalculations;
+}
+
+const ContactCalculation::TableType& ContactCalculation::getCcdContactTable()
+{
+ std::call_once(m_initializationFlag, ContactCalculation::initializeTables);
+
+ return m_contactCcdCalculations;
+}
+
void ContactCalculation::calculateContact(std::shared_ptr<CollisionPair> pair)
{
- std::pair<int,int> shapeTypes = getShapeTypes();
+ doCalculateContact(pair);
+}
+
+std::list<std::shared_ptr<Contact>> ContactCalculation::calculateDcdContact(
+ const Math::PosedShape<std::shared_ptr<Math::Shape>> posedShape1,
+ const Math::PosedShape<std::shared_ptr<Math::Shape>> posedShape2)
+{
+ auto types = getShapeTypes();
+ auto incoming = std::make_pair(posedShape1.getShape()->getType(), posedShape2.getShape()->getType());
+ if (incoming == types)
+ {
+ return doCalculateDcdContact(posedShape1, posedShape2);
+ }
+
+ if (incoming.first == types.second && incoming.second == types.first)
+ {
+ auto contacts = doCalculateDcdContact(posedShape2, posedShape1);
+ for (const auto& contact : contacts)
+ {
+ contact->normal = -contact->normal;
+ contact->force = -contact->force;
+ std::swap(contact->penetrationPoints.first, contact->penetrationPoints.second);
+ }
+ return contacts;
+ }
+
+ if (types.first != Math::SHAPE_TYPE_NONE && types.second != Math::SHAPE_TYPE_NONE)
+ {
+ SURGSIM_FAILURE() << "Incorrect shape type for this calculation expected "
+ << types.first << ", " << types.second
+ << " received " << incoming.first << ", " << incoming.second << ".";
+ }
+
+ return std::list<std::shared_ptr<Contact>>();
+}
+
+std::list<std::shared_ptr<SurgSim::Collision::Contact>> ContactCalculation::calculateCcdContact(
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>> posedShapeMotion1,
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>> posedShapeMotion2)
+{
+ auto types = getShapeTypes();
+ auto incoming = std::make_pair(posedShapeMotion1.first.getShape()->getType(),
+ posedShapeMotion2.first.getShape()->getType());
+ if (incoming == types)
+ {
+ return doCalculateCcdContact(posedShapeMotion1, posedShapeMotion2);
+ }
+
+ if (incoming.first == types.second && incoming.second == types.first)
+ {
+ auto contacts = doCalculateCcdContact(posedShapeMotion2, posedShapeMotion1);
+ for (const auto& contact : contacts)
+ {
+ contact->normal = -contact->normal;
+ contact->force = -contact->force;
+ std::swap(contact->penetrationPoints.first, contact->penetrationPoints.second);
+ }
+ return contacts;
+ }
+
+ if (types.first != Math::SHAPE_TYPE_NONE && types.second != Math::SHAPE_TYPE_NONE)
+ {
+ SURGSIM_FAILURE() << "Incorrect shape type for this calculation expected "
+ << types.first << ", " << types.second
+ << " received " << incoming.first << ", " << incoming.second << ".";
+ }
+
+ return std::list<std::shared_ptr<Contact>>();
+}
+
+void ContactCalculation::doCalculateContact(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)
+ secondShapeType == shapeTypes.first)
{
pair->swapRepresentations();
std::swap(firstShapeType, secondShapeType);
}
- if(shapeTypes.first != SurgSim::Math::SHAPE_TYPE_NONE)
+ if (shapeTypes.first != SurgSim::Math::SHAPE_TYPE_NONE)
{
SURGSIM_ASSERT(firstShapeType == shapeTypes.first) <<
- "First Object, wrong type of object" << firstShapeType;
+ "First Object, wrong type of object" << firstShapeType;
}
- if(shapeTypes.second != SurgSim::Math::SHAPE_TYPE_NONE)
+ if (shapeTypes.second != SurgSim::Math::SHAPE_TYPE_NONE)
{
SURGSIM_ASSERT(secondShapeType == shapeTypes.second) <<
- "Second Object, wrong type of object" << secondShapeType;
+ "Second Object, wrong type of object" << secondShapeType;
}
- doCalculateContact(pair);
+ std::shared_ptr<Math::Shape> shape1 = pair->getFirst()->getPosedShape();
+
+ std::shared_ptr<Math::Shape> shape2 = pair->getSecond()->getPosedShape();
+
+ std::list<std::shared_ptr<Contact>> contacts;
+ if (pair->getType() == Collision::CollisionDetectionType::COLLISION_DETECTION_TYPE_DISCRETE)
+ {
+ Math::PosedShape<std::shared_ptr<Math::Shape>> posedShape1(shape1, pair->getFirst()->getPose());
+ Math::PosedShape<std::shared_ptr<Math::Shape>> posedShape2(shape2, pair->getSecond()->getPose());
+ contacts = doCalculateDcdContact(posedShape1, posedShape2);
+ }
+ else if (pair->getType() == Collision::CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS)
+ {
+ contacts = doCalculateCcdContact(
+ pair->getFirst()->getPosedShapeMotion(),
+ pair->getSecond()->getPosedShapeMotion());
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Invalid collision detection type, neither discrete nor continuous";
+ }
+
+ for (auto& contact : contacts)
+ {
+ pair->addContact(contact);
+ }
+}
+
+std::list<std::shared_ptr<Contact>> ContactCalculation::doCalculateDcdContact(
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape1,
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape2)
+{
+ SURGSIM_FAILURE() << "Not implemented";
+ return std::list<std::shared_ptr<Contact>>();
+}
+
+std::list<std::shared_ptr<Contact>> ContactCalculation::doCalculateCcdContact(
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& posedShapeMotion1,
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& posedShapeMotion2)
+{
+ SURGSIM_FAILURE() << "Not implemented";
+ return std::list<std::shared_ptr<Contact>>();
+}
+
+void ContactCalculation::initializeTables()
+{
+ // Fill up both tables with default empty contact calculation
+ for (int i = 0; i < SurgSim::Math::SHAPE_TYPE_COUNT; ++i)
+ {
+ for (int j = 0; j < SurgSim::Math::SHAPE_TYPE_COUNT; ++j)
+ {
+ m_contactDcdCalculations[i][j].reset(new Collision::DefaultContactCalculation(false));
+ m_contactCcdCalculations[i][j].reset(new Collision::DefaultContactCalculation(false));
+ }
+ }
+
+ // Fill up the Dcd contact calculation table
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::BoxCapsuleContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::BoxDoubleSidedPlaneContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::BoxPlaneContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::BoxSphereContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::CapsuleSphereContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::OctreeCapsuleContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::OctreeDoubleSidedPlaneContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::OctreePlaneContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::OctreeSphereContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::SegmentMeshTriangleMeshContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::SphereSphereContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::SphereDoubleSidedPlaneContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::SpherePlaneContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::TriangleMeshParticlesContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::TriangleMeshPlaneContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::TriangleMeshSurfaceMeshContact>());
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::TriangleMeshTriangleMeshContact>());
+
+ const std::array<int, Math::SHAPE_TYPE_COUNT> allshapes =
+ {
+ Math::SHAPE_TYPE_BOX,
+ Math::SHAPE_TYPE_CAPSULE,
+ Math::SHAPE_TYPE_CYLINDER,
+ Math::SHAPE_TYPE_DOUBLESIDEDPLANE,
+ Math::SHAPE_TYPE_MESH,
+ Math::SHAPE_TYPE_OCTREE,
+ Math::SHAPE_TYPE_PARTICLES,
+ Math::SHAPE_TYPE_PLANE,
+ Math::SHAPE_TYPE_SPHERE,
+ Math::SHAPE_TYPE_SURFACEMESH,
+ Math::SHAPE_TYPE_SEGMENTMESH,
+ Math::SHAPE_TYPE_COMPOUNDSHAPE
+ };
+
+ for (auto type : allshapes)
+ {
+ ContactCalculation::privateDcdRegister(std::make_shared<Collision::CompoundShapeContact>(
+ std::make_pair(Math::SHAPE_TYPE_COMPOUNDSHAPE, type)));
+ }
+
+ ContactCalculation::privateCcdRegister(std::make_shared<Collision::SegmentSelfContact>());
+ ContactCalculation::privateCcdRegister(std::make_shared<Collision::SegmentMeshTriangleMeshContact>());
+
+ ContactCalculation::privateCcdRegister(std::make_shared<Collision::CompoundShapeContact>(
+ std::make_pair(Math::SHAPE_TYPE_COMPOUNDSHAPE, Math::SHAPE_TYPE_SEGMENTMESH)));
+}
+
+void ContactCalculation::privateDcdRegister(
+ const std::shared_ptr<ContactCalculation>& calculation)
+{
+ privateDcdRegister(calculation, calculation->getShapeTypes());
+}
+
+void ContactCalculation::privateDcdRegister(
+ const std::shared_ptr<ContactCalculation>& calculation,
+ const std::pair<int, int>& types)
+{
+ m_contactDcdCalculations[types.first][types.second] = calculation;
+ m_contactDcdCalculations[types.second][types.first] = calculation;
+}
+
+void ContactCalculation::privateCcdRegister(
+ const std::shared_ptr<ContactCalculation>& calculation)
+{
+ privateCcdRegister(calculation, calculation->getShapeTypes());
+}
+
+void ContactCalculation::privateCcdRegister(
+ const std::shared_ptr<ContactCalculation>& calculation,
+ const std::pair<int, int>& types)
+{
+ m_contactCcdCalculations[types.first][types.second] = calculation;
+ m_contactCcdCalculations[types.second][types.first] = calculation;
}
}; // namespace Collision
diff --git a/SurgSim/Collision/ContactCalculation.h b/SurgSim/Collision/ContactCalculation.h
index 39aa6b5..bb8d8ba 100644
--- a/SurgSim/Collision/ContactCalculation.h
+++ b/SurgSim/Collision/ContactCalculation.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -17,45 +17,122 @@
#define SURGSIM_COLLISION_CONTACTCALCULATION_H
#include <memory>
+#include <mutex>
#include "SurgSim/Collision/CollisionPair.h"
-
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Shape.h"
namespace SurgSim
{
+
+namespace Math
+{
+class Shape;
+}
+
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.
+/// Base class responsible for calculating contact data between two objects.
+/// It is used for determining whether two objects intersect. If there is
+/// contact, new Contacts are calculated.
+/// \sa Contact
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.
+ /// Calculate the contacts for a given pair
+ /// \param pair A CollisionPair that is under consideration, new contacts will be added to this pair
void calculateContact(std::shared_ptr<CollisionPair> pair);
+ /// Calculate the dcd contacts between two posed/transformed shapes
+ /// \param posedShape1, posedShape2 The two posed shape to consider for this dcd contact calculation
+ /// \return a list of contacts between the two given posed shapes
+ std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const Math::PosedShape<std::shared_ptr<Math::Shape>> posedShape1,
+ const Math::PosedShape<std::shared_ptr<Math::Shape>> posedShape2);
+
+ /// Calculate the ccd contacts between two posed/transformed shapes
+ /// \param posedShapeMotion1, posedShapeMotion2 The two posed shape motion to calculate ccd contact for
+ /// \return a list of ccd contacts between the two given shapes
+ /// \note The contact information is related to the end of the time range, so solving these contact will
+ /// \note solve the collision at the end of the time range.
+ std::list<std::shared_ptr<Contact>> calculateCcdContact(
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>> posedShapeMotion1,
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>> posedShapeMotion2);
+
/// 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;
+ virtual std::pair<int, int> getShapeTypes() = 0;
+
+ /// Register an instance of a contact calculation in the table
+ /// \param calculation The calculation to be registered
+ static void registerDcdContactCalculation(const std::shared_ptr<ContactCalculation>& calculation);
+ /// Register an instance of a contact calculation in the table
+ /// \param calculation The calculation to be registered
+ static void registerCcdContactCalculation(const std::shared_ptr<ContactCalculation>& calculation);
+
+ typedef
+ std::array<std::array<std::shared_ptr<ContactCalculation>, Math::SHAPE_TYPE_COUNT>, Math::SHAPE_TYPE_COUNT>
+ TableType;
+
+ static const TableType& getDcdContactTable();
+ static const TableType& getCcdContactTable();
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;
+ virtual void doCalculateContact(std::shared_ptr<CollisionPair> pair);
+
+ /// Virtual function receives the call from the public interface, usually will type the
+ /// shapes statically to their known types and then execute a specific contact calculation
+ /// between the two shapes
+ /// \param posedShape1, posedShape2 The two posed shapes to calculate dcd contact for
+ /// \return a list of dcd contacts between the two given posed shapes
+ virtual std::list<std::shared_ptr<Contact>> doCalculateDcdContact(
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape1,
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape2);
+
+ /// Virtual function receives the call from the public interface, usually will type the
+ /// shapes statically to their known types and then execute a specific contact calculation
+ /// between the two shapes
+ /// \param posedShapeMotion1, posedShapeMotion2 The two posed shapes motion to calculate ccd contact for
+ /// \return a list of ccd contacts between the two given posed shapes motion
+ virtual std::list<std::shared_ptr<Contact>> doCalculateCcdContact(
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& posedShapeMotion1,
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& posedShapeMotion2);
+
+ /// Statically initialize the tables, used via call once
+ static void initializeTables();
+
+ ///@{
+ /// registration to call at static scope (does not protect the initialization via call_once)
+ /// Mirroring the public functions
+ static void privateDcdRegister(
+ const std::shared_ptr<ContactCalculation>& calculation,
+ const std::pair<int, int>& types);
+ static void privateDcdRegister(const std::shared_ptr<ContactCalculation>& calculation);
+ ///@}
+
+ ///@{
+ /// registration to call at static scope (does not protect the initialization via call_once)
+ /// Mirroring the public functions
+ static void privateCcdRegister(
+ const std::shared_ptr<ContactCalculation>& calculation,
+ const std::pair<int, int>& types);
+ static void privateCcdRegister(const std::shared_ptr<ContactCalculation>& calculation);
+ ///@}
+ static TableType m_contactDcdCalculations; ///< Static table of Dcd contact calculations
+ static TableType m_contactCcdCalculations; ///< Static table of Ccd contact calculations
+ static std::once_flag m_initializationFlag; ///< Flag used for initialization.
};
}; // namespace Collision
diff --git a/SurgSim/Collision/ContactFilter.cpp b/SurgSim/Collision/ContactFilter.cpp
new file mode 100644
index 0000000..aab0714
--- /dev/null
+++ b/SurgSim/Collision/ContactFilter.cpp
@@ -0,0 +1,41 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/ContactFilter.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+ContactFilter::ContactFilter(const std::string& name) :
+ Framework::Component(name)
+{
+
+}
+
+void ContactFilter::filterContacts(const std::shared_ptr<Physics::PhysicsManagerState>& state,
+ const std::shared_ptr<CollisionPair>& pair)
+{
+ doFilterContacts(state, pair);
+}
+
+void ContactFilter::update(double dt)
+{
+ doUpdate(dt);
+}
+
+}
+}
diff --git a/SurgSim/Collision/ContactFilter.h b/SurgSim/Collision/ContactFilter.h
new file mode 100644
index 0000000..fdf9c9d
--- /dev/null
+++ b/SurgSim/Collision/ContactFilter.h
@@ -0,0 +1,72 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_CONTACTFILTER_H
+#define SURGSIM_COLLISION_CONTACTFILTER_H
+
+
+#include "SurgSim/Framework/Component.h"
+
+#include <string>
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+class PhysicsManagerState;
+}
+
+namespace Collision
+{
+class CollisionPair;
+
+/// Base class to implement a contact filter, the job of this class is to be executed by the ContactFiltering stage
+/// in the PhysicsManager.
+/// Filters are being executed in parallel over all CollisionPair objects that have contacts, be careful with thread
+/// safety of the doFilterContacts function and all other writes to class members in your subclass
+class ContactFilter : public Framework::Component
+{
+public:
+ /// Constructor
+ explicit ContactFilter(const std::string& name);
+
+ /// Base function to filter the contacts on a collision pair this will modify the 'contacts' in the collision pair
+ /// \param state the physics state
+ /// \param pair the collision pair
+ void filterContacts(
+ const std::shared_ptr<Physics::PhysicsManagerState>& state,
+ const std::shared_ptr<CollisionPair>& pair);
+
+ /// Base function to update the internal data structures with regard to the filtering operation, this should only
+ /// be called once per iteration
+ /// \param dt the time passed since the last call to update
+ void update(double dt);
+
+protected:
+
+ /// Override this with the implementation of the filter, this has to be threadsafe, it will be called in parallel
+ /// with different collision pairs.
+ virtual void doFilterContacts(const std::shared_ptr<Physics::PhysicsManagerState>& state,
+ const std::shared_ptr<CollisionPair>& pair) = 0;
+
+ virtual void doUpdate(double dt) = 0;
+
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Collision/DcdCollision.h b/SurgSim/Collision/DcdCollision.h
deleted file mode 100644
index 5c85039..0000000
--- a/SurgSim/Collision/DcdCollision.h
+++ /dev/null
@@ -1,33 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
index c5d41b6..3637e56 100644
--- a/SurgSim/Collision/DefaultContactCalculation.cpp
+++ b/SurgSim/Collision/DefaultContactCalculation.cpp
@@ -34,18 +34,47 @@ DefaultContactCalculation::~DefaultContactCalculation()
{
}
-std::pair<int,int> DefaultContactCalculation::getShapeTypes()
+std::pair<int, int> DefaultContactCalculation::getShapeTypes()
{
- return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_NONE, SurgSim::Math::SHAPE_TYPE_NONE);
+ 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() << ").";
+ SURGSIM_ASSERT(!m_doAssert)
+ << "Contact calculation not implemented for pairs with types ("
+ << pair->getFirst()->getShapeType() << ", " << pair->getSecond()->getShapeType() << ").";
+ SURGSIM_LOG_ONCE(SurgSim::Framework::Logger::getDefaultLogger(), WARNING)
+ << "Contact calculation not implemented for pairs with types ("
+ << pair->getFirst()->getShapeType() << ", " << pair->getSecond()->getShapeType() << ").";
+}
+
+std::list<std::shared_ptr<Contact>> DefaultContactCalculation::doCalculateDcdContact(
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape1,
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape2)
+{
+ SURGSIM_ASSERT(!m_doAssert)
+ << "Contact calculation not implemented for shapes with types ("
+ << posedShape1.getShape()->getType() << ", " << posedShape2.getShape()->getType() << ").";
+ SURGSIM_LOG_ONCE(SurgSim::Framework::Logger::getDefaultLogger(), WARNING)
+ << "Contact calculation not implemented for pairs with types ("
+ << posedShape1.getShape()->getType() << ", " << posedShape2.getShape()->getType() << ").";
+ return std::list<std::shared_ptr<Contact>>();
+}
+
+std::list<std::shared_ptr<Contact>> DefaultContactCalculation::doCalculateCcdContact(
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& posedShapeMotion1,
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& posedShapeMotion2)
+{
+ SURGSIM_ASSERT(!m_doAssert)
+ << "Contact calculation not implemented for pairs with types ("
+ << posedShapeMotion1.first.getShape()->getType() << ", "
+ << posedShapeMotion2.first.getShape()->getType() << ").";
+ SURGSIM_LOG_ONCE(SurgSim::Framework::Logger::getDefaultLogger(), WARNING)
+ << "Contact calculation not implemented for pairs with types ("
+ << posedShapeMotion1.first.getShape()->getType() << ", "
+ << posedShapeMotion2.first.getShape()->getType() << ").";
+ return std::list<std::shared_ptr<Contact>>();
}
}; // namespace Collision
diff --git a/SurgSim/Collision/DefaultContactCalculation.h b/SurgSim/Collision/DefaultContactCalculation.h
index c750cb6..a0c1470 100644
--- a/SurgSim/Collision/DefaultContactCalculation.h
+++ b/SurgSim/Collision/DefaultContactCalculation.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -31,7 +31,6 @@ class CollisionPair;
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
@@ -41,16 +40,20 @@ public:
/// 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;
+ 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;
+ void doCalculateContact(std::shared_ptr<CollisionPair> pair) override;
+
+ std::list<std::shared_ptr<Contact>> doCalculateDcdContact(
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape1,
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape2) override;
+
+ std::list<std::shared_ptr<Contact>> doCalculateCcdContact(
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& posedShapeMotion1,
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& posedShapeMotion2) override;
};
}; // namespace Collision
diff --git a/SurgSim/Collision/ElementContactFilter.cpp b/SurgSim/Collision/ElementContactFilter.cpp
new file mode 100644
index 0000000..7922322
--- /dev/null
+++ b/SurgSim/Collision/ElementContactFilter.cpp
@@ -0,0 +1,192 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/ElementContactFilter.h"
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, ElementContactFilter, ElementContactFilter);
+
+ElementContactFilter::ElementContactFilter(const std::string& name) : ContactFilter(name),
+ m_logger(SurgSim::Framework::Logger::getLogger("Collision/ElementContactFilter"))
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(ElementContactFilter, std::shared_ptr<Framework::Component>, Representation,
+ getRepresentation, setRepresentation);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(ElementContactFilter, FilterMapType,
+ FilterElements, getFilterElements, setFilterElements);
+}
+
+
+bool ElementContactFilter::doInitialize()
+{
+ return true;
+}
+
+bool ElementContactFilter::doWakeUp()
+{
+ bool valid = false;
+ SURGSIM_LOG_IF(m_representation == nullptr, m_logger, WARNING)
+ << "No representation for filtering on " << getFullName();
+ if (m_representation != nullptr)
+ {
+ auto type = m_representation->getShapeType();
+ if (type == SurgSim::Math::SHAPE_TYPE_MESH || type == SurgSim::Math::SHAPE_TYPE_SEGMENTMESH ||
+ type == SurgSim::Math::SHAPE_TYPE_SURFACEMESH)
+ {
+ valid = true;
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger)
+ << "ElementContactFilter " << getFullName()
+ << "invalid mesh structure for collision shape, current shape is " << type << std::endl
+ << "Should be one of " << SurgSim::Math::SHAPE_TYPE_MESH << ", "
+ << SurgSim::Math::SHAPE_TYPE_SURFACEMESH << ", or "
+ << SurgSim::Math::SHAPE_TYPE_SEGMENTMESH;
+ }
+ }
+
+ return valid;
+}
+
+void ElementContactFilter::setFilter(const std::shared_ptr<Framework::Component>& other,
+ const std::vector<size_t>& indices)
+{
+ SURGSIM_ASSERT(std::dynamic_pointer_cast<SurgSim::Collision::Representation>(other) != nullptr)
+ << "Need a collision representation as a filter object.";
+ boost::lock_guard<boost::mutex> guard(m_writeMutex);
+ m_writeBuffer[other.get()] = indices;
+}
+
+const std::vector<size_t>& ElementContactFilter::getFilter(const std::shared_ptr<Framework::Component>& other) const
+{
+ static const std::vector<size_t> empty;
+ auto it = m_filters.find(other.get());
+ if (it != m_filters.end())
+ {
+ return it->second;
+ }
+ return empty;
+}
+
+void ElementContactFilter::setFilterElements(const FilterMapType& filterElements)
+{
+ boost::lock_guard<boost::mutex> guard(m_writeMutex);
+ m_writeBuffer.clear();
+
+ for (const auto& item : filterElements)
+ {
+ auto converted = std::dynamic_pointer_cast<Collision::Representation>(item.first);
+ if (converted != nullptr)
+ {
+ m_writeBuffer[converted.get()] = item.second;
+ }
+ }
+}
+
+ElementContactFilter::FilterMapType ElementContactFilter::getFilterElements()
+{
+ FilterMapType result;
+ boost::lock_guard<boost::mutex> guard(m_writeMutex);
+ for (const auto& item : m_writeBuffer)
+ {
+ result.emplace_back(item.first->getSharedPtr(), item.second);
+ }
+
+ return result;
+}
+
+void ElementContactFilter::setRepresentation(const std::shared_ptr<SurgSim::Framework::Component>& val)
+{
+ SURGSIM_ASSERT(!isAwake()) << "Can't set representation after waking up on " << getFullName();
+ m_representation = Framework::checkAndConvert<SurgSim::Collision::Representation>(
+ val, "SurgSim::Collision::Representation");
+}
+
+std::shared_ptr<SurgSim::Collision::Representation> ElementContactFilter::getRepresentation() const
+{
+ return m_representation;
+}
+
+void ElementContactFilter::doFilterContacts(const std::shared_ptr<Physics::PhysicsManagerState>&,
+ const std::shared_ptr<CollisionPair>& pair)
+{
+ for (const auto& filter : m_filters)
+ {
+ if (filter.second.empty())
+ {
+ continue;
+ }
+
+ for (size_t i = 0, j = 1; i < 2; ++i, --j)
+ {
+ if (pairAt(pair->getRepresentations(), i).get() == getRepresentation().get() &&
+ pairAt(pair->getRepresentations(), j).get() == filter.first)
+ {
+ executeFilter(pair, i, filter.second);
+ }
+ }
+ }
+}
+
+void ElementContactFilter::doUpdate(double dt)
+{
+ boost::lock_guard<boost::mutex> guard(m_writeMutex);
+ m_filters = m_writeBuffer;
+}
+
+void ElementContactFilter::executeFilter(
+ const std::shared_ptr<CollisionPair>& pair,
+ size_t pairIndex,
+ const std::vector<size_t>& filter)
+{
+ auto shapeType = pairAt(pair->getRepresentations(), pairIndex)->getShapeType();
+ DataStructures::Location::Type locationType;
+
+ if (shapeType == Math::SHAPE_TYPE_MESH || shapeType == Math::SHAPE_TYPE_SURFACEMESH)
+ {
+ locationType = DataStructures::Location::TRIANGLE;
+ }
+ else if (shapeType == Math::SHAPE_TYPE_SEGMENTMESH)
+ {
+ locationType = DataStructures::Location::ELEMENT;
+ }
+ else
+ {
+ return;
+ }
+
+ pair->getContacts().erase(
+ (std::remove_if(pair->getContacts().begin(), pair->getContacts().end(),
+ [pairIndex, locationType, &filter](const std::shared_ptr<Collision::Contact>& contact)
+ {
+ return pairAt(contact->penetrationPoints, pairIndex).get(locationType).hasValue() &&
+ std::find(filter.begin(), filter.end(), pairAt(contact->penetrationPoints,
+ pairIndex).get(locationType).getValue().index) != filter.end();
+ })), pair->getContacts().end());
+}
+
+}
+}
diff --git a/SurgSim/Collision/ElementContactFilter.h b/SurgSim/Collision/ElementContactFilter.h
new file mode 100644
index 0000000..be3ac55
--- /dev/null
+++ b/SurgSim/Collision/ElementContactFilter.h
@@ -0,0 +1,136 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_ELEMENTCONTACTFILTER_H
+#define SURGSIM_COLLISION_ELEMENTCONTACTFILTER_H
+
+#include "SurgSim/Collision/ContactFilter.h"
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Framework/LockedContainer.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+class PhysicsManagerState;
+}
+
+namespace Collision
+{
+class CollisionPair;
+class Representation;
+struct Contact;
+
+SURGSIM_STATIC_REGISTRATION(ElementContactFilter);
+
+/// Given a DeformableCollisionRepresentation this filter can remove contacts on specific elements of that
+/// representation. The contacts will be flatly removed.
+class ElementContactFilter : public ContactFilter
+{
+public:
+ explicit ElementContactFilter(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Collision::ElementContactFilter);
+
+ bool doInitialize() override;
+
+ bool doWakeUp() override;
+
+ /// Set the filter for one representation, this means any contects on the given indices for that representation
+ /// will be removed
+ /// \param other The other side of the collisionpair
+ /// \param indices The indices to filter from the list of contacts
+ void setFilter(const std::shared_ptr<Framework::Component>& other, const std::vector<size_t>& indices);
+
+ /// Query the filters for one specific representation
+ /// \param other the representation
+ /// \return the currently ignored indices for a specific element
+ const std::vector<size_t>& getFilter(const std::shared_ptr<Framework::Component>& other) const;
+
+ /// Sets the representation used for filtering, can only be used before initialization
+ /// \param val the collision representation to be used for filtering
+ void setRepresentation(const std::shared_ptr<SurgSim::Framework::Component>& val);
+
+ /// \return the collision representation used for filtering
+ std::shared_ptr<SurgSim::Collision::Representation> getRepresentation() const;
+
+protected:
+
+ typedef std::vector<std::pair<std::shared_ptr<SurgSim::Framework::Component>, std::vector<size_t>>> FilterMapType;
+
+ void setFilterElements(const FilterMapType& filterElements);
+
+ FilterMapType getFilterElements();
+
+ void doFilterContacts(
+ const std::shared_ptr<Physics::PhysicsManagerState>& state,
+ const std::shared_ptr<CollisionPair>& pair) override;
+
+
+ void doUpdate(double dt) override;
+
+private:
+
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+
+ /// Representation whose contacts need to be filtered
+ std::shared_ptr<Collision::Representation> m_representation;
+
+ mutable boost::mutex m_writeMutex;
+ std::unordered_map<Framework::Component*, std::vector<size_t>> m_writeBuffer;
+
+ std::unordered_map<Framework::Component*, std::vector<size_t>> m_filters;
+
+ /// Run the filter over the side of the collision pair indicated by pairIndex
+ /// \param pair the collision pair that is being filtered
+ /// \param pairIndex 0/1 representing first, and second values of the pair data structure
+ /// \param filter indices to filter
+ void executeFilter(
+ const std::shared_ptr<CollisionPair>& pair,
+ size_t pairIndex,
+ const std::vector<size_t>& filter);
+};
+
+/// Get member of pair data via indexed access, the members of the pair have to have the same type
+/// const version
+/// \tparam T the member type
+/// \param p the pair
+/// \param i the index to access
+/// \return p.first if index == 0 and p.second if index == 1
+template <class T>
+const T& pairAt(const std::pair<T, T>& p, size_t i)
+{
+ SURGSIM_ASSERT(i == 0 || i == 1) << "Index for pair must be 0 or 1.";
+ return (i == 0) ? p.first : p.second;
+};
+
+/// Get member of pair data via indexed access, the members of the pair have to have the same type
+/// non-const version
+/// \tparam T the member type
+/// \param p the pair
+/// \param i the index to access
+/// \return p.first if index == 0 and p.second if index == 1
+template <class T>
+T& pairAt(std::pair<T, T>& p, size_t i) // NOLINT
+{
+ SURGSIM_ASSERT(i == 0 || i == 1) << "Index for pair must be 0 or 1.";
+ return (i == 0) ? p.first : p.second;
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Collision/OctreeCapsuleContact.cpp b/SurgSim/Collision/OctreeCapsuleContact.cpp
new file mode 100644
index 0000000..8e5c4f6
--- /dev/null
+++ b/SurgSim/Collision/OctreeCapsuleContact.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/Collision/OctreeCapsuleContact.h"
+
+using SurgSim::Math::CapsuleShape;
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+std::pair<int, int> OctreeCapsuleContact::getShapeTypes()
+{
+ return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_OCTREE, SurgSim::Math::SHAPE_TYPE_CAPSULE);
+}
+
+std::list<std::shared_ptr<Contact>> OctreeCapsuleContact::boxContactCalculation(
+ const SurgSim::Math::BoxShape& boxShape, const SurgSim::Math::RigidTransform3d& boxPose,
+ const SurgSim::Math::Shape& otherShape, const SurgSim::Math::RigidTransform3d& otherPose)
+{
+ return m_calculator.calculateDcdContact(boxShape, boxPose, static_cast<const CapsuleShape&>(otherShape), otherPose);
+}
+
+};
+};
diff --git a/SurgSim/Collision/OctreeCapsuleContact.h b/SurgSim/Collision/OctreeCapsuleContact.h
new file mode 100644
index 0000000..3641d98
--- /dev/null
+++ b/SurgSim/Collision/OctreeCapsuleContact.h
@@ -0,0 +1,44 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_OCTREECAPSULECONTACT_H
+#define SURGSIM_COLLISION_OCTREECAPSULECONTACT_H
+
+#include "SurgSim/Collision/BoxCapsuleContact.h"
+#include "SurgSim/Collision/OctreeContact.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class OctreeCapsuleContact : public OctreeContact
+{
+public:
+ std::pair<int, int> getShapeTypes() override;
+
+protected:
+ std::list<std::shared_ptr<Contact>> boxContactCalculation(const SurgSim::Math::BoxShape& boxShape,
+ const SurgSim::Math::RigidTransform3d& boxPose, const SurgSim::Math::Shape& otherShape,
+ const SurgSim::Math::RigidTransform3d& otherPose) override;
+
+private:
+ BoxCapsuleContact m_calculator;
+};
+
+};
+};
+
+#endif //SURGSIM_COLLISION_OCTREECAPSULECONTACT_H
diff --git a/SurgSim/Collision/OctreeContact.cpp b/SurgSim/Collision/OctreeContact.cpp
new file mode 100644
index 0000000..6921e12
--- /dev/null
+++ b/SurgSim/Collision/OctreeContact.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/Collision/OctreeContact.h"
+
+#include <boost/functional/hash.hpp>
+
+#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
+{
+
+size_t OctreeContact::Vector3dHash::operator()(const SurgSim::Math::Vector3d& id) const
+{
+ return boost::hash_range(id.data(), id.data() + 3);
+}
+
+
+std::list<std::shared_ptr<Contact>> OctreeContact::doCalculateDcdContact(
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape1,
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape2)
+{
+ SURGSIM_ASSERT(posedShape1.getShape()->getType() == Math::SHAPE_TYPE_OCTREE) <<
+ "Octree Contact needs an OctreeShape.";
+
+ SurgSim::DataStructures::OctreePath nodePath;
+ std::list<std::shared_ptr<Contact>> result;
+ std::shared_ptr<Math::OctreeShape> shape = std::static_pointer_cast<Math::OctreeShape>(posedShape1.getShape());
+
+ calculateContactWithNode(shape->getOctree(), posedShape1.getPose(),
+ posedShape2.getShape(), posedShape2.getPose(), &nodePath, &result);
+
+ return result;
+}
+
+void OctreeContact::calculateContactWithNode(
+ std::shared_ptr<const Math::OctreeShape::NodeType> node,
+ Math::RigidTransform3d octreePose,
+ const std::shared_ptr<Math::Shape>& shape,
+ const Math::RigidTransform3d& shapePose,
+ SurgSim::DataStructures::OctreePath* nodePath,
+ std::list<std::shared_ptr<Contact>>* result)
+{
+ if (! node->isActive())
+ {
+ return;
+ }
+
+ Math::Vector3d nodeSize = node->getBoundingBox().sizes();
+ std::shared_ptr<Math::BoxShape> nodeShape;
+ if (m_shapes.count(nodeSize) > 0)
+ {
+ nodeShape = m_shapes[nodeSize];
+ }
+ else
+ {
+ nodeShape = std::make_shared<SurgSim::Math::BoxShape>(nodeSize.x(), nodeSize.y(), nodeSize.z());
+ m_shapes[nodeSize] = nodeShape;
+ }
+ Math::Vector3d nodeCenter = node->getBoundingBox().center();
+ Math::RigidTransform3d nodePose = octreePose;
+ nodePose.translation() += nodePose.linear() * nodeCenter;
+
+ auto contacts = boxContactCalculation(*nodeShape, nodePose, *shape, shapePose);
+
+ if (!contacts.empty())
+ {
+ if (node->hasChildren())
+ {
+ for (size_t i = 0; i < node->getChildren().size(); i++)
+ {
+ nodePath->push_back(i);
+ calculateContactWithNode(node->getChild(i), octreePose, shape, shapePose, nodePath, result);
+ nodePath->pop_back();
+ }
+ }
+ else
+ {
+ Math::Vector3d contactPosition;
+ for (auto& contact : contacts)
+ {
+ contact->penetrationPoints.first.octreeNodePath.setValue(*nodePath);
+
+ contactPosition = contact->penetrationPoints.first.rigidLocalPosition.getValue();
+ contactPosition += nodeCenter;
+ contact->penetrationPoints.first.rigidLocalPosition.setValue(contactPosition);
+ }
+ result->splice(result->end(), contacts);
+ }
+ }
+}
+
+};
+};
+
diff --git a/SurgSim/Collision/OctreeContact.h b/SurgSim/Collision/OctreeContact.h
new file mode 100644
index 0000000..790dd72
--- /dev/null
+++ b/SurgSim/Collision/OctreeContact.h
@@ -0,0 +1,90 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_OCTREECONTACT_H
+#define SURGSIM_COLLISION_OCTREECONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ContactCalculation.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/OctreeShape.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+class ShapeCollisionRepresentation;
+
+/// Abstract base class to calculate intersections between an Octree and other shapes
+///
+/// Derived classes handle the calculation for specific shape types (ie
+/// OctreeSphereContact).
+class OctreeContact : public ContactCalculation
+{
+protected:
+ /// Do the calculation between an octree node (BoxShape) and the other shape
+ /// \param boxShape the box shape
+ /// \param boxPose the pose of the box
+ /// \param otherShape the other shape
+ /// \param otherPose the pose of the other shape
+ /// \return a list of contacts between the shapes, if any
+ virtual std::list<std::shared_ptr<Contact>> boxContactCalculation(
+ const SurgSim::Math::BoxShape& boxShape, const SurgSim::Math::RigidTransform3d& boxPose,
+ const SurgSim::Math::Shape& otherShape, const SurgSim::Math::RigidTransform3d& otherPose) = 0;
+
+private:
+ std::list<std::shared_ptr<Contact>> doCalculateDcdContact(
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape1,
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape2) 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 octreePose the pose of the octree shape
+ /// \param shape the shape that the octree is colliding with
+ /// \param shapePose the pose of the shape
+ /// \param nodePath the NodePath of the current octree node
+ /// \param result [in,out] all generated contacts are agreggated here
+ /// \param nodePath [in,out] the path of the current node
+ void calculateContactWithNode(
+ std::shared_ptr<const SurgSim::Math::OctreeShape::NodeType> node,
+ Math::RigidTransform3d octreePose,
+ const std::shared_ptr<Math::Shape>& shape,
+ const Math::RigidTransform3d& shapePose,
+ SurgSim::DataStructures::OctreePath* nodePath,
+ std::list<std::shared_ptr<Contact>>* result);
+
+ /// Enable a Vector3d to be used as a key in an unordered map.
+ class Vector3dHash
+ {
+ public:
+ size_t operator()(const SurgSim::Math::Vector3d& id) const;
+ };
+
+ /// The shapes used for the contact calculations are cached for performance.
+ std::unordered_map<SurgSim::Math::Vector3d, std::shared_ptr<SurgSim::Math::BoxShape>, Vector3dHash> m_shapes;
+};
+
+};
+};
+
+#endif // SURGSIM_COLLISION_OCTREECONTACT_H
diff --git a/SurgSim/Collision/OctreeDcdContact.cpp b/SurgSim/Collision/OctreeDcdContact.cpp
deleted file mode 100644
index 890f1fa..0000000
--- a/SurgSim/Collision/OctreeDcdContact.cpp
+++ /dev/null
@@ -1,109 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index a136611..0000000
--- a/SurgSim/Collision/OctreeDcdContact.h
+++ /dev/null
@@ -1,77 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/OctreeDoubleSidedPlaneContact.cpp b/SurgSim/Collision/OctreeDoubleSidedPlaneContact.cpp
new file mode 100644
index 0000000..832b0c5
--- /dev/null
+++ b/SurgSim/Collision/OctreeDoubleSidedPlaneContact.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/Collision/OctreeDoubleSidedPlaneContact.h"
+
+using SurgSim::Math::DoubleSidedPlaneShape;
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+std::pair<int, int> OctreeDoubleSidedPlaneContact::getShapeTypes()
+{
+ return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_OCTREE, SurgSim::Math::SHAPE_TYPE_DOUBLESIDEDPLANE);
+}
+
+std::list<std::shared_ptr<Contact>> OctreeDoubleSidedPlaneContact::boxContactCalculation(
+ const SurgSim::Math::BoxShape& boxShape, const SurgSim::Math::RigidTransform3d& boxPose,
+ const SurgSim::Math::Shape& otherShape, const SurgSim::Math::RigidTransform3d& otherPose)
+{
+ return m_calculator.calculateDcdContact(boxShape, boxPose, static_cast<const DoubleSidedPlaneShape&>(otherShape),
+ otherPose);
+}
+
+};
+};
diff --git a/SurgSim/Collision/OctreeDoubleSidedPlaneContact.h b/SurgSim/Collision/OctreeDoubleSidedPlaneContact.h
new file mode 100644
index 0000000..ddc9d1a
--- /dev/null
+++ b/SurgSim/Collision/OctreeDoubleSidedPlaneContact.h
@@ -0,0 +1,44 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_OCTREEDOUBLESIDEDPLANECONTACT_H
+#define SURGSIM_COLLISION_OCTREEDOUBLESIDEDPLANECONTACT_H
+
+#include "SurgSim/Collision/BoxDoubleSidedPlaneContact.h"
+#include "SurgSim/Collision/OctreeContact.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class OctreeDoubleSidedPlaneContact : public OctreeContact
+{
+public:
+ std::pair<int, int> getShapeTypes() override;
+
+protected:
+ std::list<std::shared_ptr<Contact>> boxContactCalculation(const SurgSim::Math::BoxShape& boxShape,
+ const SurgSim::Math::RigidTransform3d& boxPose, const SurgSim::Math::Shape& otherShape,
+ const SurgSim::Math::RigidTransform3d& otherPose) override;
+
+private:
+ BoxDoubleSidedPlaneContact m_calculator;
+};
+
+};
+};
+
+#endif //SURGSIM_COLLISION_OCTREEDOUBLESIDEDPLANECONTACT_H
diff --git a/SurgSim/Collision/OctreePlaneContact.cpp b/SurgSim/Collision/OctreePlaneContact.cpp
new file mode 100644
index 0000000..82f65cf
--- /dev/null
+++ b/SurgSim/Collision/OctreePlaneContact.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/Collision/OctreePlaneContact.h"
+
+using SurgSim::Math::PlaneShape;
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+std::pair<int, int> OctreePlaneContact::getShapeTypes()
+{
+ return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_OCTREE, SurgSim::Math::SHAPE_TYPE_PLANE);
+}
+
+std::list<std::shared_ptr<Contact>> OctreePlaneContact::boxContactCalculation(
+ const SurgSim::Math::BoxShape& boxShape, const SurgSim::Math::RigidTransform3d& boxPose,
+ const SurgSim::Math::Shape& otherShape, const SurgSim::Math::RigidTransform3d& otherPose)
+{
+ return m_calculator.calculateDcdContact(boxShape, boxPose, static_cast<const PlaneShape&>(otherShape), otherPose);
+}
+
+};
+};
diff --git a/SurgSim/Collision/OctreePlaneContact.h b/SurgSim/Collision/OctreePlaneContact.h
new file mode 100644
index 0000000..8c24109
--- /dev/null
+++ b/SurgSim/Collision/OctreePlaneContact.h
@@ -0,0 +1,44 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_OCTREEPLANECONTACT_H
+#define SURGSIM_COLLISION_OCTREEPLANECONTACT_H
+
+#include "SurgSim/Collision/BoxPlaneContact.h"
+#include "SurgSim/Collision/OctreeContact.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class OctreePlaneContact : public OctreeContact
+{
+public:
+ std::pair<int, int> getShapeTypes() override;
+
+protected:
+ std::list<std::shared_ptr<Contact>> boxContactCalculation(const SurgSim::Math::BoxShape& boxShape,
+ const SurgSim::Math::RigidTransform3d& boxPose, const SurgSim::Math::Shape& otherShape,
+ const SurgSim::Math::RigidTransform3d& otherPose) override;
+
+private:
+ BoxPlaneContact m_calculator;
+};
+
+};
+};
+
+#endif //SURGSIM_COLLISION_OCTREEPLANECONTACT_H
diff --git a/SurgSim/Collision/OctreeSphereContact.cpp b/SurgSim/Collision/OctreeSphereContact.cpp
new file mode 100644
index 0000000..53587d6
--- /dev/null
+++ b/SurgSim/Collision/OctreeSphereContact.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/Collision/OctreeSphereContact.h"
+
+using SurgSim::Math::SphereShape;
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+std::pair<int, int> OctreeSphereContact::getShapeTypes()
+{
+ return std::pair<int,int>(SurgSim::Math::SHAPE_TYPE_OCTREE, SurgSim::Math::SHAPE_TYPE_SPHERE);
+}
+
+std::list<std::shared_ptr<Contact>> OctreeSphereContact::boxContactCalculation(
+ const SurgSim::Math::BoxShape& boxShape, const SurgSim::Math::RigidTransform3d& boxPose,
+ const SurgSim::Math::Shape& otherShape, const SurgSim::Math::RigidTransform3d& otherPose)
+{
+ return m_calculator.calculateDcdContact(boxShape, boxPose, static_cast<const SphereShape&>(otherShape), otherPose);
+}
+
+};
+};
diff --git a/SurgSim/Collision/OctreeSphereContact.h b/SurgSim/Collision/OctreeSphereContact.h
new file mode 100644
index 0000000..c6ed825
--- /dev/null
+++ b/SurgSim/Collision/OctreeSphereContact.h
@@ -0,0 +1,44 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_OCTREESPHERECONTACT_H
+#define SURGSIM_COLLISION_OCTREESPHERECONTACT_H
+
+#include "SurgSim/Collision/BoxSphereContact.h"
+#include "SurgSim/Collision/OctreeContact.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class OctreeSphereContact : public OctreeContact
+{
+public:
+ std::pair<int, int> getShapeTypes() override;
+
+protected:
+ std::list<std::shared_ptr<Contact>> boxContactCalculation(const SurgSim::Math::BoxShape& boxShape,
+ const SurgSim::Math::RigidTransform3d& boxPose, const SurgSim::Math::Shape& otherShape,
+ const SurgSim::Math::RigidTransform3d& otherPose) override;
+
+private:
+ BoxSphereContact m_calculator;
+};
+
+};
+};
+
+#endif //SURGSIM_COLLISION_OCTREESPHERECONTACT_H
diff --git a/SurgSim/Collision/PerformanceTests/CMakeLists.txt b/SurgSim/Collision/PerformanceTests/CMakeLists.txt
new file mode 100644
index 0000000..d235c80
--- /dev/null
+++ b/SurgSim/Collision/PerformanceTests/CMakeLists.txt
@@ -0,0 +1,42 @@
+# 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
+ TriangleMeshTriangleMeshContactCalculationPerformanceTest.cpp
+)
+
+set(UNIT_TEST_HEADERS
+)
+
+set(LIBS
+ SurgSimCollision
+ SurgSimDataStructures
+ SurgSimFramework
+)
+
+# 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(SurgSimCollisionPerformanceTest)
+
+set_target_properties(SurgSimCollisionPerformanceTest PROPERTIES FOLDER "Collision")
diff --git a/SurgSim/Collision/PerformanceTests/TriangleMeshTriangleMeshContactCalculationPerformanceTest.cpp b/SurgSim/Collision/PerformanceTests/TriangleMeshTriangleMeshContactCalculationPerformanceTest.cpp
new file mode 100644
index 0000000..8bbb813
--- /dev/null
+++ b/SurgSim/Collision/PerformanceTests/TriangleMeshTriangleMeshContactCalculationPerformanceTest.cpp
@@ -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.
+
+#include "SurgSim/Collision/TriangleMeshTriangleMeshContact.h"
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Timer.h"
+
+using SurgSim::Math::MeshShape;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+TEST(TriangleMeshTriangleMeshContactCalculationPerformanceTests, IntersectionTest)
+{
+
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ auto meshA = std::make_shared<MeshShape>();
+ meshA->load("Geometry/stapler_collision.ply");
+
+ auto meshB = std::make_shared<MeshShape>();
+ meshB->load("Geometry/wound_deformable_with_texture.ply");
+
+ std::shared_ptr<ShapeCollisionRepresentation> meshARep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Mesh 0");
+ meshARep->setShape(meshA);
+
+ std::shared_ptr<ShapeCollisionRepresentation> meshBRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Mesh 1");
+ meshBRep->setShape(meshB);
+
+ TriangleMeshTriangleMeshContact calcContact;
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(meshARep, meshBRep);
+
+ Framework::Timer timer;
+ int loops = 100;
+ size_t contacts = 0;
+ for (int i = 0 ; i < loops; ++i)
+ {
+ pair->clearContacts();
+ meshARep->getCollisions().unsafeGet().clear();
+ meshBRep->getCollisions().unsafeGet().clear();
+ RigidTransform3d pose =
+ Math::makeRigidTransform(Math::makeRotationQuaternion(2.0 * M_PI * i / loops, Vector3d::UnitX().eval()),
+ Vector3d(i, -1.5 * i, 0.0));
+ meshARep->setLocalPose(pose);
+ meshBRep->setLocalPose(pose);
+ timer.beginFrame();
+ calcContact.calculateContact(pair);
+ timer.endFrame();
+ contacts += pair->getContacts().size();
+ }
+
+ RecordProperty("FrameRate", boost::to_string(loops / timer.getCumulativeTime()));
+ RecordProperty("Duration", boost::to_string(timer.getCumulativeTime()));
+ RecordProperty("Loops", boost::to_string(loops));
+}
+
+}
+}
diff --git a/SurgSim/Collision/PerformanceTests/config.txt.in b/SurgSim/Collision/PerformanceTests/config.txt.in
new file mode 100644
index 0000000..8dc3922
--- /dev/null
+++ b/SurgSim/Collision/PerformanceTests/config.txt.in
@@ -0,0 +1 @@
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data/
diff --git a/SurgSim/Collision/Representation.cpp b/SurgSim/Collision/Representation.cpp
index ead4cd3..6fcebcb 100644
--- a/SurgSim/Collision/Representation.cpp
+++ b/SurgSim/Collision/Representation.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -14,16 +14,29 @@
// limitations under the License.
#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Shape.h"
#include "SurgSim/Physics/Representation.h"
+
namespace SurgSim
{
namespace Collision
{
Representation::Representation(const std::string& name) :
- SurgSim::Framework::Representation(name)
+ SurgSim::Framework::Representation(name),
+ m_logger(Framework::Logger::getLogger("Collision/Representation")),
+ m_collisionDetectionType(COLLISION_DETECTION_TYPE_DISCRETE),
+ m_selfCollisionDetectionType(COLLISION_DETECTION_TYPE_NONE)
{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, std::vector<std::string>, Ignore, getIgnoring, setIgnoring);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, std::vector<std::string>, Allow, getAllowing, setAllowing);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, CollisionDetectionType, CollisionDetectionType,
+ getCollisionDetectionType, setCollisionDetectionType);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, CollisionDetectionType, SelfCollisionDetectionType,
+ getSelfCollisionDetectionType, setSelfCollisionDetectionType);
}
Representation::~Representation()
@@ -31,13 +44,264 @@ Representation::~Representation()
}
+void Representation::setCollisionDetectionType(CollisionDetectionType type)
+{
+ m_collisionDetectionType = type;
+}
+
+CollisionDetectionType Representation::getCollisionDetectionType() const
+{
+ return m_collisionDetectionType;
+}
+
+void Representation::setSelfCollisionDetectionType(CollisionDetectionType type)
+{
+ m_selfCollisionDetectionType = type;
+}
+
+CollisionDetectionType Representation::getSelfCollisionDetectionType() const
+{
+ return m_selfCollisionDetectionType;
+}
+
+const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& Representation::getPosedShapeMotion() const
+{
+ boost::shared_lock<boost::shared_mutex> lock(m_posedShapeMotionMutex);
+
+ return m_posedShapeMotion;
+}
+
+void Representation::setPosedShapeMotion(const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& posedShapeMotion)
+{
+ boost::unique_lock<boost::shared_mutex> lock(m_posedShapeMotionMutex);
+
+ m_posedShapeMotion = posedShapeMotion;
+}
+
+std::shared_ptr<Math::Shape> Representation::getPosedShape()
+{
+ if (getShape()->isTransformable())
+ {
+ // HS-3-mar-2016 This is still being used by all representations it will be superceded by
+ // local update functionality after ryans merge request goes in
+ // #todo get rid of this in favor of transforming the mesh shape
+ boost::unique_lock<boost::shared_mutex> lock(m_posedShapeMotionMutex);
+
+ Math::RigidTransform3d identity = Math::RigidTransform3d::Identity();
+ Math::RigidTransform3d pose = getPose();
+ if (pose.isApprox(identity))
+ {
+ Math::PosedShape<std::shared_ptr<Math::Shape>> newPosedShape(getShape(), identity);
+ m_posedShapeMotion.second = newPosedShape;
+ }
+ else if (m_posedShapeMotion.second.getShape() == nullptr || !pose.isApprox(m_posedShapeMotion.second.getPose()))
+ {
+ Math::PosedShape<std::shared_ptr<Math::Shape>> newPosedShape(getShape()->getTransformed(pose), pose);
+ m_posedShapeMotion.second = newPosedShape;
+ }
+ return m_posedShapeMotion.second.getShape();
+ }
+ else
+ {
+ return getShape();
+ }
+}
+
+void Representation::invalidatePosedShapeMotion()
+{
+ boost::unique_lock<boost::shared_mutex> lock(m_posedShapeMotionMutex);
+
+ m_posedShapeMotion.invalidate();
+}
+
SurgSim::DataStructures::BufferedValue<ContactMapType>& Representation::getCollisions()
{
return m_collisions;
}
+void Representation::addContact(const std::shared_ptr<Representation>& other,
+ const std::shared_ptr<SurgSim::Collision::Contact>& contact)
+{
+ boost::lock_guard<boost::mutex> lock(m_collisionsMutex);
+
+ m_collisions.unsafeGet()[other].push_back(contact);
+}
+
+bool Representation::collidedWith(const std::shared_ptr<Representation>& other)
+{
+ auto collisions = m_collisions.safeGet();
+ return (collisions->find(other) != collisions->end());
+}
+
void Representation::update(const double& dt)
{
+ // HS-2-Mar-2016
+ // #todo if we decide to keep this it should be used to make a threadsafe copy of the data and enable outside
+ // access to it, this can then be used by a behavior to access the correct shape
+ // currently this is unused
+}
+
+bool Representation::ignore(const std::string& fullName)
+{
+
+ bool result = false;
+ if (m_allowing.empty())
+ {
+ result = m_ignoring.insert(fullName).second;
+ }
+ else
+ {
+ auto found = m_allowing.find(fullName);
+ if (found != m_allowing.end())
+ {
+ m_allowing.erase(found);
+ result = true;
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger)
+ << getFullName() << " Trying to un-allow" << fullName << " but it wasn't found.";
+ }
+ }
+
+ return result;
+}
+
+bool Representation::ignore(const std::shared_ptr<Representation>& representation)
+{
+ if (representation->getSceneElement() == nullptr)
+ {
+ SURGSIM_LOG_WARNING(m_logger) << getFullName() << " cannot ignore " << representation->getName() <<
+ ", which is not in a scene element.";
+ return false;
+ }
+ return ignore(representation->getFullName());
+}
+
+bool Representation::allow(const std::string& fullName)
+{
+ bool result = false;
+ if (m_ignoring.empty())
+ {
+ result = m_allowing.insert(fullName).second;
+ }
+ else
+ {
+ auto found = m_ignoring.find(fullName);
+ if (found != m_ignoring.end())
+ {
+ m_ignoring.erase(found);
+ result = true;
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger)
+ << getFullName() << " Trying un-ignore" << fullName << " but it wasn't found.";
+ }
+ }
+ return result;
+}
+
+bool Representation::allow(const std::shared_ptr<Representation>& representation)
+{
+ if (representation->getSceneElement() == nullptr)
+ {
+ SURGSIM_LOG_WARNING(m_logger) << getFullName() << " cannot allow " << representation->getName() <<
+ ", which is not in a scene element.";
+ return false;
+ }
+ return allow(representation->getFullName());
+}
+
+void Representation::setIgnoring(const std::vector<std::string>& fullNames)
+{
+ if (m_allowing.empty())
+ {
+ m_ignoring.clear();
+ std::copy(fullNames.cbegin(), fullNames.cend(), std::inserter(m_ignoring, m_ignoring.begin()));
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << getFullName() << " cannot use setIgnoring. "
+ << "You can only set what representations to ignore or allow, not both.";
+ }
+}
+
+std::vector<std::string> Representation::getIgnoring() const
+{
+ return std::vector<std::string>(std::begin(m_ignoring), std::end(m_ignoring));
+}
+
+bool Representation::isIgnoring(const std::string& fullName) const
+{
+ if (m_allowing.empty())
+ {
+ return m_ignoring.find(fullName) != m_ignoring.end();
+ }
+ return m_allowing.find(fullName) == m_allowing.end();
+}
+
+bool Representation::isIgnoring(const std::shared_ptr<Representation>& representation) const
+{
+ return isIgnoring(representation->getFullName());
+}
+
+bool Representation::isAllowing(const std::string& fullName) const
+{
+ return !isIgnoring(fullName);
+}
+
+bool Representation::isAllowing(const std::shared_ptr<Representation>& representation) const
+{
+ return isAllowing(representation->getFullName());
+}
+
+
+void Representation::setAllowing(const std::vector<std::string>& fullNames)
+{
+ if (m_ignoring.empty())
+ {
+ m_allowing.clear();
+ std::copy(fullNames.cbegin(), fullNames.cend(), std::inserter(m_allowing, m_allowing.begin()));
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << getFullName() << " cannot use setAllowing. "
+ << "You can only set what representations to ignore or allow, not both.";
+ }
+}
+
+std::vector<std::string> Representation::getAllowing() const
+{
+ return std::vector<std::string>(std::begin(m_allowing), std::end(m_allowing));
+}
+
+void Representation::doRetire()
+{
+ m_collisions.unsafeGet().clear();
+ m_collisions.publish();
+ Framework::Representation::doRetire();
+}
+
+Math::Aabbd Representation::getBoundingBox() const
+{
+ SURGSIM_ASSERT(getShape() != nullptr);
+ return Math::transformAabb(getPose(), getShape()->getBoundingBox());
+}
+
+void Representation::updateDcdData()
+{
+
+}
+
+void Representation::updateCcdData(double timeOfImpact)
+{
+
+}
+
+void Representation::updateShapeData()
+{
+ getPosedShape();
}
}; // namespace Collision
diff --git a/SurgSim/Collision/Representation.h b/SurgSim/Collision/Representation.h
index 972e935..4b009b6 100644
--- a/SurgSim/Collision/Representation.h
+++ b/SurgSim/Collision/Representation.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,12 +16,17 @@
#ifndef SURGSIM_COLLISION_REPRESENTATION_H
#define SURGSIM_COLLISION_REPRESENTATION_H
+#include <boost/thread/mutex.hpp>
#include <list>
#include <memory>
#include <unordered_map>
+#include <unordered_set>
#include "SurgSim/DataStructures/BufferedValue.h"
+#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/Representation.h"
+#include "SurgSim/Math/Aabb.h"
+#include "SurgSim/Math/Shape.h"
namespace SurgSim
{
@@ -42,7 +47,10 @@ struct Contact;
class Representation;
typedef std::unordered_map<std::shared_ptr<SurgSim::Collision::Representation>,
- std::list<std::shared_ptr<SurgSim::Collision::Contact>>> ContactMapType;
+ std::list<std::shared_ptr<SurgSim::Collision::Contact>>> ContactMapType;
+
+/// The type of collision detection
+enum CollisionDetectionType : SURGSIM_ENUM_TYPE;
/// 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
@@ -63,28 +71,188 @@ public:
/// \return The unique type of the shape, used to determine which calculation to use.
virtual int getShapeType() const = 0;
+ /// Set the type of collision detection to use between this representation and other representations
+ /// \param type The collision detection type
+ void setCollisionDetectionType(CollisionDetectionType type);
+
+ /// Get the type of collision detection used between this representation and other representations
+ /// \return The collision detection type
+ CollisionDetectionType getCollisionDetectionType() const;
+
+ /// Set the type of collision detection to use between this representation and itself
+ /// \param type The collision detection type
+ void setSelfCollisionDetectionType(CollisionDetectionType type);
+
+ /// Get the type of collision detection used between this representation and itself
+ /// \return The collision detection type
+ CollisionDetectionType getSelfCollisionDetectionType() const;
+
/// Get the shape
/// \return The actual shape used for collision.
virtual const std::shared_ptr<SurgSim::Math::Shape> getShape() const = 0;
+ /// Get the shape, posed
+ /// \return The shape transformed by the pose of this representation
+ virtual std::shared_ptr<SurgSim::Math::Shape> getPosedShape();
+
+ /// \return the posed shape motion
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& getPosedShapeMotion() const;
+
/// 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();
+ /// Add a contact with another representation
+ /// \param other The other collision representation
+ /// \param contact The contact to be added
+ /// \note This method is thread-safe
+ void addContact(const std::shared_ptr<Representation>& other,
+ const std::shared_ptr<SurgSim::Collision::Contact>& contact);
+
+ /// Check whether this collision representation collided with another during the last update
+ /// \param other other collision representation to check against
+ /// \return true if there were contacts recorded, false otherwise
+ bool collidedWith(const std::shared_ptr<Representation>& other);
+
/// Update the representation.
/// \param dt the time passed from the last update.
virtual void update(const double& dt);
+ /// Update the basic Shape's state from the physics state, so that the bounding box can correctly be determined
+ virtual void updateShapeData();
+
+ /// Update the data (the shape) in preparation for a DCD contact calculation
+ virtual void updateDcdData();
+
+ /// Update the data (the motionShape) in preparation for a CCD contact calcul ation
+ /// \param timeOfImpact the last time of impact, the representation is responsible for managing
+ /// the time correctly
+ virtual void updateCcdData(double timeOfImpact);
+
+ /// Set a collision representation to ignore
+ /// Collisions with this collision representation will not be detected
+ /// This acts as the opposite of allow if the representation that is passed here was previously added via allow()
+ /// \param fullName The full name of the collision representation to ignore
+ bool ignore(const std::string& fullName);
+
+ /// Set a collision representation to ignore
+ /// Collisions with this collision representation will not be detected
+ /// This acts as the opposite of allow if the representation that is passed here was previously added via allow()
+ /// \param representation The collision representation to ignore
+ bool ignore(const std::shared_ptr<Representation>& representation);
+
+ /// Set the collision representations to ignore
+ /// Collisions with these collision representation will not be detected
+ /// \note This method conflicts with setAllowing. You can only set what
+ /// representations to ignore or allow collisions with, not both.
+ /// \param fullNames The collision representations (given by full name) to ignore
+ void setIgnoring(const std::vector<std::string>& fullNames);
+
+ /// Is the collision representation being ignored
+ /// \param fullName The full name of the collision representation to check
+ /// return True if the collision representation is being ignored
+ bool isIgnoring(const std::string& fullName) const;
+
+ /// Is the collision representation being ignored
+ /// \param representation The collision representation to check
+ /// return True if the collision representation is being ignored
+ bool isIgnoring(const std::shared_ptr<Representation>& representation) const;
+
+ /// Set a collision representation to allow
+ /// Only collisions with "allowed" collision representation will be detected
+ /// If the the representation is currently being "ignored" then it will be removed from that state and
+ /// collisions will be allowed again.
+ /// \note When both the allow and ignore lists are empty calling allow may cause a change of behavior that
+ /// might not be wanted (i.e. the representation will go from colliding with all others to just colliding with
+ /// one other representation). This might be caused by trying to revert an "ignore" that has already been reversed.
+ /// \param fullName The full name of the collision representation to allow
+ bool allow(const std::string& fullName);
+
+ /// Set a collision representation to allow
+ /// Only collisions with "allowed" collision representation will be detected
+ /// If the the representation is currently being "ignored" then it will be removed from that state and
+ /// collisions will be allowed again.
+ /// \note When both the allow and ignore lists are empty calling allow may cause a change of behavior that
+ /// might not be wanted (i.e. the representation will go from colliding with all others to just colliding with
+ /// one other representation). This might be caused by trying to revert an "ignore" that has already been reversed.
+ /// \param representation The collision representation to allow
+ bool allow(const std::shared_ptr<Representation>& representation);
+
+ /// Set the only collision representations to allow collisions with
+ /// Only Collisions with these collision representation will be detected
+ /// \note This method conflicts with ignore and setIgnoring. You can only set what
+ /// representations to ignore or allow collisions with, not both.
+ /// \param fullNames The collision representations (given by full name) to allow
+ void setAllowing(const std::vector<std::string>& fullNames);
+
+ /// Is the collision representation being allowed
+ /// \param fullName The full name of the collision representation to check
+ /// return True if the collision representation is being allowed
+ bool isAllowing(const std::string& fullName) const;
+
+ /// Is the collision representation being allowed
+ /// \param representation The collision representation to check
+ /// return True if the collision representation is being allowed
+ bool isAllowing(const std::shared_ptr<Representation>& representation) const;
+
+ /// \return the Bounding box for this object
+ Math::Aabbd getBoundingBox() const;
+
protected:
+ /// Invalidate the cached posed shape motion
+ void invalidatePosedShapeMotion();
+
+ /// Get the ignored collision representations
+ /// \return The full names of all the ignored collision representations
+ std::vector<std::string> getIgnoring() const;
+
+ /// Get the only collision representations that this representation is allowed to collide with
+ /// \return The full names of all the collision representations to allow
+ std::vector<std::string> getAllowing() const;
+
+ void doRetire() override;
+
+ /// \param posedShape the posed shape motion to be set
+ void setPosedShapeMotion(const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& posedShape);
+
+ std::shared_ptr<Framework::Logger> m_logger;
+
+private:
+ /// The type of collision detection
+ CollisionDetectionType m_collisionDetectionType;
+
+ /// The type of self collision detection
+ CollisionDetectionType m_selfCollisionDetectionType;
+
/// 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;
-};
+ /// Mutex to lock write access to m_collisions
+ boost::mutex m_collisionsMutex;
+
+ /// The shape transformed in space and defined through time, i.e. with 2 differents configurations
+ Math::PosedShapeMotion<std::shared_ptr<Math::Shape>> m_posedShapeMotion;
+
+ /// Mutex to lock write access to m_posedShapeMotion
+ mutable boost::shared_mutex m_posedShapeMotionMutex;
+
+ /// Ignored collision representations
+ std::unordered_set<std::string> m_ignoring;
+
+ /// Allowed collision representations
+ std::unordered_set<std::string> m_allowing;
+};
}; // namespace Collision
}; // namespace SurgSim
+SURGSIM_SERIALIZABLE_ENUM(SurgSim::Collision::CollisionDetectionType,
+ (COLLISION_DETECTION_TYPE_NONE)
+ (COLLISION_DETECTION_TYPE_DISCRETE)
+ (COLLISION_DETECTION_TYPE_CONTINUOUS)
+ (MAX_COLLISION_DETECTION_TYPES))
+
#endif
diff --git a/SurgSim/Collision/SegmentMeshTriangleMeshContact.cpp b/SurgSim/Collision/SegmentMeshTriangleMeshContact.cpp
new file mode 100644
index 0000000..bf92251
--- /dev/null
+++ b/SurgSim/Collision/SegmentMeshTriangleMeshContact.cpp
@@ -0,0 +1,419 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/SegmentMeshTriangleMeshContact.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"
+#include "SurgSim/Math/SegmentMeshShape.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::DataStructures::TriangleMesh;
+using SurgSim::Math::MeshShape;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::SegmentMeshShape;
+using SurgSim::Math::Vector3d;
+
+
+namespace SegmentMeshTriangleMesh
+{
+
+bool isThisContactADuplicate(
+ const std::shared_ptr<SurgSim::Collision::Contact>& newContact,
+ const std::list<std::shared_ptr<SurgSim::Collision::Contact>>& contacts)
+{
+ for (const auto& contact : contacts)
+ {
+ if (*newContact == *contact)
+ {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+}
+namespace SurgSim
+{
+namespace Collision
+{
+
+std::pair<int, int> SegmentMeshTriangleMeshContact::getShapeTypes()
+{
+ return std::pair<int, int>(SurgSim::Math::SHAPE_TYPE_SEGMENTMESH, SurgSim::Math::SHAPE_TYPE_MESH);
+}
+
+std::list<std::shared_ptr<Contact>> SegmentMeshTriangleMeshContact::calculateDcdContact(
+ const Math::SegmentMeshShape& segmentMeshShape,
+ const Math::RigidTransform3d& segmentMeshPose,
+ const Math::MeshShape& triangleMeshShape,
+ const Math::RigidTransform3d& triangleMeshPose) const
+{
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ std::list<SurgSim::DataStructures::AabbTree::TreeNodePairType> intersectionList
+ = segmentMeshShape.getAabbTree()->spatialJoin(*triangleMeshShape.getAabbTree());
+
+ double radius = segmentMeshShape.getRadius();
+ double depth = 0.0;
+ Vector3d normal;
+ Vector3d penetrationPointCapsule, penetrationPointTriangle, penetrationPointCapsuleAxis;
+
+ for (const auto& intersection : intersectionList)
+ {
+ std::shared_ptr<SurgSim::DataStructures::AabbTreeNode> nodeSegment = intersection.first;
+ std::shared_ptr<SurgSim::DataStructures::AabbTreeNode> nodeTriangle = intersection.second;
+
+ std::list<size_t> edgeList;
+ std::list<size_t> triangleList;
+
+ nodeSegment->getIntersections(nodeTriangle->getAabb(), &edgeList);
+ nodeTriangle->getIntersections(nodeSegment->getAabb(), &triangleList);
+
+ for (auto i = triangleList.begin(); i != triangleList.end(); ++i)
+ {
+ const Vector3d& normalTriangle = triangleMeshShape.getNormal(*i);
+ if (normalTriangle.isZero())
+ {
+ continue;
+ }
+
+ const auto& verticesTriangle = triangleMeshShape.getTrianglePositions(*i);
+
+ for (auto j = edgeList.begin(); j != edgeList.end(); ++j)
+ {
+ const auto& verticesSegment = segmentMeshShape.getEdgePositions(*j);
+
+ // Check if the triangle and capsule intersect.
+ if (SurgSim::Math::calculateContactTriangleCapsule(
+ verticesTriangle[0], verticesTriangle[1], verticesTriangle[2], normalTriangle,
+ verticesSegment[0], verticesSegment[1], radius,
+ &depth, &penetrationPointTriangle, &penetrationPointCapsule, &normal,
+ &penetrationPointCapsuleAxis))
+ {
+ // Create the contact.
+ std::pair<Location, Location> penetrationPoints;
+ SurgSim::Math::Vector2d barycentricCoordinate2;
+ SurgSim::Math::barycentricCoordinates(penetrationPointCapsuleAxis,
+ verticesSegment[0],
+ verticesSegment[1],
+ &barycentricCoordinate2);
+ penetrationPoints.first.elementMeshLocalCoordinate.setValue(
+ SurgSim::DataStructures::IndexedLocalCoordinate(*j, barycentricCoordinate2));
+ penetrationPoints.first.rigidLocalPosition.setValue(
+ segmentMeshPose.inverse() * penetrationPointCapsuleAxis);
+
+ Vector3d barycentricCoordinate;
+ SurgSim::Math::barycentricCoordinates(penetrationPointTriangle,
+ verticesTriangle[0],
+ verticesTriangle[1],
+ verticesTriangle[2],
+ normalTriangle,
+ &barycentricCoordinate);
+
+ penetrationPoints.second.triangleMeshLocalCoordinate.setValue(
+ SurgSim::DataStructures::IndexedLocalCoordinate(*i, barycentricCoordinate));
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ triangleMeshPose.inverse() * penetrationPointTriangle);
+
+ // Create the contact.
+ contacts.push_back(std::make_shared<Contact>(COLLISION_DETECTION_TYPE_DISCRETE,
+ std::abs(depth) +
+ (penetrationPointCapsule - penetrationPointCapsuleAxis).dot(normal),
+ 1.0, Vector3d::Zero(), -normal, penetrationPoints));
+ }
+ }
+ }
+ }
+
+ return contacts;
+}
+
+std::list<std::shared_ptr<Contact>>SegmentMeshTriangleMeshContact::calculateCcdContact(
+ const Math::SegmentMeshShape& shape1AtTime0,
+ const Math::RigidTransform3d& pose1AtTime0,
+ const Math::SegmentMeshShape& shape1AtTime1,
+ const Math::RigidTransform3d& pose1AtTime1,
+ const Math::MeshShape& shape2AtTime0,
+ const Math::RigidTransform3d& pose2AtTime0,
+ const Math::MeshShape& shape2AtTime1,
+ const Math::RigidTransform3d& pose2AtTime1) const
+{
+ using SegmentMeshTriangleMesh::isThisContactADuplicate;
+ using Math::calculateCcdContactSegmentSegment;
+ using Math::calculateCcdContactPointTriangle;
+ double epsilon = Math::Geometry::DistanceEpsilon;
+
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ // This code is not tested for SegmentMeshes on Rigids, warn the user !
+ SURGSIM_LOG_ONCE_IF(! pose1AtTime0.isApprox(Math::RigidTransform3d::Identity()) &&
+ ! pose1AtTime1.isApprox(Math::RigidTransform3d::Identity()),
+ SurgSim::Framework::Logger::getLogger("Collision"), SEVERE)
+ << "It looks like you're using the SegmentMesh with a rigid object under CCD, the "
+ << "SegmentMeshTriangleMeshContact is not tested for this case.";
+
+
+ SURGSIM_ASSERT(shape1AtTime0.getNumEdges() > 0);
+ SURGSIM_ASSERT(shape1AtTime0.getNumEdges() == shape1AtTime1.getNumEdges());
+ SURGSIM_ASSERT(shape2AtTime0.getNumTriangles() > 0);
+ SURGSIM_ASSERT(shape2AtTime0.getNumTriangles() == shape2AtTime1.getNumTriangles());
+
+ for (size_t edgeId = 0; edgeId < shape1AtTime0.getNumEdges(); edgeId++)
+ {
+ auto edgeT0 = shape1AtTime0.getEdge(edgeId);
+ auto edgeT1 = shape1AtTime1.getEdge(edgeId);
+
+ SURGSIM_ASSERT(edgeT0.verticesId == edgeT1.verticesId) << "Edges are different:\n" <<
+ "(" << edgeT0.verticesId[0] << "," << edgeT0.verticesId[1] << ")\n" <<
+ "(" << edgeT1.verticesId[0] << "," << edgeT1.verticesId[1] << ")\n" <<
+ "edgeT0.valid = " << edgeT0.isValid << "\nedgeT1.valid = " << edgeT1.isValid;
+
+ std::pair<Math::Vector3d, Math::Vector3d> sv0 = std::make_pair(
+ shape1AtTime0.getVertexPosition(edgeT0.verticesId[0]),
+ shape1AtTime1.getVertexPosition(edgeT1.verticesId[0]));
+ std::pair<Math::Vector3d, Math::Vector3d> sv1 = std::make_pair(
+ shape1AtTime0.getVertexPosition(edgeT0.verticesId[1]),
+ shape1AtTime1.getVertexPosition(edgeT1.verticesId[1]));
+ Math::Aabbd segmentAabb;
+ segmentAabb.extend(sv0.first);
+ segmentAabb.extend(sv0.second);
+ segmentAabb.extend(sv1.first);
+ segmentAabb.extend(sv1.second);
+
+ for (size_t triangleId = 0; triangleId < shape2AtTime0.getNumTriangles(); triangleId++)
+ {
+ auto triangleT0 = shape2AtTime0.getTriangle(triangleId);
+ auto triangleT1 = shape2AtTime1.getTriangle(triangleId);
+
+ SURGSIM_ASSERT(triangleT0.verticesId == triangleT1.verticesId) << "Triangles are different:\n" <<
+ "(" << triangleT0.verticesId[0] << "," << triangleT0.verticesId[1] << "," <<
+ triangleT0.verticesId[2] << ")\n" <<
+ "(" << triangleT1.verticesId[0] << "," << triangleT1.verticesId[1] << "," <<
+ triangleT1.verticesId[2] << ")\n" <<
+ "triangleT0.valid = " << triangleT0.isValid << "\ntriangleT1.valid = " << triangleT1.isValid;
+
+ std::pair<Math::Vector3d, Math::Vector3d> tv0 = std::make_pair(
+ shape2AtTime0.getVertexPosition(triangleT0.verticesId[0]),
+ shape2AtTime1.getVertexPosition(triangleT1.verticesId[0]));
+ std::pair<Math::Vector3d, Math::Vector3d> tv1 = std::make_pair(
+ shape2AtTime0.getVertexPosition(triangleT0.verticesId[1]),
+ shape2AtTime1.getVertexPosition(triangleT1.verticesId[1]));
+ std::pair<Math::Vector3d, Math::Vector3d> tv2 = std::make_pair(
+ shape2AtTime0.getVertexPosition(triangleT0.verticesId[2]),
+ shape2AtTime1.getVertexPosition(triangleT1.verticesId[2]));
+
+ Math::Aabbd triangleAabb;
+ triangleAabb.extend(tv0.first);
+ triangleAabb.extend(tv0.second);
+ triangleAabb.extend(tv1.first);
+ triangleAabb.extend(tv1.second);
+ triangleAabb.extend(tv2.first);
+ triangleAabb.extend(tv2.second);
+
+ if (!SurgSim::Math::doAabbIntersect(segmentAabb, triangleAabb))
+ {
+ continue;
+ }
+
+ double earliestTimeOfImpact = std::numeric_limits<double>::max();
+ double segmentAlpha = -1.0; //!< Barycentric coordinates of P in the segment sv0sv1
+ //!< P = sv0 + segmentAlpha.sv0sv1
+ double triangleAlpha = -1.0; //!< Barycentric coordinates of P in triangle tv0tv1tv2
+ double triangleBeta = -1.0; //!< P = tv0 + triangleAlpha.tv0tv1 + triangleBeta.tv0tv2
+
+ // Check collision at time t = 0
+ Math::Vector3d pt;
+ Math::Vector3d tn = ((tv1.first - tv0.first).cross(tv2.first - tv0.first));
+ if (tn.norm() < Math::Geometry::DistanceEpsilon)
+ {
+ SURGSIM_LOG_WARNING(Framework::Logger::getLogger("SegmentMeshTriangleMeshContact")) <<
+ "The triangle mesh contains a degenerate triangle (null normal)";
+ }
+ tn.normalize();
+ bool segmentSegmentCcdFound = false;
+
+ if (Math::doesCollideSegmentTriangle<Math::Vector3d::Scalar, Math::Vector3d::Options>(
+ sv0.first, sv1.first,
+ tv0.first, tv1.first, tv2.first,
+ tn,
+ &pt))
+ {
+ Math::Vector2d baryCoordSegment;
+ Math::Vector3d baryCoordTriangle;
+ if (!Math::barycentricCoordinates(pt, sv0.first, sv1.first, &baryCoordSegment))
+ {
+ SURGSIM_LOG_WARNING(Framework::Logger::getLogger("SegmentMeshTriangleMeshContact")) <<
+ "[t=0] Could not deduce the barycentric coordinate of (" << pt.transpose() <<
+ ") in the segment (" << sv0.first.transpose() << ") (" << sv1.first.transpose() << ")";
+ }
+ if (!Math::barycentricCoordinates(pt, tv0.first, tv1.first, tv2.first, &baryCoordTriangle))
+ {
+ SURGSIM_LOG_WARNING(Framework::Logger::getLogger("SegmentMeshTriangleMeshContact")) <<
+ "[t=0] Could not deduce the barycentric coordinate of (" << pt.transpose() <<
+ ") in the triangle (" << tv0.first.transpose() << ") (" << tv1.first.transpose() <<
+ ") (" << tv2.first.transpose() << ")";
+ }
+ segmentSegmentCcdFound = false;
+ earliestTimeOfImpact = 0.0;
+ segmentAlpha = baryCoordSegment[1];
+ triangleAlpha = baryCoordTriangle[1];
+ triangleBeta = baryCoordTriangle[2];
+ }
+ else
+ {
+ // No collision at time t = 0, let's look for collision in the interval ]0..1]
+
+ // Calculate Segment/Segment ccd
+ double timeOfImpact;
+ double sFactor, tFactor;
+ if (calculateCcdContactSegmentSegment(sv0, sv1, tv0, tv1, &timeOfImpact, &sFactor, &tFactor))
+ {
+ if (timeOfImpact < earliestTimeOfImpact)
+ {
+ segmentSegmentCcdFound = true;
+ earliestTimeOfImpact = timeOfImpact;
+ segmentAlpha = sFactor;
+ triangleAlpha = tFactor;
+ triangleBeta = 0.0;
+ }
+ }
+
+ if (calculateCcdContactSegmentSegment(sv0, sv1, tv1, tv2, &timeOfImpact, &sFactor, &tFactor))
+ {
+ if (timeOfImpact < earliestTimeOfImpact)
+ {
+ segmentSegmentCcdFound = true;
+ earliestTimeOfImpact = timeOfImpact;
+ segmentAlpha = sFactor;
+ triangleAlpha = 1.0 - tFactor; // P = P0 + P0P1.(1 - tFactor) + P0P2.tFactor
+ triangleBeta = tFactor;
+ }
+ }
+
+ if (calculateCcdContactSegmentSegment(sv0, sv1, tv2, tv0, &timeOfImpact, &sFactor, &tFactor))
+ {
+ if (timeOfImpact < earliestTimeOfImpact)
+ {
+ segmentSegmentCcdFound = true;
+ earliestTimeOfImpact = timeOfImpact;
+ segmentAlpha = sFactor;
+ triangleAlpha = 0.0; // P = P0 + P0P2.(1 - tFactor)
+ triangleBeta = 1.0 - tFactor;
+ }
+ }
+
+ // Calculate Point/Triangle ccd
+ double u, v;
+ if (calculateCcdContactPointTriangle(sv0, tv0, tv1, tv2, &timeOfImpact, &u, &v))
+ {
+ if (timeOfImpact < earliestTimeOfImpact)
+ {
+ segmentSegmentCcdFound = false;
+ earliestTimeOfImpact = timeOfImpact;
+ segmentAlpha = 0.0;
+ triangleAlpha = u;
+ triangleBeta = v;
+ }
+ }
+
+ if (calculateCcdContactPointTriangle(sv1, tv0, tv1, tv2, &timeOfImpact, &u, &v))
+ {
+ if (timeOfImpact < earliestTimeOfImpact)
+ {
+ segmentSegmentCcdFound = false;
+ earliestTimeOfImpact = timeOfImpact;
+ segmentAlpha = 1.0;
+ triangleAlpha = u;
+ triangleBeta = v;
+ }
+ }
+ }
+
+ // False positive from the AABB, no collision found
+ if (earliestTimeOfImpact == std::numeric_limits<double>::max())
+ {
+ continue;
+ }
+
+ SURGSIM_ASSERT(segmentAlpha >= -epsilon && segmentAlpha <= (1.0 + epsilon)) <<
+ "earliestTimeOfImpact = " << earliestTimeOfImpact <<
+ "; segmentAlpha = " << segmentAlpha;
+ SURGSIM_ASSERT(triangleAlpha >= -epsilon && triangleBeta >= -epsilon &&
+ triangleAlpha + triangleBeta <= (1.0 + epsilon + epsilon)) <<
+ "earliestTimeOfImpact = " << earliestTimeOfImpact <<
+ "; triangleAlpha = " << triangleAlpha <<
+ "; triangleBeta = " << triangleBeta <<
+ "; triangleAlpha + triangleBeta = " << triangleAlpha + triangleBeta;
+
+
+ Math::Vector3d T, Tn;
+ double penentrationDepthAtT1;
+ Math::Vector3d S = Math::interpolate(sv0.second, sv1.second, segmentAlpha);
+ {
+ Math::Vector3d T0T1 = tv1.second - tv0.second;
+ Math::Vector3d T0T2 = tv2.second - tv0.second;
+ T = tv0.second + triangleAlpha * T0T1 + triangleBeta * T0T2;
+ Tn = (segmentSegmentCcdFound) ? (T - S).normalized() : T0T1.cross(T0T2).normalized();
+ penentrationDepthAtT1 = (S - T).dot(Tn);
+ }
+
+ Math::Vector segmentBaryCoord(2);
+ segmentBaryCoord << 1.0 - segmentAlpha, segmentAlpha;
+ DataStructures::IndexedLocalCoordinate localCoordinateSegment(edgeId, segmentBaryCoord);
+ DataStructures::Location locationSegment(localCoordinateSegment, Location::ELEMENT);
+ locationSegment.rigidLocalPosition = S;
+
+ Math::Vector triangleBaryCoord(3);
+ triangleBaryCoord << 1.0 - triangleAlpha - triangleBeta, triangleAlpha, triangleBeta;
+ DataStructures::IndexedLocalCoordinate localCoordinateTriangle(triangleId, triangleBaryCoord);
+ // The location related to the TriangleMesh can carry a TRIANGLE information
+ // e.g. part of a Mass-Spring with deformable triangulation for collision
+ DataStructures::Location locationTriangle(localCoordinateTriangle, Location::TRIANGLE);
+ // The location related to the TriangleMesh can carry an ELEMENT information
+ // e.g. part of an Fem2D for example
+ locationTriangle.elementMeshLocalCoordinate = locationTriangle.triangleMeshLocalCoordinate;
+ // The location related to the TriangleMesh can carry a RIGID LOCAL POSITION information
+ // e.g. part of a rigid body
+ locationTriangle.rigidLocalPosition = pose2AtTime1.inverse() * T;
+
+ auto contact = std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_CONTINUOUS,
+ penentrationDepthAtT1,
+ earliestTimeOfImpact,
+ T,
+ Tn,
+ std::make_pair(locationSegment, locationTriangle));
+
+ if (!isThisContactADuplicate(contact, contacts))
+ {
+ contacts.push_back(std::move(contact));
+ }
+ }
+ }
+
+ return contacts;
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/SegmentMeshTriangleMeshContact.h b/SurgSim/Collision/SegmentMeshTriangleMeshContact.h
new file mode 100644
index 0000000..35811be
--- /dev/null
+++ b/SurgSim/Collision/SegmentMeshTriangleMeshContact.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_COLLISION_SEGMENTMESHTRIANGLEMESHCONTACT_H
+#define SURGSIM_COLLISION_SEGMENTMESHTRIANGLEMESHCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/SegmentMeshShape.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between a segment mesh and a triangle mesh
+class SegmentMeshTriangleMeshContact : public ShapeShapeContactCalculation<Math::SegmentMeshShape, Math::MeshShape>
+{
+public:
+ using ContactCalculation::calculateDcdContact;
+
+ std::pair<int, int> getShapeTypes() override;
+
+ std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const Math::SegmentMeshShape& segmentMeshShape,
+ const Math::RigidTransform3d& segmentMeshPose,
+ const Math::MeshShape& triangleMeshShape,
+ const Math::RigidTransform3d& triangleMeshPose) const override;
+
+ std::list<std::shared_ptr<Contact>> calculateCcdContact(
+ const Math::SegmentMeshShape& shape1AtTime0, const Math::RigidTransform3d& pose1AtTime0,
+ const Math::SegmentMeshShape& shape1AtTime1, const Math::RigidTransform3d& pose1AtTime1,
+ const Math::MeshShape& shape2AtTime0, const Math::RigidTransform3d& pose2AtTime0,
+ const Math::MeshShape& shape2AtTime1, const Math::RigidTransform3d& pose2AtTime1) const override;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif // SURGSIM_COLLISION_SEGMENTMESHTRIANGLEMESHCONTACT_H
diff --git a/SurgSim/Collision/SegmentSegmentCcdIntervalCheck.cpp b/SurgSim/Collision/SegmentSegmentCcdIntervalCheck.cpp
new file mode 100644
index 0000000..f5741a3
--- /dev/null
+++ b/SurgSim/Collision/SegmentSegmentCcdIntervalCheck.cpp
@@ -0,0 +1,319 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/SegmentSegmentCcdIntervalCheck.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+// The order of initialization MUST match the necessary initialization order --
+// if member B is calculated from member A, then B must of course be initialized after A in
+// the constructor, and B must be declared after A in here the class definition.
+SegmentSegmentCcdIntervalCheck::SegmentSegmentCcdIntervalCheck(
+ const std::array<Math::Vector3d, 2>& pT0,
+ const std::array<Math::Vector3d, 2>& pT1,
+ const std::array<Math::Vector3d, 2>& qT0,
+ const std::array<Math::Vector3d, 2>& qT1,
+ double thicknessP, double thicknessQ,
+ double timePrecisionEpsilon, double distanceEpsilon)
+ : m_motionP1(pT0[0], pT1[0]), m_motionP2(pT0[1], pT1[1]), m_motionQ1(qT0[0], qT1[0]), m_motionQ2(qT0[1], qT1[1]),
+ m_relativeP1Q1(m_motionQ1 - m_motionP1),
+ m_relativeQ1Q2(m_motionQ2 - m_motionQ1),
+ m_relativeP1P2(m_motionP2 - m_motionP1),
+ m_P1Q1_P1P2_Q1Q2(Math::analyticTripleProduct(m_relativeP1Q1, m_relativeP1P2, m_relativeQ1Q2)),
+ m_P1P2_P1Q1(Math::analyticDotProduct(m_relativeP1P2, m_relativeP1Q1)),
+ m_Q1Q2_P1Q1(Math::analyticDotProduct(m_relativeQ1Q2, m_relativeP1Q1)),
+ m_P1P2_Q1Q2(Math::analyticDotProduct(m_relativeP1P2, m_relativeQ1Q2)),
+ m_P1P2_sq(Math::analyticMagnitudeSquared(m_relativeP1P2)),
+ m_Q1Q2_sq(Math::analyticMagnitudeSquared(m_relativeQ1Q2)),
+ m_P1P2xQ1Q2_x(Math::analyticCrossProductXAxis(m_relativeP1P2, m_relativeQ1Q2)),
+ m_P1P2xQ1Q2_y(Math::analyticCrossProductYAxis(m_relativeP1P2, m_relativeQ1Q2)),
+ m_P1P2xQ1Q2_z(Math::analyticCrossProductZAxis(m_relativeP1P2, m_relativeQ1Q2)),
+ m_thicknessP(thicknessP), m_thicknessQ(thicknessQ),
+ m_timePrecisionEpsilon(timePrecisionEpsilon), m_distanceEpsilon(distanceEpsilon),
+ m_volumeEpsilonTimes6(0), m_muNuEpsilon(0)
+{
+}
+
+// Accessors
+const Math::LinearMotionND<double, 3>& SegmentSegmentCcdIntervalCheck::motionP1() const
+{
+ return m_motionP1;
+}
+
+const Math::LinearMotionND<double, 3>& SegmentSegmentCcdIntervalCheck::motionP2() const
+{
+ return m_motionP2;
+}
+
+const Math::LinearMotionND<double, 3>& SegmentSegmentCcdIntervalCheck::motionQ1() const
+{
+ return m_motionQ1;
+}
+
+const Math::LinearMotionND<double, 3>& SegmentSegmentCcdIntervalCheck::motionQ2() const
+{
+ return m_motionQ2;
+}
+
+Math::Vector3d SegmentSegmentCcdIntervalCheck::p1T0() const
+{
+ return m_motionP1.getStart();
+}
+
+Math::Vector3d SegmentSegmentCcdIntervalCheck::p1T1() const
+{
+ return m_motionP1.getEnd();
+}
+
+Math::Vector3d SegmentSegmentCcdIntervalCheck::p2T0() const
+{
+ return m_motionP2.getStart();
+}
+
+Math::Vector3d SegmentSegmentCcdIntervalCheck::p2T1() const
+{
+ return m_motionP2.getEnd();
+}
+
+Math::Vector3d SegmentSegmentCcdIntervalCheck::q1T0() const
+{
+ return m_motionQ1.getStart();
+}
+
+Math::Vector3d SegmentSegmentCcdIntervalCheck::q1T1() const
+{
+ return m_motionQ1.getEnd();
+}
+
+Math::Vector3d SegmentSegmentCcdIntervalCheck::q2T0() const
+{
+ return m_motionQ2.getStart();
+}
+
+Math::Vector3d SegmentSegmentCcdIntervalCheck::q2T1() const
+{
+ return m_motionQ2.getEnd();
+}
+
+const Math::PolynomialValues<double, 3>& SegmentSegmentCcdIntervalCheck::P1Q1_P1P2_Q1Q2() const
+{
+ return m_P1Q1_P1P2_Q1Q2;
+}
+
+const Math::PolynomialValues<double, 2>& SegmentSegmentCcdIntervalCheck::P1P2_P1Q1() const
+{
+ return m_P1P2_P1Q1;
+}
+
+const Math::PolynomialValues<double, 2>& SegmentSegmentCcdIntervalCheck::Q1Q2_P1Q1() const
+{
+ return m_Q1Q2_P1Q1;
+}
+
+const Math::PolynomialValues<double, 2>& SegmentSegmentCcdIntervalCheck::P1P2_Q1Q2() const
+{
+ return m_P1P2_Q1Q2;
+}
+
+const Math::PolynomialValues<double, 2>& SegmentSegmentCcdIntervalCheck::P1P2_sq() const
+{
+ return m_P1P2_sq;
+}
+
+const Math::PolynomialValues<double, 2>& SegmentSegmentCcdIntervalCheck::Q1Q2_sq() const
+{
+ return m_Q1Q2_sq;
+}
+
+Math::Interval<double> SegmentSegmentCcdIntervalCheck::crossValueOnInterval(
+ const Math::Interval<double>& range) const
+{
+ return (
+ m_P1P2xQ1Q2_x.valuesOverInterval(range).square() +
+ m_P1P2xQ1Q2_y.valuesOverInterval(range).square() +
+ m_P1P2xQ1Q2_z.valuesOverInterval(range).square()
+ );
+}
+
+double SegmentSegmentCcdIntervalCheck::thicknessP() const
+{
+ return m_thicknessP;
+}
+double SegmentSegmentCcdIntervalCheck::thicknessQ() const
+{
+ return m_thicknessQ;
+}
+
+void SegmentSegmentCcdIntervalCheck::setTimePrecisionEpsilon(double epsilon)
+{
+ m_timePrecisionEpsilon = epsilon;
+}
+
+void SegmentSegmentCcdIntervalCheck::setDistanceEpsilon(double epsilon)
+{
+ m_distanceEpsilon = epsilon;
+}
+
+void SegmentSegmentCcdIntervalCheck::setTripleProductEpsilon(double epsilon)
+{
+ m_volumeEpsilonTimes6 = epsilon;
+}
+
+void SegmentSegmentCcdIntervalCheck::setMuNuEpsilon(double epsilon)
+{
+ m_muNuEpsilon = epsilon;
+}
+
+double SegmentSegmentCcdIntervalCheck::timePrecisionEpsilon() const
+{
+ return m_timePrecisionEpsilon;
+}
+
+double SegmentSegmentCcdIntervalCheck::distanceEpsilon() const
+{
+ return m_distanceEpsilon;
+}
+
+double SegmentSegmentCcdIntervalCheck::tripleProductEpsilon() const
+{
+ return m_volumeEpsilonTimes6;
+}
+
+double SegmentSegmentCcdIntervalCheck::muNuEpsilon() const
+{
+ return m_muNuEpsilon;
+}
+
+SegmentSegmentCcdIntervalCheck::IntervalCheckResults SegmentSegmentCcdIntervalCheck::possibleCollisionTestNoThickness(
+ const Math::Interval<double>& range) const
+{
+ Math::Interval<double> P1Q1_P1P2_Q1Q2_values = P1Q1_P1P2_Q1Q2().valuesOverInterval(range);
+ Math::Interval<double> P1P2_P1Q1_values = P1P2_P1Q1().valuesOverInterval(range);
+ Math::Interval<double> Q1Q2_P1Q1_values = Q1Q2_P1Q1().valuesOverInterval(range);
+ Math::Interval<double> P1P2_Q1Q2_values = P1P2_Q1Q2().valuesOverInterval(range);
+ Math::Interval<double> P1P2_sq_values = P1P2_sq().valuesOverInterval(range);
+ Math::Interval<double> Q1Q2_sq_values = Q1Q2_sq().valuesOverInterval(range);
+ Math::Interval<double> crossProductSquared_values = crossValueOnInterval(range);
+
+ P1Q1_P1P2_Q1Q2_values.addThickness(m_volumeEpsilonTimes6);
+
+ bool lineDistanceIsZero = P1Q1_P1P2_Q1Q2_values.containsZero();
+ if (!lineDistanceIsZero)
+ {
+ return IntervalCheckNoCollisionVolume;
+ }
+
+ // The following eqns and their derivation are written up in
+ // the document Segment-segmentCCDlocationcheck.pdf in the Assembla OSS Files page. See link:
+ // https://www.assembla.com/spaces/OpenSurgSim/documents/dce2Euy6Cr5znOdmr6CpXy/download/dce2Euy6Cr5znOdmr6CpXy
+ // 0 = \mu (P1P2 x Q1Q2)^2 - (P1P2 * P1Q1) Q1Q2^2 + (Q1Q2 * P1Q1) (P1P2 * Q1Q2)
+ // 0 = \nu (P1P2 x Q1Q2)^2 - (Q1Q2 * P1Q1) P1P2^2 + (P1P2 * P1Q1) (P1P2 * Q1Q2)
+
+ // Calculate the values of the \mu and \nu terms in the equations above; note that valid \mu and \nu are in [0,1].
+ // We make use of the fact that I*0 = [0,0] and I*1 = I, so I*[0,1] can be found by simply extending I to include 0.
+ Math::Interval<double> muNuTerm_values = crossProductSquared_values;
+ muNuTerm_values.extendToInclude(0);
+ // Add an epsilon to the result, to handle possible numerical noise.
+ muNuTerm_values.addThickness(m_muNuEpsilon);
+
+ Math::Interval<double> muExpression = muNuTerm_values - P1P2_P1Q1_values * Q1Q2_sq_values + Q1Q2_P1Q1_values *
+ P1P2_Q1Q2_values;
+ Math::Interval<double> nuExpression = muNuTerm_values + Q1Q2_P1Q1_values * P1P2_sq_values - P1P2_P1Q1_values *
+ P1P2_Q1Q2_values;
+
+ bool segmentLocationsMakeCollisionPossible = muExpression.containsZero() && nuExpression.containsZero();
+ if (!segmentLocationsMakeCollisionPossible)
+ {
+ return IntervalCheckNoCollisionEndpoints;
+ }
+
+ return IntervalCheckPossibleCollision;
+}
+
+SegmentSegmentCcdIntervalCheck::IntervalCheckResults SegmentSegmentCcdIntervalCheck::possibleCollisionTestWithThickness(
+ const Math::Interval<double>& range) const
+{
+ Math::Interval<double> P1Q1_P1P2_Q1Q2_values = P1Q1_P1P2_Q1Q2().valuesOverInterval(range);
+ Math::Interval<double> P1P2_P1Q1_values = P1P2_P1Q1().valuesOverInterval(range);
+ Math::Interval<double> Q1Q2_P1Q1_values = Q1Q2_P1Q1().valuesOverInterval(range);
+ Math::Interval<double> P1P2_Q1Q2_values = P1P2_Q1Q2().valuesOverInterval(range);
+ Math::Interval<double> P1P2_sq_values = P1P2_sq().valuesOverInterval(range);
+ Math::Interval<double> Q1Q2_sq_values = Q1Q2_sq().valuesOverInterval(range);
+ Math::Interval<double> P1P2xQ1Q2_sq_values = crossValueOnInterval(range);
+
+ // Now we need to account for thickness.
+ // The following eqns and their derivation are written up in
+ // the document Line-lineCCDwiththickness.pdf in the Assembla OSS Files page. See link:
+ // https://www.assembla.com/spaces/OpenSurgSim/documents/c_CS4My6Cr5APjacwqjQXA/download/c_CS4My6Cr5APjacwqjQXA
+ double maxLengthP = std::sqrt(P1P2_sq_values.getMax());
+ double maxLengthQ = std::sqrt(Q1Q2_sq_values.getMax());
+ // Figure out some upper bounds on the value of | P1P2 x Q1Q2 |.
+ // TO DO: figure out if both of the bounds are effective, i.e. if it makes sense to keep them both.
+ // (My guess would be that the first is more effective for perpendicular segments, second for near-parallel.)
+ double crossProductUpperBound1 = maxLengthP * maxLengthQ; // ignores the sine of the angle
+ double crossProductUpperBound2 = std::sqrt(P1P2xQ1Q2_sq_values.getMax()); // NOT necessarily tight
+ double crossProductUpperBound = std::min(crossProductUpperBound1,
+ crossProductUpperBound2); // pick the LOWEST of the upper bounds...
+ // Compute the upper bound on the volume that the tetrahedron P1P2Q1Q2 would have if the segments were
+ // just coming into contact. (What we actually calculate is the bound on the triple product, which is
+ // six times the tetrahedron volume.)
+ double touchingVolumeUpperBound = crossProductUpperBound * (thicknessP() + thicknessQ());
+
+ // Just to be clear here: The triple product P1Q1_P1P2_Q1Q2_values is the 6*volume of the
+ // tetrahedron defined by [p1, p2, q1, q2] over the interval. The value touchingVolumeUpperBound
+ // is an upper bound of 6*volume for the tetrahedron if the the two segments just reach contact.
+ // If the interval contains 0, then somewhere in the interval, the two volumes meet and we have
+ // contact at that point.
+ P1Q1_P1P2_Q1Q2_values.addThickness(touchingVolumeUpperBound);
+ bool lineDistanceIsSmallEnough = P1Q1_P1P2_Q1Q2_values.containsZero();
+ if (!lineDistanceIsSmallEnough)
+ {
+ return IntervalCheckNoCollisionVolume;
+ }
+
+ // If we detected that a collision is possible based on segment orientation, then we will make one more
+ // check to verify that the actual points of closest approach live on the segments.
+ double minLengthP = std::sqrt(P1P2_sq_values.getMin());
+ double minLengthQ = std::sqrt(Q1Q2_sq_values.getMin());
+ Math::Interval<double> weightCoefficient = P1P2xQ1Q2_sq_values;
+
+ // So, if we consider the endpoints, then the lambda and \mu overshoots extend the physical dimension
+ // to include the end caps of the capsules. The division of the radii by minLengthX scales the implicit
+ // overshoot proportionately to the length of the vector. E.g. so if the radii of the segments combine
+ // to be 10% of the length of segment P, then the lambda interval will be extended by 10% = 0.1 at
+ // the beginning and end, i.e. [-0.1, 1.1]
+ double lambdaOvershoot = (thicknessP() + thicknessQ()) / minLengthP;
+ Math::Interval<double> lambdaInterval = Math::Interval<double>(-lambdaOvershoot, 1. + lambdaOvershoot);
+ Math::Interval<double> lambdaExpression = lambdaInterval * weightCoefficient - P1P2_P1Q1_values * Q1Q2_sq_values +
+ Q1Q2_P1Q1_values * P1P2_Q1Q2_values;
+
+ double muOvershoot = (thicknessP() + thicknessQ()) / minLengthQ;
+ Math::Interval<double> muInterval = Math::Interval<double>(-muOvershoot, 1. + muOvershoot);
+ Math::Interval<double> muExpression = muInterval * weightCoefficient + Q1Q2_P1Q1_values * P1P2_sq_values -
+ P1P2_P1Q1_values * P1P2_Q1Q2_values;
+
+ bool segmentLocationsMakeCollisionPossible = lambdaExpression.containsZero() && muExpression.containsZero();
+ if (!segmentLocationsMakeCollisionPossible)
+ {
+ return IntervalCheckNoCollisionEndpoints;
+ }
+
+ return IntervalCheckPossibleCollision;
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/SegmentSegmentCcdIntervalCheck.h b/SurgSim/Collision/SegmentSegmentCcdIntervalCheck.h
new file mode 100644
index 0000000..1604c42
--- /dev/null
+++ b/SurgSim/Collision/SegmentSegmentCcdIntervalCheck.h
@@ -0,0 +1,229 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_SEGMENTSEGMENTCCDINTERVALCHECK_H
+#define SURGSIM_COLLISION_SEGMENTSEGMENTCCDINTERVALCHECK_H
+
+#include "SurgSim/Math/LinearMotionArithmetic.h"
+#include "SurgSim/Math/PolynomialValues.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+///
+/// SegmentSegmentCcdIntervalCheck uses the Interval classes including the LinearMotion
+/// and Polynomial families to quickly determine if there is a possible collision between
+/// two moving segments over a specified time interval. The time interval
+/// under consideration is defined as a subset of the parametric time interval [0, 1].
+///
+/// Details of the actual time of collision and the implementation of the recursion
+/// strategy are at a higher level.
+///
+/// \sa Interval<T>, IntervalND<T, N>, LinearMotion<T, N>, and Polynomial<T, N>
+///
+class SegmentSegmentCcdIntervalCheck
+{
+public:
+
+ /// Enum
+ /// Possible interval check return values. IntervalCheckPossibleCollision indicates
+ /// the given interval may have a collision between the segments, while
+ /// IntervalCheckNoCollisionVolume indicates no collision based on a gross volume
+ /// calculation and IntervalCheckNoCollisionEndpoints indicates that the endpoint
+ /// check indicates that the segments do not overlap at closest approach.
+ enum IntervalCheckResults
+ {
+ IntervalCheckPossibleCollision,
+ IntervalCheckNoCollisionVolume,
+ IntervalCheckNoCollisionEndpoints
+ };
+
+ /// Constructor
+ /// \param pT0 Starting and ending vertices for segment p at time 0
+ /// \param pT1 Starting and ending vertices for segment p at time 1
+ /// \param qT0 Starting and ending vertices for segment q at time 0
+ /// \param qT1 Starting and ending vertices for segment q at time 1
+ /// \param thicknessP Radius of segment P
+ /// \param thicknessQ Radius of segment Q
+ /// \param timePrecisionEpsilon Desired time accuracy
+ /// \param distanceEpsilon Desired distance accuracy
+ SegmentSegmentCcdIntervalCheck(const std::array<Math::Vector3d, 2>& pT0,
+ const std::array<Math::Vector3d, 2>& pT1,
+ const std::array<Math::Vector3d, 2>& qT0,
+ const std::array<Math::Vector3d, 2>& qT1,
+ double thicknessP, double thicknessQ,
+ double timePrecisionEpsilon, double distanceEpsilon);
+ /// @{
+ /// Motion accessors
+ /// \return the motion vector (value(t1) - value(t0)) for the segment endpoints p1, p2, q1, and q2, respectively.
+ const Math::LinearMotionND<double, 3>& motionP1() const;
+ const Math::LinearMotionND<double, 3>& motionP2() const;
+ const Math::LinearMotionND<double, 3>& motionQ1() const;
+ const Math::LinearMotionND<double, 3>& motionQ2() const;
+ /// @}
+
+ /// @{
+ /// Endpoint accessors
+ /// \return the motion vector [value(t0), value(t0)] for the segment endpoints p1, p2, q1, and q2, respectively.
+ Math::Vector3d p1T0() const;
+ Math::Vector3d p1T1() const;
+ Math::Vector3d p2T0() const;
+ Math::Vector3d p2T1() const;
+ Math::Vector3d q1T0() const;
+ Math::Vector3d q1T1() const;
+ Math::Vector3d q2T0() const;
+ Math::Vector3d q2T1() const;
+ /// @}
+
+ /// Triple product value
+ /// \return the triple product of (Q1(t) - P1(t)) X (P2(t) - P1(t)) X (Q2(t) - Q1(t)) as a 3rd degree polynomial
+ /// where P1, P2,Q1 and Q2 are time dependent positions for the segment endpoints.
+ /// \note the triple product is equivalent to 6 x the volume of tetrahedron P1P2Q1Q2. The polynomial
+ /// captures the variation in volume over the time interval.
+ const Math::PolynomialValues<double, 3>& P1Q1_P1P2_Q1Q2() const;
+
+ /// @{
+ /// Dot product accessors for time dependent vertex positions P1(t), P2(t), Q1(t) and Q2(t)
+ /// \return the dot product of the difference operators for the named endpoints
+ const Math::PolynomialValues<double, 2>& P1P2_P1Q1() const;
+ const Math::PolynomialValues<double, 2>& Q1Q2_P1Q1() const;
+ const Math::PolynomialValues<double, 2>& P1P2_Q1Q2() const;
+ /// @}
+
+ /// @{
+ /// Magnitude squared product accessors
+ /// \return the squared magnitude of the difference operators for time dependent
+ /// vertex positions P1(t), P2(t), Q1(t) and Q2(t).
+ const Math::PolynomialValues<double, 2>& P1P2_sq() const;
+ const Math::PolynomialValues<double, 2>& Q1Q2_sq() const;
+ /// @}
+
+ /// \param range the interval over which the cross product values are to be bounded.
+ /// \return the minimum and maximum of (P2 - P1) X (Q2 - Q1) restricted to the interval range.
+ Math::Interval<double> crossValueOnInterval(const Math::Interval<double>& range) const;
+
+ /// @{
+ /// Thickness accessors
+ /// \return the thickness parameters for P and Q, respectively.
+ double thicknessP() const;
+ double thicknessQ() const;
+ /// @}
+
+ /// @{
+ /// Algorithm epsilons. Set the epsilon values for the various member variables.
+ /// \param epsilon the algorithm epsilon parameters for "close enough" decisions.
+ void setTimePrecisionEpsilon(double epsilon);
+ void setDistanceEpsilon(double epsilon);
+ void setTripleProductEpsilon(double epsilon);
+ void setMuNuEpsilon(double epsilon);
+ /// @}
+
+ /// @{
+ /// Algorithm epsilons
+ /// \return the algorithm epsilon parameters for "close enough" decisions.
+ double timePrecisionEpsilon() const;
+ double distanceEpsilon() const;
+ double tripleProductEpsilon() const;
+ double muNuEpsilon() const;
+ /// @}
+
+ /// Check if a collision is possible within a specified time interval assuming ideal (0 thickness) segments
+ /// \param range the parametric [0, 1] time interval over which the collision is to be detected.
+ /// \return IntervalCheckPossibleCollision, IntervalCheckNoCollisionVolume, or IntervalCheckNoCollisionEndpoints
+ /// indicating if a collision is possible (returns IntervalCheckPossibleCollision); if tetrahedron
+ /// (P1, P2, Q1, Q2) has too great a volume for a collision (returns IntervalCheckNoCollisionVolume);
+ /// or if the possibly valid collision is not contained
+ /// within segments (P1, P2) and (Q1, Q2) (returns IntervalCheckNoCollisionEndpoints).
+ IntervalCheckResults possibleCollisionTestNoThickness(const Math::Interval<double>& range) const;
+
+ /// Check if a collision is possible within a specified time interval assuming segments with fixed radius
+ /// \param range the parametric [0, 1] time interval over which the collision is to be detected.
+ /// \return IntervalCheckPossibleCollision, IntervalCheckNoCollisionVolume, or IntervalCheckNoCollisionEndpoints
+ /// indicating if a collision is possible (returns IntervalCheckPossibleCollision); if tetrahedron
+ /// (P1, P2, Q1, Q2) has too great a volume for a collision (returns IntervalCheckNoCollisionVolume);
+ /// or if the possibly valid collision is not contained
+ /// within segments (P1, P2) and (Q1, Q2) (returns IntervalCheckNoCollisionEndpoints).
+ IntervalCheckResults possibleCollisionTestWithThickness(const Math::Interval<double>& range) const;
+
+private:
+ /// @{
+ /// Private constructor and assignment operators to prevent copying.
+ SegmentSegmentCcdIntervalCheck(const SegmentSegmentCcdIntervalCheck&);
+ SegmentSegmentCcdIntervalCheck& operator=(const SegmentSegmentCcdIntervalCheck&);
+ /// @}
+
+ /// @{
+ /// Linear motion intervals for each of the segment endpoints from t(0) to t(1).
+ Math::LinearMotionND<double, 3> m_motionP1;
+ Math::LinearMotionND<double, 3> m_motionP2;
+ Math::LinearMotionND<double, 3> m_motionQ1;
+ Math::LinearMotionND<double, 3> m_motionQ2;
+ /// @}
+
+ /// @{
+ /// Linear motion intervals for relative endpoint differences (i.e. P1Q1 indicates that the
+ /// interval encodes Q1 - P1).
+ Math::LinearMotionND<double, 3> m_relativeP1Q1;
+ Math::LinearMotionND<double, 3> m_relativeQ1Q2;
+ Math::LinearMotionND<double, 3> m_relativeP1P2;
+ /// @}
+
+ /// The triple product of (Q1(t) - P1(t)) X (P2(t) - P1(t)) X (Q2(t) - Q1(t)) as a 3rd degree polynomial
+ /// where P1, P2,Q1 and Q2 are time dependent positions for the segment endpoints.
+ /// \note the triple product is equivalent to 6 x the volume of tetrahedron P1P2Q1Q2.
+ Math::PolynomialValues<double, 3> m_P1Q1_P1P2_Q1Q2;
+
+ /// @{
+ /// Dot product accessors
+ /// The dot product for time dependent vertex positions P1(t), P2(t), Q1(t) and Q2(t)
+ Math::PolynomialValues<double, 2> m_P1P2_P1Q1;
+ Math::PolynomialValues<double, 2> m_Q1Q2_P1Q1;
+ Math::PolynomialValues<double, 2> m_P1P2_Q1Q2;
+ /// @}
+
+ /// @{
+ /// The squared magnitude of the difference operators for time dependent
+ /// vertex positions P1(t), P2(t), Q1(t) and Q2(t).
+ Math::PolynomialValues<double, 2> m_P1P2_sq;
+ Math::PolynomialValues<double, 2> m_Q1Q2_sq;
+ /// @}
+
+ /// @{
+ /// The x, y and z components of (P2 - P1) X (Q2 - Q1).
+ Math::PolynomialValues<double, 2> m_P1P2xQ1Q2_x;
+ Math::PolynomialValues<double, 2> m_P1P2xQ1Q2_y;
+ Math::PolynomialValues<double, 2> m_P1P2xQ1Q2_z;
+ /// @}
+
+ /// @{
+ /// The thickness parameters for P and Q, respectively.
+ double m_thicknessP;
+ double m_thicknessQ;
+ /// @}
+
+ /// @{
+ /// The algorithm epsilon parameters for "close enough" decisions.
+ double m_timePrecisionEpsilon;
+ double m_distanceEpsilon;
+ double m_volumeEpsilonTimes6;
+ double m_muNuEpsilon;
+ /// @}
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif // SURGSIM_COLLISION_SEGMENTSEGMENTCCDINTERVALCHECK_H
\ No newline at end of file
diff --git a/SurgSim/Collision/SegmentSegmentCcdMovingContact.cpp b/SurgSim/Collision/SegmentSegmentCcdMovingContact.cpp
new file mode 100644
index 0000000..a443687
--- /dev/null
+++ b/SurgSim/Collision/SegmentSegmentCcdMovingContact.cpp
@@ -0,0 +1,542 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/SegmentSegmentCcdMovingContact.h"
+
+#include <vector>
+
+#include "SurgSim/Collision/SegmentSegmentCcdIntervalCheck.h"
+#include "SurgSim/Collision/SegmentSegmentCcdStaticContact.h"
+#include "SurgSim/Framework/LogMacros.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+SegmentSegmentCcdMovingContact::SegmentSegmentCcdMovingContact() :
+ m_distanceEpsilon(1.0e-09), m_logger(Framework::Logger::getLogger("CCDMovingContactLog"))
+{
+}
+
+bool SegmentSegmentCcdMovingContact::collideMovingSegmentSegment(
+ const std::array<Math::Vector3d, 2>& pt0Positions,
+ const std::array<Math::Vector3d, 2>& pt1Positions,
+ const std::array<Math::Vector3d, 2>& qt0Positions,
+ const std::array<Math::Vector3d, 2>& qt1Positions,
+ double thicknessEpsilon,
+ double timePrecisionEpsilon,
+ double* t, double* r, double* s, Math::Vector3d* pToQDir)
+{
+ return collideMovingSegmentSegment(pt0Positions, pt1Positions, qt0Positions, qt1Positions,
+ thicknessEpsilon / 2.0, thicknessEpsilon / 2.0, timePrecisionEpsilon,
+ t, r, s, pToQDir);
+}
+
+bool SegmentSegmentCcdMovingContact::collideMovingSegmentSegment(
+ const std::array<Math::Vector3d, 2>& pt0Positions,
+ const std::array<Math::Vector3d, 2>& pt1Positions,
+ const std::array<Math::Vector3d, 2>& qt0Positions,
+ const std::array<Math::Vector3d, 2>& qt1Positions,
+ double thicknessP,
+ double thicknessQ,
+ double timePrecisionEpsilon,
+ double* t, double* r, double* s, Math::Vector3d* pToQDir)
+{
+ bool ans = collideSegmentSegmentBaseCase(
+ pt0Positions, pt1Positions, qt0Positions, qt1Positions,
+ thicknessP, thicknessQ, timePrecisionEpsilon, t, r, s);
+
+ if (ans)
+ {
+ // Calculate the contact points at t.
+ Math::Vector3d contactPointP = Math::interpolate(
+ Math::interpolate(pt0Positions[0], pt0Positions[1], *r),
+ Math::interpolate(pt1Positions[0], pt1Positions[1], *r),
+ *t);
+ Math::Vector3d contactPointQ = Math::interpolate(
+ Math::interpolate(qt0Positions[0], qt0Positions[1], *s),
+ Math::interpolate(qt1Positions[0], qt1Positions[1], *s),
+ *t);
+ *pToQDir = contactPointQ - contactPointP;
+ }
+ return ans;
+}
+
+bool SegmentSegmentCcdMovingContact::collideSegmentSegmentBaseCase(
+ const std::array<Math::Vector3d, 2>& pT0,
+ const std::array<Math::Vector3d, 2>& pT1,
+ const std::array<Math::Vector3d, 2>& qT0,
+ const std::array<Math::Vector3d, 2>& qT1,
+ double thicknessP,
+ double thicknessQ,
+ double timePrecisionEpsilon,
+ double* t, double* r, double* s)
+{
+ bool collisionFound = false;
+
+ const double parallelEpsilon = 1e-9;
+ const double degenerateEpsilon = 1e-10;
+ const double coplanarEpsilon = 1e-18;
+
+ const double parallelEpsilon2 = parallelEpsilon * parallelEpsilon;
+
+ Math::Vector3d p0t0p1t0 = pT0[1] - pT0[0];
+ Math::Vector3d q0t0q1t0 = qT0[1] - qT0[0];
+
+ Math::Vector3d p0t1p1t1 = pT1[1] - pT1[0];
+ Math::Vector3d q0t1q1t1 = qT1[1] - qT1[0];
+
+ //#################################################################
+ // 1st) Case of parallel segment through time step (at t=0 and t=1)
+ //
+ // Check if the cross product is near zero at both t0 and t1.
+ Math::Vector3d p0t0p1t0_vec_q0t0q1t0 = p0t0p1t0.cross(q0t0q1t0);
+ Math::Vector3d p0t1p1t1_vec_q0t1q1t1 = p0t1p1t1.cross(q0t1q1t1);
+ double p0t0p1t0_vec_q0t0q1t0_SQ = p0t0p1t0_vec_q0t0q1t0.squaredNorm();
+ double p0t1p1t1_vec_q0t1q1t1_SQ = p0t1p1t1_vec_q0t1q1t1.squaredNorm();
+ if (p0t0p1t0_vec_q0t0q1t0_SQ < parallelEpsilon2 && p0t1p1t1_vec_q0t1q1t1_SQ < parallelEpsilon2)
+ {
+ SURGSIM_LOG_WARNING(m_logger) <<
+ "Segments are parallel at start and end of time step. " <<
+ "Handling with parallel method rather than general method.";
+
+ // Either it collides at t=0 or we do a dichotomy to find intersection in [0..1]
+ if (m_staticTest.collideStaticSegmentSegment(pT0, qT0, thicknessP, thicknessQ, r, s))
+ {
+ *t = 0.0;
+ collisionFound = true;
+ }
+ else
+ {
+ collisionFound = collideSegmentSegmentParallelCase(
+ pT0, // Segment 1 at t=0
+ pT1, // Segment 1 at t=1
+ qT0, // Segment 2 at t=0
+ qT1, // Segment 2 at t=1
+ 0.0, 1.0,
+ thicknessP, thicknessQ,
+ timePrecisionEpsilon,
+ t, r, s);
+ }
+ }
+ else
+ {
+ //#################################################################
+ // 2nd) Case of coplanar segment through time step (at t=0 and t=1)
+
+ // Calculate the normal of p X q for at least one of the end points of
+ // q. Do this at both t=0 and t=1.
+ Math::Vector3d pt0Xqt0 = Math::robustCrossProduct(pT0, qT0, degenerateEpsilon);
+ Math::Vector3d pt1Xqt1 = Math::robustCrossProduct(pT1, qT1, degenerateEpsilon);
+
+ // TODO(wturner): Currently set to (10^-9), but has been as high as (10^-7) in previous versions.
+ // verify that this value works as intended. If not, we will need to add and set another
+ // member variable for the additional threshold.
+ normalizeSegmentsConsistently(&q0t0q1t0, &q0t1q1t1, m_distanceEpsilon);
+
+ // Do the segments remain coplanar all the time ?
+ if (std::fabs(pt0Xqt0.dot(q0t0q1t0)) < coplanarEpsilon &&
+ std::fabs(pt1Xqt1.dot(q0t1q1t1)) < coplanarEpsilon)
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Segments are coplanar at start and end of " <<
+ "the time step. Handling with coplanar method.";
+
+ if (m_staticTest.collideStaticSegmentSegment(pT0, qT0, thicknessP, thicknessQ, r, s))
+ {
+ *t = 0.0;
+ collisionFound = true;
+ }
+ else
+ {
+ // At this point, {r,s} are computed for t=0 !
+ collisionFound = collideSegmentSegmentCoplanarCase(
+ pT0, // Segment 1 at t=0
+ pT1, // Segment 1 at t=1
+ qT0, // Segment 2 at t=0
+ qT1, // Segment 2 at t=1
+ 0.0, 1.0, // Interval boundaries
+ timePrecisionEpsilon,
+ thicknessP, thicknessQ,
+ t, r, s);
+ }
+ }
+ else
+ {
+ //#################################################################
+ // 3rd) General case
+ SegmentSegmentCcdIntervalCheck state(pT0, pT1, qT0, qT1,
+ thicknessP, thicknessQ,
+ timePrecisionEpsilon, -1);
+
+ collisionFound = collideSegmentSegmentGeneralCase(
+ state,
+ 0.0, 1.0, // Look inside the interval t [0..1]
+ t, r, s);
+ }
+ }
+ return collisionFound;
+}
+
+bool SegmentSegmentCcdMovingContact::collideSegmentSegmentParallelCase(
+ const std::array<Math::Vector3d, 2>& pT0,
+ const std::array<Math::Vector3d, 2>& pT1,
+ const std::array<Math::Vector3d, 2>& qT0,
+ const std::array<Math::Vector3d, 2>& qT1,
+ double a, double b,
+ double thicknessP,
+ double thicknessQ,
+ double timePrecisionEpsilon,
+ double* t, double* r, double* s, int depth /*= 0*/)
+{
+ // Geometry at time t=b
+ Math::Vector3d p0Tb = Math::interpolate(pT0[0], pT1[0], b); // p[0] interpolated at time b
+ Math::Vector3d p1Tb = Math::interpolate(pT0[1], pT1[1], b); // p[1] interpolated at time b
+ Math::Vector3d q0Tb = Math::interpolate(qT0[0], qT1[0], b); // q[0] interpolated at time b
+ Math::Vector3d q1Tb = Math::interpolate(qT0[1], qT1[1], b); // q[1] interpolated at time b
+
+ std::array<Vector3d, 2> pb = {p0Tb, p1Tb};
+ std::array<Vector3d, 2> qb = {q0Tb, q1Tb};
+
+ if (b - a < timePrecisionEpsilon)
+ {
+ // We know that no collision happened at t=a, that is why we recursed to this level, but
+ // we believe that there is a collision in the interval. If our time precision is good enough,
+ // the segments should be colliding at t=b. Otherwise, we are either moving too fast to detect
+ // this collision, or something went wrong. Report true and make a best guess at the middle of
+ // the current interval.
+ if (m_staticTest.collideStaticSegmentSegment(pb, qb, thicknessP, thicknessQ, r, s))
+ {
+ *t = b;
+ }
+ else
+ {
+ *t = ((b + a) / 2.0);
+ }
+ return true;
+ }
+
+ // Geometry at time t=a
+ Math::Vector3d p0Ta = Math::interpolate(pT0[0], pT1[0], a); // p[0] interpolated at time a
+ Math::Vector3d q0Ta = Math::interpolate(qT0[0], qT1[0], a); // q[0] interpolated at time a
+ Math::Vector3d q1Ta = Math::interpolate(qT0[1], qT1[1], a); // q[1] interpolated at time a
+
+ // Compute intermediate geometry..and work on subdivisions based on 1/nbSubPoint (instead of 1/2)
+ //
+ // During this next phase, we potentially take two passes through the system. In the first pass, we
+ // do a quick check to see if we can find a collision at the end of any subdivision using our static
+ // segment code. If we do, we can immediately recurse only on that subdivision to further refine
+ // our time estimate.
+ //
+ // Otherwise, failing to find an easy detection, there is the possibility that we are moving
+ // towards an intersection at the start of an interval and away at the end. Any possible
+ // contact would be within the bracketed time frame, so if we detect it, we also recurse on the
+ // candidate interval.
+ //
+ double t_i[SUB_POINTS_PARALLEL_CASE + 1];
+ Math::Vector3d p0p0Proj[SUB_POINTS_PARALLEL_CASE + 1];
+
+ Math::Vector3d p0Proj = Math::nearestPointOnLine(p0Ta, q0Ta, q1Ta);
+ p0p0Proj[0] = p0Proj - p0Ta;
+ t_i[0] = a;
+
+ double deltaT = 1.0 / SUB_POINTS_PARALLEL_CASE;
+ for (int i = 1 ; i <= SUB_POINTS_PARALLEL_CASE ; i++)
+ {
+ t_i[i] = a + (b - a) * (i * deltaT);
+ double r_i;
+ double s_i;
+
+ // p and q at time i
+ std::array<Math::Vector3d, 2> p_i = {Math::interpolate(pT0[0], pT1[0], t_i[i]),
+ Math::interpolate(pT0[1], pT1[1], t_i[i])
+ };
+ std::array<Math::Vector3d, 2> q_i = {Math::interpolate(qT0[0], qT1[0], t_i[i]),
+ Math::interpolate(qT0[1], qT1[1], t_i[i])
+ };
+
+ if (m_staticTest.collideStaticSegmentSegment(p_i, q_i, thicknessP, thicknessQ, &r_i, &s_i))
+ {
+ // Collision happens between [t[i-1]..t[i]]
+ return collideSegmentSegmentParallelCase(
+ pT0, pT1, qT0, qT1, t_i[i - 1], t_i[i],
+ thicknessP, thicknessQ, timePrecisionEpsilon,
+ t, r, s,
+ depth + 1);
+ }
+
+ p0Proj = Math::nearestPointOnLine(p_i[0], q_i[0], q_i[1]);
+ p0p0Proj[i] = p0Proj - p_i[0];
+ }
+
+ // No collision found at the discretization
+ // Let analyze the variation of p1q1...if it switches direction, we more
+ // likely passed a parallel intersection case in between !
+ for (int i = 0; i < SUB_POINTS_PARALLEL_CASE; i++)
+ {
+ if (p0p0Proj[i].dot(p0p0Proj[i + 1]) < 0.0)
+ {
+ bool found = collideSegmentSegmentParallelCase(
+ pT0, pT1, qT0, qT1, t_i[i], t_i[i + 1],
+ thicknessP, thicknessQ, timePrecisionEpsilon,
+ t, r, s,
+ depth + 1);
+ if (found)
+ {
+ return true;
+ }
+ }
+ }
+ return false;
+}
+
+bool SegmentSegmentCcdMovingContact::collideSegmentSegmentCoplanarCase(
+ const std::array<Math::Vector3d, 2>& pT0, /* Segment 1 at t=0 */
+ const std::array<Math::Vector3d, 2>& pT1, /* Segment 1 at t=1 */
+ const std::array<Math::Vector3d, 2>& qT0, /* Segment 2 at t=0 */
+ const std::array<Math::Vector3d, 2>& qT1, /* Segment 2 at t=1 */
+ double a, double b, /* Interval boundaries */
+ double timePrecisionEpsilon,
+ double thickness_p, double thickness_q,
+ double* t, double* r, double* s,
+ int depth /*= 0*/)
+{
+ // Geometry at time t=b
+ Math::Vector3d p0Tb = Math::interpolate(pT0[0], pT1[0], b); // p[0] interpolated at time b
+ Math::Vector3d p1Tb = Math::interpolate(pT0[1], pT1[1], b); // p[1] interpolated at time b
+ Math::Vector3d q0Tb = Math::interpolate(qT0[0], qT1[0], b); // q[0] interpolated at time b
+ Math::Vector3d q1Tb = Math::interpolate(qT0[1], qT1[1], b); // q[1] interpolated at time b
+
+
+ if (b - a < timePrecisionEpsilon)
+ {
+ std::array<Math::Vector3d, 2> pTb = {p0Tb, p1Tb};
+ std::array<Math::Vector3d, 2> qTb = {q0Tb, q1Tb};
+
+ // We do know that no collision happen at t=a.
+ // We do know that a collision happen in between, but not sure exactly when...
+ // and more likely the 2 segments are colliding at t=b
+ if (m_staticTest.collideStaticSegmentSegment(pTb, qTb, thickness_p, thickness_q, r, s))
+ {
+ *t = b;
+ }
+ else
+ {
+ *t = ((b + a) / 2.0);
+ }
+ return true;
+ }
+ // Geometry at time t=a
+ Math::Vector3d p0Ta = Math::interpolate(pT0[0], pT1[0], a); // p[0] interpolated at time a
+ Math::Vector3d p1Ta = Math::interpolate(pT0[1], pT1[1], a); // p[1] interpolated at time a
+ Math::Vector3d q0Ta = Math::interpolate(qT0[0], qT1[0], a); // q[0] interpolated at time a
+ Math::Vector3d q1Ta = Math::interpolate(qT0[1], qT1[1], a); // q[1] interpolated at time a
+
+ // Compute intermediate geometry..and work on dichotomy based on 1/nbSubPoint (instead of 1/2)
+ double t_i[SUB_POINTS_COPLANAR_CASE + 1];
+ Math::Vector3d normal[SUB_POINTS_COPLANAR_CASE + 1];
+ double deltaT = 1.0 / SUB_POINTS_COPLANAR_CASE;
+ double r_i[SUB_POINTS_COPLANAR_CASE + 1], s_i[SUB_POINTS_COPLANAR_CASE + 1];
+ t_i[0] = a;
+ r_i[0] = *r;
+ s_i[0] = *s;
+ normal[0] = (p1Ta - p0Ta).cross(q1Ta - q0Ta).normalized();
+
+ for (int i = 1 ; i <= SUB_POINTS_COPLANAR_CASE ; i++)
+ {
+ t_i[i] = a + (b - a) * (i * deltaT);
+
+ // p and q at time i
+ std::array<Math::Vector3d, 2> p_i = {Math::interpolate(pT0[0], pT1[0], t_i[i]),
+ Math::interpolate(pT0[1], pT1[1], t_i[i])
+ };
+ std::array<Math::Vector3d, 2> q_i = {Math::interpolate(qT0[0], qT1[0], t_i[i]),
+ Math::interpolate(qT0[1], qT1[1], t_i[i])
+ };
+
+ if (m_staticTest.collideStaticSegmentSegment(p_i, q_i, thickness_p, thickness_q, &r_i[i], &s_i[i]))
+ {
+ *r = r_i[i - 1];
+ *s = s_i[i - 1];
+ return collideSegmentSegmentCoplanarCase(
+ pT0, // Segment 1 at t=0
+ pT1, // Segment 1 at t=1
+ qT0, // Segment 2 at t=0
+ qT1, // Segment 2 at t=1
+ t_i[i - 1], t_i[i], // Interval boundaries
+ timePrecisionEpsilon,
+ thickness_p, thickness_q,
+ t, r, s,
+ depth + 1);
+ }
+ normal[i] = (p_i[1] - p_i[0]).cross(q_i[1] - q_i[0]).normalized();
+ }
+
+ // Check for a flip or a change in normal. If one is detected, then a collision might occur within the
+ // current interval. Recurse and check.
+ for (int i = 1; i <= SUB_POINTS_COPLANAR_CASE; i++)
+ {
+ if (checkForCoplanarContactWithinInterval(r_i[i - 1], r_i[i], s_i[i - 1], s_i[i],
+ normal[i - 1], normal[i]))
+ {
+ double old_r = *r;
+ double old_s = *s;
+ *r = r_i[i - 1];
+ *s = s_i[i - 1];
+ bool found = collideSegmentSegmentCoplanarCase(
+ pT0, // Segment 1 at t=0
+ pT1, // Segment 1 at t=1
+ qT0, // Segment 2 at t=0
+ qT1, // Segment 2 at t=1
+ t_i[i - 1], t_i[i], // Interval boundaries
+ timePrecisionEpsilon,
+ thickness_p, thickness_q,
+ t, r, s,
+ depth + 1);
+ if (found)
+ {
+ return true;
+ }
+ *r = old_r;
+ *s = old_s;
+ }
+ }
+
+ return false;
+}
+
+bool SegmentSegmentCcdMovingContact::collideSegmentSegmentGeneralCase(
+ const SegmentSegmentCcdIntervalCheck& state,
+ double a, double b, // Interval boundaries
+ double* t, double* r, double* s,
+ int depth)
+{
+ Math::Interval<double> range(a, b);
+
+ if (state.possibleCollisionTestWithThickness(range) !=
+ SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision)
+ {
+ return false;
+ }
+
+ // Recursion bottoms out at time precision.
+ if (b - a < state.timePrecisionEpsilon())
+ {
+ std::array<Math::Vector3d, 2> pTb = {state.motionP1().atTime(b), state.motionP2().atTime(b)};
+ std::array<Math::Vector3d, 2> qTb = {state.motionQ1().atTime(b), state.motionQ2().atTime(b)};
+ *t = b;
+
+ // The recursion has bottomed out, and we should have already detected if we are colliding at t=0.
+ // Make one final check at the other end of the interval and end the recursion.
+ return m_staticTest.collideStaticSegmentSegment(pTb, qTb, state.thicknessP(), state.thicknessQ(), r, s);
+ }
+
+ // Otherwise, recursion has not yet bottomed out, go down one more level.
+ double midpoint = (a + b) * 0.5;
+
+ // Test 1st semi-interval [a, (a + b) / 2]
+ if (collideSegmentSegmentGeneralCase(
+ state,
+ a, midpoint,
+ t, r, s,
+ depth + 1))
+ {
+ return true;
+ }
+
+ // Test 2nd semi-interval [(a + b) / 2, b]
+ if (collideSegmentSegmentGeneralCase(
+ state,
+ midpoint, b,
+ t, r, s,
+ depth + 1))
+ {
+ return true;
+ }
+
+ return false;
+}
+
+void SegmentSegmentCcdMovingContact::normalizeSegmentsConsistently(Math::Vector3d* t0, Math::Vector3d* t1,
+ double epsilon) const
+{
+ // safely normalize t0 and t1. We will need to calculate
+ // dot products against them to test orthogonality with p X q at
+ // the two time points.
+ double norm_t0 = t0->norm();
+ double norm_t1 = t1->norm();
+
+ if (norm_t0 >= epsilon)
+ {
+ *t0 *= 1.0 / norm_t0;
+ if (norm_t1 >= epsilon)
+ {
+ // t0 and t1 both good
+ *t1 *= 1.0 / norm_t1;
+ }
+ else
+ {
+ // t0 good, t1 bad
+ *t1 = *t0; // t1 <- t0
+ SURGSIM_LOG_WARNING(m_logger) <<
+ "Segment is degenerate at time 1. Using time 0 " <<
+ "value for both coplanarity tests.";
+ }
+ }
+ else if (norm_t1 >= epsilon)
+ {
+ // t1 good, t0 bad
+ *t1 *= 1.0 / norm_t1;
+ *t0 = *t1; // t0 <- t1
+ SURGSIM_LOG_WARNING(m_logger) <<
+ "Segment is degenerate at time 0. Using time 1 value for " <<
+ "both coplanarity tests.";
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger) <<
+ "Segment is degenerate at time 0 and time 1. Unable to " <<
+ "normalize for coplanarity tests.";
+ }
+}
+
+bool SegmentSegmentCcdMovingContact::checkForCoplanarContactWithinInterval(
+ double rCurrent, double rNext,
+ double sCurrent, double sNext,
+ const Math::Vector3d& nCurrent, const Math::Vector3d& nNext) const
+{
+// TODO(wturner): Given the original code, this test IS ALWAYS TRUE as r and s are clamped to [0, 1]
+// and numberSubpoints > 1. I do not think this was operational and that may explain the extraordinarily
+// small epsilon (1.0e-18) being used a the gatekeeper to this function:
+//
+// return (((rCurrent >= 0 && rCurrent < 1) || (rCurrent < 0 && rNext >= 0) || (rCurrent > 1 && rNext <= 1)) &&
+// ((sCurrent >= 0 && sCurrent < 1) || (sCurrent < 0 && sNext >= 0) || (sCurrent > 1 && sNext <= 1)) &&
+// rCurrent * rNext > -(double)numberSubpoints * 0.5
+// && sCurrent * sNext > -(double)numberSubpoints * 0.5) ||
+// (nCurrent.dot(nNext) < 0.0);
+
+ return (((rCurrent > 0.0 && rCurrent < 1.0) || (rCurrent == 0.0 && rNext > 0) ||
+ (std::abs(rCurrent - 1.0) < m_distanceEpsilon && rNext < 1.0)) &&
+ ((sCurrent > 0.0 && sCurrent < 1.0) || (sCurrent == 0.0 && sNext > 0) ||
+ (std::abs(sCurrent - 1.0) < m_distanceEpsilon && sNext < 1.0))) ||
+ (nCurrent.dot(nNext) < 0.0);
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/SegmentSegmentCcdMovingContact.h b/SurgSim/Collision/SegmentSegmentCcdMovingContact.h
new file mode 100644
index 0000000..cc5fe10
--- /dev/null
+++ b/SurgSim/Collision/SegmentSegmentCcdMovingContact.h
@@ -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.
+
+#ifndef SURGSIM_COLLISION_SEGMENTSEGMENTCCDMOVINGCONTACT_H
+#define SURGSIM_COLLISION_SEGMENTSEGMENTCCDMOVINGCONTACT_H
+
+#include <array>
+
+#include "SurgSim/Collision/SegmentSegmentCcdIntervalCheck.h"
+#include "SurgSim/Collision/SegmentSegmentCcdStaticContact.h"
+#include "SurgSim/Framework/Logger.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+class SegmentMeshShape;
+};
+
+namespace Collision
+{
+
+class CollisionPair;
+
+/// SegmentSegmentCcdMovingContact computes the self collisions among a SegmentMesh under motion at two
+/// time points parametrized over the time interval [0,1]. An initial phase uses the AABB tree to
+/// select a set of potentially colliding segments from the SegmentMesh. For each of these
+/// candidate segment pairs, the goal is to determine the point of earliest contact should any exist.
+///
+/// At the highest level the actual collision detection of candidate segment pairs is a two phase
+/// algorithm. First determine if there is contact at the start of an interval and report the contact if
+/// found. If no contact is found at the start, subdivide the interval, determine which of the resulting
+/// candidate subintervals may have collisions, and then recursively check those promising subintervals.
+/// Note that a simple algorithm based on interval arithmetic (including the Interval, LinearMotion and
+/// Polynomial interval classes) allows for a quick determination of promising subintervals allowing many
+/// of the subintervals to be pruned during the subdivision step without forcing the recursion to bottom out.
+///
+/// \sa Interval, LinearMotion, Polynomial, SegmentSegmentCcdIntervalCheck
+///
+class SegmentSegmentCcdMovingContact
+{
+public:
+ static const int SUB_POINTS_PARALLEL_CASE = 5;
+ static const int SUB_POINTS_COPLANAR_CASE = 10;
+
+ /// Constructor.
+ SegmentSegmentCcdMovingContact();
+
+ /// Calculate if, where, and when the segments p and q collide in the interval from t = 0 to t = 1
+ /// for "zero" thickness segments.
+ /// \param pt0Positions are the segment endpoints for the first segment at time t=0.
+ /// \param pt1Positions are the segment endpoints for the first segment at time t=1.
+ /// \param qt0Positions are the segment endpoints for the second segment at time t=0.
+ /// \param qt1Positions are the segment endpoints for the second segment at time t=1.
+ /// \param thicknessEpsilon spatial nearness criteria for declaring a contact.
+ /// \param timePrecisionEpsilon time nearness criteria for declaring a contact.
+ /// \param t [out] parametric location of the collision along the time axes in the interval [0, 1]
+ /// \param r [out] parametric location of the collision along p in the interval [0, 1]
+ /// \param s [out] parametric location of the collision along q in the interval [0, 1]
+ /// \param pToQDir [out] direction from the contact point on p to the contact point on q
+ /// \return true if p and q collide in interval [0, 1]
+ bool collideMovingSegmentSegment(
+ const std::array<Math::Vector3d, 2>& pt0Positions,
+ const std::array<Math::Vector3d, 2>& pt1Positions,
+ const std::array<Math::Vector3d, 2>& qt0Positions,
+ const std::array<Math::Vector3d, 2>& qt1Positions,
+ double thicknessEpsilon,
+ double timePrecisionEpsilon,
+ double* t, double* r, double* s, Math::Vector3d* pToQDir);
+
+ /// Calculate if, where, and when the segments p and q collide in the interval from t = 0 to t = 1
+ /// for thick segments.
+ /// \param pt0Positions are the segment endpoints for the first segment at time t=0.
+ /// \param pt1Positions are the segment endpoints for the first segment at time t=1.
+ /// \param qt0Positions are the segment endpoints for the second segment at time t=0.
+ /// \param qt1Positions are the segment endpoints for the second segment at time t=1.
+ /// \param thicknessP radius of segment p.
+ /// \param thicknessQ radius of segment q.
+ /// \param timePrecisionEpsilon time nearness criteria for declaring a contact.
+ /// \param t [out] parametric location of the collision along the time axes in the interval [0, 1]
+ /// \param r [out] parametric location of the collision along p in the interval [0, 1]
+ /// \param s [out] parametric location of the collision along q in the interval [0, 1]
+ /// \param pToQDir [out] direction from the contact point on p to the contact point on q at time t
+ /// \return true if p and q collide in interval [0, 1]
+ bool collideMovingSegmentSegment(
+ const std::array<Math::Vector3d, 2>& pt0Positions,
+ const std::array<Math::Vector3d, 2>& pt1Positions,
+ const std::array<Math::Vector3d, 2>& qt0Positions,
+ const std::array<Math::Vector3d, 2>& qt1Positions,
+ double thicknessP,
+ double thicknessQ,
+ double timePrecisionEpsilon,
+ double* t, double* r, double* s, Math::Vector3d* pToQDir);
+
+protected:
+ /// Manage the collision of moving segments as a series of cases based on the segment
+ /// relationships over the moving interval.
+ /// \param pT0 are the segment endpoints for the first segment at time t=0.
+ /// \param pT1 are the segment endpoints for the first segment at time t=1.
+ /// \param qT0 are the segment endpoints for the second segment at time t=0.
+ /// \param qT1 are the segment endpoints for the second segment at time t=1.
+ /// \param thicknessP radius of segment p.
+ /// \param thicknessQ radius of segment q.
+ /// \param timePrecisionEpsilon time nearness criteria for declaring a contact.
+ /// \param t [out] parametric location of the collision along the time axes in the interval [0, 1]
+ /// \param r [out] parametric location of the collision along p in the interval [0, 1]
+ /// \param s [out] parametric location of the collision along q in the interval [0, 1]
+ /// \return true if p and q collide in interval [0, 1]
+ bool collideSegmentSegmentBaseCase(
+ const std::array<Math::Vector3d, 2>& pT0,
+ const std::array<Math::Vector3d, 2>& pT1,
+ const std::array<Math::Vector3d, 2>& qT0,
+ const std::array<Math::Vector3d, 2>& qT1,
+ double thicknessP,
+ double thicknessQ,
+ double timePrecisionEpsilon,
+ double* t, double* r, double* s);
+
+ /// Manage the specific case of detecting collisions between segments p and q which are parallel throughout
+ /// the parametric time interval of interest [a, b].
+ /// \param pT0 are the segment endpoints for the first segment at time t=0.
+ /// \param pT1 are the segment endpoints for the first segment at time t=1.
+ /// \param qT0 are the segment endpoints for the second segment at time t=0.
+ /// \param qT1 are the segment endpoints for the second segment at time t=1.
+ /// \param a parametric starting point of the interval of interest.
+ /// \param b parametric ending point of the interval of interest.
+ /// \param thicknessP radius of segment p.
+ /// \param thicknessQ radius of segment q.
+ /// \param timePrecisionEpsilon time nearness criteria for declaring a contact.
+ /// \param t [out] parametric location of the collision along the time axes in the interval [0, 1]
+ /// \param r [in/out] parametric location of the collision along p in the interval [0, 1]
+ /// \param s [in/out] parametric location of the collision along q in the interval [0, 1]
+ /// \param depth recursion depth.
+ /// \return true if p and q collide in interval [a, b]
+ bool collideSegmentSegmentParallelCase(
+ const std::array<Math::Vector3d, 2>& pT0,
+ const std::array<Math::Vector3d, 2>& pT1,
+ const std::array<Math::Vector3d, 2>& qT0,
+ const std::array<Math::Vector3d, 2>& qT1,
+ double a, double b,
+ double thicknessP, double thicknessQ,
+ double timePrecisionEpsilon,
+ double* t, double* r, double* s, int depth = 0);
+
+ /// Manage the specific case of detecting collisions between segments p and q which are coplanar throughout
+ /// the parametric time interval of interest [a, b].
+ /// \param pT0 are the segment endpoints for the first segment at time t=0.
+ /// \param pT1 are the segment endpoints for the first segment at time t=1.
+ /// \param qT0 are the segment endpoints for the second segment at time t=0.
+ /// \param qT1 are the segment endpoints for the second segment at time t=1.
+ /// \param a parametric starting point of the interval of interest.
+ /// \param b parametric ending point of the interval of interest.
+ /// \param thicknessP radius of segment p.
+ /// \param thicknessQ radius of segment q.
+ /// \param timePrecisionEpsilon time nearness criteria for declaring a contact.
+ /// \param t [out] parametric location of the collision along the time axes in the interval [0, 1]
+ /// \param r [in/out] parametric location of the collision along p in the interval [0, 1]
+ /// \param s [in/out] parametric location of the collision along q in the interval [0, 1]
+ /// \param depth recursion depth.
+ /// \return true if p and q collide in interval [a, b]
+ bool collideSegmentSegmentCoplanarCase(
+ const std::array<Math::Vector3d, 2>& pT0, /* Segment 1 at t=0 */
+ const std::array<Math::Vector3d, 2>& pT1, /* Segment 1 at t=1 */
+ const std::array<Math::Vector3d, 2>& qT0, /* Segment 2 at t=0 */
+ const std::array<Math::Vector3d, 2>& qT1, /* Segment 2 at t=1 */
+ double a, double b, /* Interval boundaries */
+ double timePrecisionEpsilon,
+ double thicknessP, double thicknessQ,
+ double* t, double* r, double* s,
+ int depth = 0);
+
+ /// Manage the general case of detecting collisions between segments p and q over the parametric time
+ /// interval [a, b] when no special spatial relationships can be observed that improve performance.
+ /// \param state an encapsulation of the segment locations, movements, and detection parameters.
+ /// \param a parametric starting point of the interval of interest.
+ /// \param b parametric ending point of the interval of interest.
+ /// \param t [out] parametric location of the collision along the time axes in the interval [0, 1]
+ /// \param r [out] parametric location of the collision along p in the interval [0, 1]
+ /// \param s [out] parametric location of the collision along q in the interval [0, 1]
+ /// \param depth recursion depth.
+ /// \return true if p and q collide in interval [a, b]
+ bool collideSegmentSegmentGeneralCase(
+ const SegmentSegmentCcdIntervalCheck& state,
+ double a, double b, // Interval boundaries
+ double* t, double* r, double* s,
+ int depth = 0);
+
+ /// Safely normalize segments t0 and t1 consistently with each other. Under the assumption that they
+ /// both represent the same segment at two different time points. Ensure that for cases where the segment
+ /// is too small at one or both time points (i.e. they essentially degenerate to a point) that we make
+ /// an intelligent choice.
+ /// \param t0 segment at time 0
+ /// \param t1 segment at time 1
+ /// \param epsilon threshold for valid normalization value.
+ void normalizeSegmentsConsistently(Math::Vector3d* t0, Math::Vector3d* t1, double epsilon) const;
+
+private:
+ /// Utility routine to perform a series of checks to determine if a collision is likely within an interval. The
+ /// checks seek to determine if two coplanar segments could have been out of contact at the start of an interval,
+ /// and then moved through a contact and back away. Among the values checked are the normals because a change in
+ /// normal direction of p X q indicates that p is now on the other side of q. Other checks are made for flipping
+ /// segments, etc.
+ /// \param rCurrent is the parametric location on segment p at the start of the current time interval
+ /// \param rNext is the parametric location on segment p at the end of the current time interval
+ /// \param sCurrent is the parametric location on segment q at the start of the current time interval
+ /// \param sNext is the parametric location on segment q at the end of the current time interval
+ /// \param nCurrent is the normal of p x q at the start of the current time interval
+ /// \param nNext is the normal of p x q at the current time point
+ /// \return true if the check indicates a collision may be possible for coplanar segments p and
+ /// q at the current time interval.
+ bool checkForCoplanarContactWithinInterval(double rCurrent, double rNext, double sCurrent, double sNext,
+ const Math::Vector3d& nCurrent, const Math::Vector3d& nNext) const;
+
+ /// Minimum distance precision epsilon used in continuous collision detection.
+ const double m_distanceEpsilon;
+
+ /// Utility class for testing interval boundary collisions.
+ Collision::SegmentSegmentCcdStaticContact m_staticTest;
+
+ /// Logger
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif // SURGSIM_COLLISION_SEGMENTSEGMENTCCDMOVINGCONTACT_H
diff --git a/SurgSim/Collision/SegmentSegmentCcdStaticContact.cpp b/SurgSim/Collision/SegmentSegmentCcdStaticContact.cpp
new file mode 100644
index 0000000..d856290
--- /dev/null
+++ b/SurgSim/Collision/SegmentSegmentCcdStaticContact.cpp
@@ -0,0 +1,436 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/SegmentSegmentCcdStaticContact.h"
+#include "SurgSim/Math/Scalar.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+SegmentSegmentCcdStaticContact::SegmentSegmentCcdStaticContact(): m_degenerateEpsilon(1.0e-15)
+{
+}
+
+bool SegmentSegmentCcdStaticContact::collideStaticSegmentSegment(
+ const std::array<SurgSim::Math::Vector3d, 2>& p,
+ const std::array<SurgSim::Math::Vector3d, 2>& q,
+ double distanceEpsilon,
+ double* r, double* s)
+{
+ return collideStaticSegmentSegment(p, q, distanceEpsilon / 2.0, distanceEpsilon / 2.0, r, s);
+}
+
+bool SegmentSegmentCcdStaticContact::collideStaticSegmentSegment(
+ const std::array<SurgSim::Math::Vector3d, 2>& p,
+ const std::array<SurgSim::Math::Vector3d, 2>& q,
+ double radiusP, double radiusQ,
+ double* r, double* s)
+{
+ const double totalThickness = radiusP + radiusQ;
+ const double totalThickness2 = totalThickness * totalThickness;
+
+ // Based on the outline of:
+ // https://www.assembla.com/spaces/OpenSurgSim/documents/cRWomWC2er5ykpacwqjQYw/download/cRWomWC2er5ykpacwqjQYw.
+ // We are minimizing the squared distance R(sp,sq) = (a.sp)^2 + 2b.sp.sq + (c.sq)^2 + 2d.sq + 2e.sp + f
+ // for P(s) = (1-s).P0 + s.P1 and Q(t) = (1-t).Q0 + t.Q1, and as defined in the paper:
+ // a = (P1 - P0)(P1 - P0)
+ // b = -(P1 - P0)(Q1 - Q0)
+ // c = (Q1 - Q0)(Q1 - Q0)
+ // d = (P1 - P0)(P0 - Q0)
+ // e = -(Q1 - Q0)(P0 - Q0)
+ // f = (P0 - Q0)(P0 - Q0)
+
+ // First determine if either of our segments are really points. If so this is easier. Take advantage.
+ auto p0p1 = p[1] - p[0];
+ double a = p0p1.dot(p0p1);
+ if (a <= m_degenerateEpsilon) // Degenerate segment P
+ {
+ *r = 0.0;
+ return collideStaticPointSegment(p[0], q, radiusP, radiusQ, s);
+ }
+
+ auto q0q1 = q[1] - q[0];
+ double c = q0q1.dot(q0q1);
+ if (c <= m_degenerateEpsilon) // Degenerate segment Q
+ {
+ *s = 0;
+ return collideStaticPointSegment(q[0], p, radiusQ, radiusP, r);
+ }
+
+ double b = -p0p1.dot(q0q1);
+ auto q0p0 = p[0] - q[0];
+ double d = p0p1.dot(q0p0);
+ double e = -q0q1.dot(q0p0);
+ double ratio = a * c - b * b;
+
+ // Characterize the angle between p and q. If it is big enough
+ // then handle it with the generic code. Otherwise, use an
+ // algorithm specific to parallel segments.
+ if (ratio >= m_degenerateEpsilon)
+ {
+ // This section of the code carries out the steps of the cited paper.
+ // 1. Determine the points of closest approach for the infinite lines containing p and q
+ // 2. Determine where these values fall with respect to the segment end points and which edges (if any)
+ // must be clamped to the parametric range [0.0, 1.0]
+ // 3. Calculate the parametrics for the closest approach of the segments using the edge information.
+ double infiniteLineR = b * e - c * d;
+ double infiniteLineS = b * d - a * e;
+ *r = infiniteLineR;
+ *s = infiniteLineS;
+ SegmentCcdEdgeType edge = computeCollisionEdge(a, b, d, infiniteLineR, infiniteLineS, ratio);
+ computeCollisionParametrics(edge, a, b, c, d, e, ratio, r, s);
+ }
+ else // Parallel case
+ {
+ computeParallelSegmentParametrics(a, b, d, r, s);
+ }
+
+ SURGSIM_ASSERT(*r >= 0.0 && *r <= 1.0) << "Segment collision s should be in [0,1]!";
+ SURGSIM_ASSERT(*s >= 0.0 && *s <= 1.0) << "Segment collision s should be in [0,1]!";
+
+ Vector3d pBar = Math::interpolate(p[0], p[1], *r);
+ Vector3d qBar = Math::interpolate(q[0], q[1], *s);
+
+ Vector3d pq = qBar - pBar;
+ return pq.squaredNorm() <= totalThickness2;
+}
+
+bool SegmentSegmentCcdStaticContact::collideStaticPointSegment(
+ const Math::Vector3d& point,
+ const std::array<SurgSim::Math::Vector3d, 2>& p,
+ double thicknessPoint, double thicknessSegment,
+ double* r)
+{
+ Math::Vector3d b = p[0];
+ Math::Vector3d c = p[1];
+ auto ba = point - b;
+ auto ca = point - c;
+ auto bc = c - b;
+ double baNormSQ = ba.squaredNorm();
+ double caNormSQ = ca.squaredNorm();
+ double bcNormSQ = bc.squaredNorm();
+
+ double totalThicknessSQ = (thicknessPoint + thicknessSegment) * (thicknessPoint + thicknessSegment);
+
+ // p is essentially a point
+ if (bcNormSQ < m_degenerateEpsilon)
+ {
+ if (baNormSQ <= totalThicknessSQ)
+ {
+ *r = 0.0;
+ return true;
+ }
+ if (caNormSQ <= totalThicknessSQ)
+ {
+ *r = 1.0;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+
+ // b!=c => compute the projection abscissa
+ *r = bc.dot(ba) / bcNormSQ;
+
+ // Clamp abscissa
+ *r = Math::clamp(*r, 0.0, 1.0, 0.0);
+
+ // Compute the closest point of a on [bc]
+ Math::Vector3d closestPtOnBC = Math::interpolate(b, c, *r);
+ return (closestPtOnBC - point).squaredNorm() <= totalThicknessSQ;
+}
+
+SegmentSegmentCcdStaticContact::SegmentCcdEdgeType SegmentSegmentCcdStaticContact::computeCollisionEdge(
+ double a, double b, double d,
+ double r, double s, double ratio) const
+{
+ // Region mappings from reference:
+ //
+ // r=0 r=1
+ // ^
+ // | |
+ // 4 | 3 | 2
+ // ----|-------|------- s=1
+ // | |
+ // 5 | 0 | 1
+ // | |
+ // ----|-------|-------> s=0
+ // | |
+ // 6 | 7 | 8
+ // | |
+ //
+ SegmentSegmentCcdStaticContact::SegmentCcdEdgeType edge = SegmentCcdEdgeTypeEdgeInvalid;
+
+ if (r >= 0)
+ {
+ if (r <= ratio)
+ {
+ if (s >= 0)
+ {
+ if (s <= ratio)
+ {
+ // region = 0; (0 <= r,s <= 1)
+ edge = SegmentCcdEdgeTypeEdgeSkip;
+ }
+ else
+ {
+ // region = 3; (0 <= r <= 1; 1 <= s)
+ edge = SegmentCcdEdgeTypeS1;
+ }
+ }
+ else
+ {
+ // region = 7; (0 <= r <= 1; s <= 0)
+ edge = SegmentCcdEdgeTypeS0;
+ }
+ }
+ else
+ {
+ if (s >= 0)
+ {
+ if (s <= ratio)
+ {
+ // region = 1; (1 <= r; 0 <= s <= 1)
+ edge = SegmentCcdEdgeTypeR1;
+ }
+ else
+ {
+ // region = 2; (1 <= r,s)
+ if (a + b + d > 0)
+ {
+ edge = SegmentCcdEdgeTypeS1;
+ }
+ else
+ {
+ edge = SegmentCcdEdgeTypeR1;
+ }
+ }
+ }
+ else
+ {
+ // region = 8; (1 <= r; s <= 0)
+ if (a + d > 0)
+ {
+ edge = SegmentCcdEdgeTypeS0;
+ }
+ else
+ {
+ edge = SegmentCcdEdgeTypeR1;
+ }
+ }
+ }
+ }
+ else
+ {
+ if (s >= 0)
+ {
+ if (s <= ratio)
+ {
+ // region = 5;(r <= 0; 0 <= s <= 1)
+ edge = SegmentCcdEdgeTypeR0;
+ }
+ else
+ {
+ // region = 4; (r <= 0; 1 <= s)
+ if (b + d > 0)
+ {
+ edge = SegmentCcdEdgeTypeR0;
+ }
+ else
+ {
+ edge = SegmentCcdEdgeTypeS1;
+ }
+ }
+ }
+ else
+ {
+ // region = 6; (r <= 0; s <= 0)
+ if (d > 0)
+ {
+ edge = SegmentCcdEdgeTypeR0;
+ }
+ else
+ {
+ edge = SegmentCcdEdgeTypeS0;
+ }
+ }
+ }
+ return edge;
+}
+
+void SegmentSegmentCcdStaticContact::computeCollisionParametrics(SegmentCcdEdgeType edge,
+ double a, double b, double c, double d, double e,
+ double ratio, double* r, double* s) const
+{
+ // On entry r and s are parametrically calculated based on infinite lines, i.e., r and s may not lie in [0, 1]
+ // *r = b * e - c * d
+ // *s = b * d - a * e
+ double tmp;
+ switch (edge)
+ {
+ case SegmentCcdEdgeTypeR0:
+ // F(s) = Q(0,s), F?(s) = 2*(e+c*s)
+ // F?(T) = 0 when T = -e/c, then clamp between 0 and 1 (c always >= 0)
+ *r = 0.0;
+ tmp = -e;
+ if (tmp < 0)
+ {
+ *s = 0;
+ }
+ else if (tmp > c)
+ {
+ *s = 1.0;
+ }
+ else
+ {
+ *s = tmp / c;
+ }
+ break;
+ case SegmentCcdEdgeTypeR1:
+ // F(s) = Q(1,s), F?(s) = 2*((b+e)+c*s)
+ // F?(T) = 0 when T = -(b+e)/c, then clamp between 0 and 1 (c always >= 0)
+ *r = 1.0;
+ tmp = -b - e;
+ if (tmp < 0)
+ {
+ *s = 0.0;
+ }
+ else if (tmp > c)
+ {
+ *s = 1.0;
+ }
+ else
+ {
+ *s = tmp / c;
+ }
+ break;
+ case SegmentCcdEdgeTypeS0:
+ // F(r) = Q(r,0), F?(r) = 2*(d+a*r) =>
+ // F?(S) = 0 when S = -d/a, then clamp between 0 and 1 (a always >= 0)
+ *s = 0.0;
+ tmp = -d;
+ if (tmp < 0)
+ {
+ *r = 0.0;
+ }
+ else if (tmp > a)
+ {
+ *r = 1.0;
+ }
+ else
+ {
+ *r = tmp / a;
+ }
+ break;
+ case SegmentCcdEdgeTypeS1:
+ // F(r) = Q(r,1), F?(r) = 2*(b+d+a*r) =>
+ // F?(S) = 0 when S = -(b+d)/a, then clamp between 0 and 1 (a always >= 0)
+ *s = 1.0;
+ tmp = -b - d;
+ if (tmp < 0.0)
+ {
+ *r = 0.0;
+ }
+ else if (tmp > a)
+ {
+ *r = 1.0;
+ }
+ else
+ {
+ *r = tmp / a;
+ }
+ break;
+ case SegmentCcdEdgeTypeEdgeSkip:
+ tmp = 1.0 / ratio;
+ *r *= tmp;
+ *s *= tmp;
+ break;
+ default:
+ break;
+ }
+}
+
+void SegmentSegmentCcdStaticContact::computeParallelSegmentParametrics(double a, double b, double d, double* r,
+ double* s) const
+{
+ if (b > 0.0)
+ {
+ // Segments have different directions
+ if (d >= 0.0)
+ {
+ // 0-0 end points since r-segment 0 less than s-segment 0
+ *r = 0.0;
+ *s = 0.0;
+ }
+ else if (-d <= a)
+ {
+ // s-segment 0 end-point in the middle of the r 0-1 segment, get distance
+ *r = -d / a;
+ *s = 0.0;
+ }
+ else
+ {
+ // r-segment 1 is definitely closer
+ *r = 1.0;
+ double tmp = a + d;
+ if (-tmp >= b)
+ {
+ *s = 1.0;
+ }
+ else
+ {
+ *s = -tmp / b;
+ }
+ }
+ }
+ else
+ {
+ // Both segments have the same dir
+ if (-d >= a)
+ {
+ // 1-0
+ *r = 1.0;
+ *s = 0.0;
+ }
+ else if (d <= 0.0)
+ {
+ // mid-0
+ *r = -d / a;
+ *s = 0.0;
+ }
+ else
+ {
+ *r = 0.0;
+ // 1-mid
+ if (d >= -b)
+ {
+ *s = 1.0;
+ }
+ else
+ {
+ *s = -d / b;
+ }
+ }
+ }
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/SegmentSegmentCcdStaticContact.h b/SurgSim/Collision/SegmentSegmentCcdStaticContact.h
new file mode 100644
index 0000000..08f4ee8
--- /dev/null
+++ b/SurgSim/Collision/SegmentSegmentCcdStaticContact.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_COLLISION_SEGMENTSEGMENTCCDSTATICCONTACT_H
+#define SURGSIM_COLLISION_SEGMENTSEGMENTCCDSTATICCONTACT_H
+
+#include <array>
+
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+/// SegmentSegmentCcdStaticContact computes if there is contact between two segments at a specific point in time
+/// in support of the CCD calculations for moving intervals. Algorithm optimizations improve performance for
+/// specific orientations and positions of segments such as parallel segments, or segments where the closest
+/// approach is at one or both of the segment endpoints.
+///
+/// \sa SegmentSegmentCcdIntervalCheck
+///
+class SegmentSegmentCcdStaticContact
+{
+public:
+ enum SegmentCcdEdgeType
+ {
+ SegmentCcdEdgeTypeR0, // Closest approach occurs at parametric value r = 0
+ SegmentCcdEdgeTypeR1, // Closest approach occurs at parametric value r = 1
+ SegmentCcdEdgeTypeS0, // Closest approach occurs at parametric value s = 0
+ SegmentCcdEdgeTypeS1, // Closest approach occurs at parametric value s = 1
+ SegmentCcdEdgeTypeEdgeSkip, // Closest approach is not at segment boundary (0.0 <= r,s, <= 1.0)
+ SegmentCcdEdgeTypeEdgeInvalid // Invalid value
+ };
+
+ /// Constructor.
+ SegmentSegmentCcdStaticContact();
+
+ /// Determine whether two "zero radius" segments collide. For moving segments, this represents contact at a
+ /// specific point in time.
+ /// \param p segment 1 endpoints.
+ /// \param q segment 2 endpoints.
+ /// \param distanceEpsilon closeness parameter for the zero thickness collision.
+ /// \param r [out] parametric location of the collision point (if any) on segment 1.
+ /// \param s [out] parametric location of the collision point (if any) on segment 2.
+ /// \return false if no collision is occurring, or true otherwise.
+ bool collideStaticSegmentSegment(
+ const std::array<SurgSim::Math::Vector3d, 2>& p,
+ const std::array<SurgSim::Math::Vector3d, 2>& q,
+ double distanceEpsilon,
+ double* r, double* s);
+
+ /// Determine whether two thick segments collide. For moving segments, this represents contact at a
+ /// specific point in time.
+ /// \param p segment 1 endpoints.
+ /// \param q segment 2 endpoints.
+ /// \param radiusP thickness of segment 1.
+ /// \param radiusQ thickness of segment 2.
+ /// \param r [out] parametric location of the collision point (if any) on segment 1.
+ /// \param s [out] parametric location of the collision point (if any) on segment 2.
+ /// \return false if no collision is occurring, or true otherwise.
+ bool collideStaticSegmentSegment(
+ const std::array<SurgSim::Math::Vector3d, 2>& p,
+ const std::array<SurgSim::Math::Vector3d, 2>& q,
+ double radiusP, double radiusQ,
+ double* r, double* s);
+
+protected:
+ /// Determine whether a single point and a segment collide.
+ /// \param point point position.
+ /// \param p segment endpoints.
+ /// \param thicknessPoint radius of the point.
+ /// \param thicknessSegment radius of the segment.
+ /// \param r [out] parametric location of the collision point (if any) on segment p.
+ /// \return false if no collision is occurring, or true otherwise.
+ bool collideStaticPointSegment(
+ const Math::Vector3d& point,
+ const std::array<SurgSim::Math::Vector3d, 2>& p,
+ double thicknessPoint, double thicknessSegment,
+ double* r
+ );
+
+ /// Find the edge to be clamped for the closest point solution using the outline of:
+ /// https://www.assembla.com/spaces/OpenSurgSim/documents/cRWomWC2er5ykpacwqjQYw/download/cRWomWC2er5ykpacwqjQYw
+ ///
+ /// Calculates the parametric value that must be clamped in determining the segment -
+ /// segment distance where:
+ /// SegmentCcdEdgeTypeR0 clamp parametric value r to 0
+ /// SegmentCcdEdgeTypeR1 clamp parametric value r to 1
+ /// SegmentCcdEdgeTypeS0 clamp parametric value s to 0
+ /// SegmentCcdEdgeTypeS1 clamp parametric value s to 1
+ /// SegmentCcdEdgeSkip both values are with [0, 1]
+ /// a = (P1 - P0)(P1 - P0)
+ /// b = -(P1 - P0)(Q1 - Q0)
+ /// c = (Q1 - Q0)(Q1 - Q0)
+ /// d = (P1 - P0)(P0 - Q0)
+ /// e = -(Q1 - Q0)(P0 - Q0)
+ /// f = (P0 - Q0)(P0 - Q0)
+ /// \param a value of p dot p
+ /// \param b value of -(p dot q)
+ /// \param d value of p dot (q[0] - p[0])
+ /// \param r unnormalized parametric location of the intersection point on line p
+ /// \param s unnormalized parametric location of the intersection point on line q
+ /// \param ratio normalization value defined as (p dot p) . (q dot q) - (p dot q)^2.
+ /// \return an indicator of the edge (r and s) which must be clamped and its clamp value.
+ SegmentCcdEdgeType computeCollisionEdge(double a, double b, double d,
+ double r, double s, double ratio) const;
+
+ /// Given an edge indicator, clamp the indicated parametric edge and calculate the minimum parametric
+ /// value for the other segment using the outline of:
+ /// https://www.assembla.com/spaces/OpenSurgSim/documents/cRWomWC2er5ykpacwqjQYw/download/cRWomWC2er5ykpacwqjQYw
+ /// Definitions of the values are:
+ /// a = (P1 - P0)(P1 - P0)
+ /// b = -(P1 - P0)(Q1 - Q0)
+ /// c = (Q1 - Q0)(Q1 - Q0)
+ /// d = (P1 - P0)(P0 - Q0)
+ /// e = -(Q1 - Q0)(P0 - Q0)
+ /// f = (P0 - Q0)(P0 - Q0)
+ /// \param edge indicator of previously calculated edge constraint
+ /// \param a value of p dot p
+ /// \param b value of -(p dot q)
+ /// \param c value of q dot q
+ /// \param d value of p dot (p[0] - q[0])
+ /// \param e value of -(q dot (p[0] - q[0]))
+ /// \param ratio normalization value defined as (p dot p) . (q dot q) - (p dot q)^2.
+ /// \param r [out] parametric location of the intersection point on segment p
+ /// \param s [out] parametric location of the intersection point on segment q
+ void computeCollisionParametrics(SegmentCcdEdgeType edge, double a, double b, double c, double d, double e,
+ double ratio, double* r, double* s) const;
+
+ /// Calculate the parametric values that give the minimum distance for two parallel segments
+ /// value for the other edge. Definitions of the values are:
+ /// a = (P1 - P0)(P1 - P0)
+ /// b = -(P1 - P0)(Q1 - Q0)
+ /// c = (Q1 - Q0)(Q1 - Q0)
+ /// d = (P1 - P0)(P0 - Q0)
+ /// e = -(Q1 - Q0)(P0 - Q0)
+ /// f = (P0 - Q0)(P0 - Q0)
+ /// \param a value of p dot p
+ /// \param b value of -(p dot q)
+ /// \param d value of p dot (p[0] - q[0])
+ /// \param r [out] parametric location of the intersection point on segment p
+ /// \param s [out] parametric location of the intersection point on segment q
+ void computeParallelSegmentParametrics(double a, double b, double d, double* r, double* s) const;
+
+private:
+ /// During collision, points closer than this value are considered a single point
+ const double m_degenerateEpsilon;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif // SURGSIM_COLLISION_SEGMENTSEGMENTCCDSTATICCONTACT_H
diff --git a/SurgSim/Collision/SegmentSelfContact.cpp b/SurgSim/Collision/SegmentSelfContact.cpp
new file mode 100644
index 0000000..fad8fed
--- /dev/null
+++ b/SurgSim/Collision/SegmentSelfContact.cpp
@@ -0,0 +1,567 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/SegmentSelfContact.h"
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Collision/SegmentSegmentCcdMovingContact.h"
+#include "SurgSim/Collision/SegmentSegmentCcdStaticContact.h"
+#include "SurgSim/DataStructures/AabbTreeNode.h"
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/Scalar.h"
+#include "SurgSim/Math/SegmentMeshShape.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::MeshShape;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+SegmentSelfContact::SegmentSelfContact():
+ m_distanceEpsilon(1.0e-04),
+ m_timeMinPrecisionEpsilon(1.0e-06),
+ m_timeMaxPrecisionEpsilon(1.0),
+ m_maxMovementThreshold(0.1),
+ m_useSegmentThickness(true),
+ m_logger(Framework::Logger::getLogger("Collision/SegmentSelfContact"))
+{
+}
+
+std::pair<int, int> SegmentSelfContact::getShapeTypes()
+{
+ return std::pair<int, int>(Math::SHAPE_TYPE_SEGMENTMESH, Math::SHAPE_TYPE_SEGMENTMESH);
+}
+
+void SegmentSelfContact::setTimeMinPrecisionEpsilon(double precision)
+{
+ SURGSIM_ASSERT(precision > 0.0) << "Cannot set a negative min/max time precision.";
+ m_timeMinPrecisionEpsilon = precision;
+}
+
+double SegmentSelfContact::getTimeMinPrecisionEpsilon()
+{
+ return m_timeMinPrecisionEpsilon;
+}
+
+void SegmentSelfContact::setTimeMaxPrecisionEpsilon(double precision)
+{
+ SURGSIM_ASSERT(precision > 0.0) << "Cannot set a negative min/max time precision.";
+ m_timeMaxPrecisionEpsilon = precision;
+}
+
+double SegmentSelfContact::getTimeMaxPrecisionEpsilon()
+{
+ return m_timeMaxPrecisionEpsilon;
+}
+
+void SegmentSelfContact::setDistanceEpsilon(double precision)
+{
+ m_distanceEpsilon = precision;
+}
+
+double SegmentSelfContact::distanceEpsilon()
+{
+ return m_distanceEpsilon;
+}
+
+std::list<std::shared_ptr<Contact>> SegmentSelfContact::calculateCcdContact(
+ const Math::SegmentMeshShape& segmentShape1AtTime0,
+ const Math::RigidTransform3d& segmentPose1AtTime0,
+ const Math::SegmentMeshShape& segmentShape1AtTime1,
+ const Math::RigidTransform3d& segmentPose1AtTime1,
+ const Math::SegmentMeshShape& segmentShape2AtTime0,
+ const Math::RigidTransform3d& segmentPose2AtTime0,
+ const Math::SegmentMeshShape& segmentShape2AtTime1,
+ const Math::RigidTransform3d& segmentPose2AtTime1) const
+{
+
+ const Math::SegmentMeshShape& segmentShape1 = segmentShape1AtTime0;
+ const Math::RigidTransform3d& segmentPose1 = segmentPose1AtTime0;
+ const Math::SegmentMeshShape& segmentShape2 = segmentShape1AtTime1;
+ const Math::RigidTransform3d& segmentPose2 = segmentPose1AtTime1;
+
+ SURGSIM_ASSERT(segmentShape1.getNumEdges() == segmentShape2.getNumEdges()) <<
+ "Segment CCD self collision detects that " <<
+ "the segment at time t and time t + 1 have different numbers of edges.";
+
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ // Intersect the AABB trees of the Segment Mesh at time 0 and time 1 to get a list of
+ // potential intersecting segments.
+ std::set<std::pair<size_t, size_t>> segmentIds;
+
+ // Use a local aabb tree for the movement volume to calculate the first set of possible intersections.
+ auto const& edges1 = segmentShape1.getEdges();
+ auto const& edges2 = segmentShape2.getEdges();
+ const Math::Vector3d halfExtent = Math::Vector3d(segmentShape1.getRadius(), segmentShape1.getRadius(),
+ segmentShape1.getRadius());
+ std::list<DataStructures::AabbTreeData::Item> items;
+ for (size_t id = 0; id < edges1.size(); ++id)
+ {
+ if (edges1[id].isValid && edges2[id].isValid)
+ {
+ const auto& vertices1 = segmentShape1.getEdgePositions(id);
+ Math::Aabbd aabb((vertices1[0] - halfExtent).eval());
+ aabb.extend((vertices1[0] + halfExtent).eval());
+ aabb.extend((vertices1[1] - halfExtent).eval());
+ aabb.extend((vertices1[1] + halfExtent).eval());
+ const auto& vertices2 = segmentShape2.getEdgePositions(id);
+ aabb.extend((vertices2[0] - halfExtent).eval());
+ aabb.extend((vertices2[0] + halfExtent).eval());
+ aabb.extend((vertices2[1] - halfExtent).eval());
+ aabb.extend((vertices2[1] + halfExtent).eval());
+ items.emplace_back(aabb, id);
+ }
+ }
+ DataStructures::AabbTree tree;
+ tree.set(std::move(items));
+
+ std::list<DataStructures::AabbTree::TreeNodePairType> intersectionList = tree.spatialJoin(tree);
+ getUniqueCandidates(intersectionList, &segmentIds);
+
+ size_t evaluations = 0;
+ for (const auto& idPair : segmentIds)
+ {
+ size_t id1 = idPair.first;
+ size_t id2 = idPair.second;
+
+ SURGSIM_ASSERT(id1 >= 0 && id1 < segmentShape1.getNumEdges()) << "Invalid segment detected in "
+ << "Segment CCD self collision. Colliding segment at time point 0 does not exist.";
+ SURGSIM_ASSERT(id2 >= 0 && id2 < segmentShape2.getNumEdges()) << "Invalid segment detected in "
+ << "Segment CCD self collision. Colliding segment at time point 1 does not exist.";
+
+ // Do a little filtering. We do not allow a segment to collide with itself or
+ // with an immediate neighbor; and pragmatically, it seems reasonable to disregard any segments that show
+ // too much movement between times. At best this means that we should be taking smaller time steps,
+ // but it is probably more likely to reflect some other error such as an unstable solution.
+ if (removeInvalidCollisions(segmentShape1, segmentShape2, id1, id2))
+ {
+ continue;
+ }
+
+ evaluations++;
+ const auto& pt0Positions = segmentShape1.getEdgePositions(id1);
+ const auto& pt1Positions = segmentShape2.getEdgePositions(id1);
+ const auto& qt0Positions = segmentShape1.getEdgePositions(id2);
+ const auto& qt1Positions = segmentShape2.getEdgePositions(id2);
+
+ double segmentRadius1 = 0.0;
+ double segmentRadius2 = 0.0;
+ double effectiveThickness = m_distanceEpsilon;
+ if (m_useSegmentThickness)
+ {
+ // TODO(wdturner-11/2015): We need to get thickness as a property. Until then use
+ // the radius ...
+ //
+ // segmentRadius1 = segmentA->getEdge(id1).data.thickness;
+ // segmentRadius2 = segmentB->getEdge(id2).data.thickness;
+ segmentRadius1 = segmentShape1.getRadius();
+ segmentRadius2 = segmentShape2.getRadius();
+ effectiveThickness = segmentRadius1 + segmentRadius2;
+ }
+
+ //
+ // Based on movement speed, calculate the maximum time interval that will maintain detection accuracy.
+ //
+ double timePrecision = maxTimePrecision(pt0Positions, pt1Positions, qt0Positions, qt1Positions,
+ effectiveThickness);
+
+ double pLen = (pt0Positions[1] - pt0Positions[0]).squaredNorm();
+ double rParametricPrecision = m_distanceEpsilon;
+ if (pLen > 0.0)
+ {
+ rParametricPrecision = std::min(0.5, m_distanceEpsilon / std::sqrt(pLen));
+ }
+
+ double qLen = (pt1Positions[1] - pt1Positions[0]).squaredNorm();
+ double sParametricPrecision = m_distanceEpsilon;
+ if (qLen > 0.0)
+ {
+ sParametricPrecision = std::min(0.5, m_distanceEpsilon / std::sqrt(qLen));
+ }
+
+ // Perform the actual collision detection and create the contact.
+ double r; // Parametric location of collision on segment p
+ double s; // Parametric location of collision on segment q
+ double t; // Time of collision (if any)
+ Math::Vector3d pToQDir;
+ Math::Vector3d segmentPContact;
+ Math::Vector3d segmentQContact;
+ if (detectCollision(pt0Positions, pt1Positions, qt0Positions, qt1Positions,
+ segmentRadius1, segmentRadius2, timePrecision,
+ &r, &s, &t, &pToQDir, &segmentPContact, &segmentQContact))
+ {
+ // The segments collide within tolerance, but if the collision is really close to an endpoint
+ // then move it to the start of the segment to aid in removing duplicates.
+ r = Math::clamp(r, 0.0, 1.0, rParametricPrecision);
+ s = Math::clamp(s, 0.0, 1.0, sParametricPrecision);
+
+ // When a segment extremity collides, its collision can be detected twice,
+ // as this point is shared between 2 segments! Here, we choose *one* of them to add!
+ // Be sure to check both directions.
+ if (!findSegSegContact(segmentShape1, contacts, t,
+ COLLISION_DETECTION_TYPE_CONTINUOUS,
+ id1, r, id2, s, timePrecision) &&
+ !findSegSegContact(segmentShape1, contacts, t,
+ COLLISION_DETECTION_TYPE_CONTINUOUS,
+ id2, s, id1, r, timePrecision))
+ {
+ // Encode the segment specific intersection points for later recall. Here we encode each
+ // side of the contact point specific to each segment.
+ std::pair<Location, Location> penetrationPoints;
+ Math::Vector2d parametricCoordinateP(1.0 - r, r);
+ penetrationPoints.first.elementMeshLocalCoordinate.setValue(
+ DataStructures::IndexedLocalCoordinate(id1, parametricCoordinateP));
+ Math::Vector2d parametricCoordinateQ(1.0 - s, s);
+ penetrationPoints.second.elementMeshLocalCoordinate.setValue(
+ DataStructures::IndexedLocalCoordinate(id2, parametricCoordinateQ));
+ penetrationPoints.first.rigidLocalPosition.setValue(
+ segmentPose1.inverse() * segmentPContact);
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ segmentPose2.inverse() * segmentQContact);
+
+ // Calculate the normal and the contact point. The contact point is a point along the normal
+ // connecting the segments. If we are using the segment thickness, the point should be located
+ // a segment radius distance from each individual contact point. Otherwise, it should be within
+ // m_distanceEpsilon/2. For accuracy, it is calculated from both starting points and then averaged.
+ double effectiveRadiusP = m_useSegmentThickness ? segmentRadius1 : m_distanceEpsilon / 2.0;
+ double effectiveRadiusQ = m_useSegmentThickness ? segmentRadius2 : m_distanceEpsilon / 2.0;
+
+ auto normal = pToQDir.normalized();
+ Math::Vector3d contactP = segmentPContact + (effectiveRadiusP * normal);
+ Math::Vector3d contactQ = segmentQContact - (effectiveRadiusQ * normal);
+ auto contactPoint = 0.5 * (contactP + contactQ);
+ auto depth = ((contactP - contactQ).dot(normal) > 0.0) ?
+ (contactP - contactQ).norm() : -(contactP - contactQ).norm();
+ SURGSIM_LOG_DEBUG(m_logger) << "Contact detected: time:\t" << t << "\tsegment 1 id:\t"
+ << id1 << "\tsegment 2 id:\t" << id2
+ << "\tbarymetric coordinate r:\t" << r << "\tbarymetric coordinate s:\t"
+ << s << "\tMagnitude:\t" <<
+ pToQDir.norm() << "\tDepth:\t" << depth;
+ contacts.emplace_back(std::make_shared<Contact>(
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, depth, t,
+ contactPoint, -normal, penetrationPoints));
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_DEBUG(m_logger) << "AABB tree detected false positive between segments "
+ << id1 << " and " << id2;
+ }
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "Segment intersections detected: " << segmentIds.size() <<
+ "\tActual intersections evaluated: " << evaluations;
+ return contacts;
+}
+
+bool SegmentSelfContact::detectCollision(
+ const std::array<Math::Vector3d, 2>& pt0Positions,
+ const std::array<Math::Vector3d, 2>& pt1Positions,
+ const std::array<Math::Vector3d, 2>& qt0Positions,
+ const std::array<Math::Vector3d, 2>& qt1Positions,
+ double segmentRadius1, double segmentRadius2, double timePrecision,
+ double* r, double* s, double* t,
+ Math::Vector3d* pToQDir,
+ Math::Vector3d* contactPtP,
+ Math::Vector3d* contactPtQ) const
+{
+ //
+ // First check for intersection at the start of the interval. If this is true, we can just report on the
+ // contact at t=0 and return. Note that the original code compared totalThickness == 0.0. This is slightly
+ // different in that it assumes that totalThickness cannot be less than m_distanceEpsilon since
+ // m_distanceEpsilon represents the 0 segment thickness case.
+ //
+ // Important safety note. These calls to compute the static segment collision originally had two flavors: one
+ // for segments with thickness and one for segments with no thickness (which used an attributed thickness of
+ // m_distanceEpsilon). I dug into this and the only difference in the two was in the handling of potential
+ // intersections beyond the segment end point. For segments with thickness, the intersection point was
+ // clamped to the end of the segment but was deemed real if the modified endpoint was within an acceptable
+ // distance of the true intersection point as determined by the segment radius. For the zero radius case,
+ // this margin at the segment ends was not observed. While this implementation maintains the two different
+ // APIs, it does not maintain the algorithmic differences. The duplicate collision removal as implemented
+ // should resolve any places within a string of segments where extra collisions are detected and the only
+ // observable change should be at unattached segment boundaries where it is arguable as to which method is
+ // more correct. Certainly, it will be easier to selectively impose this condition should it prove desirable,
+ // than it is to maintain two nearly identical methods as was done previously.
+ //
+ double totalThickness = segmentRadius1 + segmentRadius2;
+ bool collidingAtT0;
+ static SegmentSegmentCcdStaticContact staticContact;
+ if (totalThickness <= m_distanceEpsilon)
+ {
+ collidingAtT0 = staticContact.collideStaticSegmentSegment(pt0Positions, qt0Positions,
+ m_distanceEpsilon, r, s);
+ }
+ else
+ {
+ collidingAtT0 = staticContact.collideStaticSegmentSegment(pt0Positions, qt0Positions,
+ segmentRadius1, segmentRadius2, r, s);
+ }
+
+ if (collidingAtT0)
+ {
+ *t = 0;
+ *contactPtP = Math::interpolate(pt0Positions[0], pt0Positions[1], *r);
+ *contactPtQ = Math::interpolate(qt0Positions[0], qt0Positions[1], *s);
+ *pToQDir = *contactPtQ - *contactPtP;
+ return true;
+ }
+
+ //
+ // Finally, detect if the segments collide throughout the period of movement.
+ //
+ bool collisionDetected;
+
+ static SegmentSegmentCcdMovingContact movingContact;
+ if (totalThickness < m_distanceEpsilon)
+ {
+ collisionDetected = movingContact.collideMovingSegmentSegment(pt0Positions, pt1Positions,
+ qt0Positions, qt1Positions,
+ m_distanceEpsilon, timePrecision,
+ t, r, s, pToQDir);
+ }
+ else
+ {
+ collisionDetected = movingContact.collideMovingSegmentSegment(pt0Positions, pt1Positions,
+ qt0Positions, qt1Positions,
+ segmentRadius1, segmentRadius2, timePrecision, t, r, s, pToQDir);
+ }
+
+ //
+ // If a collision is detected, use interpolation to determine the point in time and space.
+ //
+ if (collisionDetected)
+ {
+ Math::Vector3d p0Contact = Math::interpolate(pt0Positions[0], pt0Positions[1], *r);
+ Math::Vector3d p1Contact = Math::interpolate(pt1Positions[0], pt1Positions[1], *r);
+ Math::Vector3d q0Contact = Math::interpolate(qt0Positions[0], qt0Positions[1], *s);
+ Math::Vector3d q1Contact = Math::interpolate(qt1Positions[0], qt1Positions[1], *s);
+
+ *contactPtP = Math::interpolate(p0Contact, p1Contact, *t);
+ *contactPtQ = Math::interpolate(q0Contact, q1Contact, *t);
+
+ *pToQDir = *contactPtQ - *contactPtP;
+ }
+
+ return collisionDetected;
+}
+
+void SegmentSelfContact::getUniqueCandidates(
+ const std::list<SurgSim::DataStructures::AabbTree::TreeNodePairType>& intersectionList,
+ std::set<std::pair<size_t, size_t>>* segmentIds) const
+{
+ for (const auto& intersection : intersectionList)
+ {
+ std::shared_ptr<DataStructures::AabbTreeNode> nodeA = intersection.first;
+ std::shared_ptr<DataStructures::AabbTreeNode> nodeB = intersection.second;
+ std::list<size_t> localIdListA;
+ std::list<size_t> localIdListB;
+
+ nodeA->getIntersections(nodeB->getAabb(), &localIdListA);
+ nodeB->getIntersections(nodeA->getAabb(), &localIdListB);
+ for (const auto& idA : localIdListA)
+ {
+ for (const auto& idB : localIdListB)
+ {
+ // Segments are the same
+ if (idA == idB)
+ {
+ continue;
+ }
+
+ // Segment pair has already been added
+ auto testValue = (idA < idB) ? std::pair<size_t, size_t>(idA, idB) :
+ std::pair<size_t, size_t>(idB, idA);
+ segmentIds->insert(testValue);
+ }
+ }
+ }
+}
+
+bool SegmentSelfContact::removeInvalidCollisions(
+ const Math::SegmentMeshShape& segmentT0,
+ const Math::SegmentMeshShape& segmentT1,
+ size_t id1, size_t id2) const
+{
+ auto& verticesA = segmentT0.getEdge(id1).verticesId;
+ auto& verticesB = segmentT1.getEdge(id2).verticesId;
+ if ((verticesA[0] == verticesB[0]) ||
+ (verticesA[0] == verticesB[1]) ||
+ (verticesA[1] == verticesB[0]) ||
+ (verticesA[1] == verticesB[1]))
+ {
+ SURGSIM_LOG_DEBUG(m_logger)
+ << "Locality: Edge " << id1 << " and "
+ << id2 << " share a common vertex.";
+ return true;
+ }
+
+ const auto& pt0Positions = segmentT0.getEdgePositions(id1);
+ const auto& pt1Positions = segmentT1.getEdgePositions(id1);
+ const auto& qt0Positions = segmentT0.getEdgePositions(id2);
+ const auto& qt1Positions = segmentT1.getEdgePositions(id2);
+
+ auto p1t0 = pt0Positions[0];
+ auto p2t0 = pt0Positions[1];
+ auto p1t1 = pt1Positions[0];
+ auto p2t1 = pt1Positions[1];
+
+ auto q1t0 = qt0Positions[0];
+ auto q2t0 = qt0Positions[1];
+ auto q1t1 = qt1Positions[0];
+ auto q2t1 = qt1Positions[1];
+
+ if (detectExcessMovement(p1t0, p1t1, m_maxMovementThreshold) ||
+ detectExcessMovement(p2t0, p2t1, m_maxMovementThreshold) ||
+ detectExcessMovement(q1t0, q1t1, m_maxMovementThreshold) ||
+ detectExcessMovement(q2t0, q2t1, m_maxMovementThreshold))
+ {
+ SURGSIM_LOG_WARNING(m_logger)
+ << "Excessive movement detected during contact evaluation";
+ return true;
+ }
+ return false;
+}
+
+bool SegmentSelfContact::detectExcessMovement(const Math::Vector3d& pt0,
+ const Math::Vector3d& pt1,
+ double threshold) const
+{
+ return (std::abs(pt0[0] - pt1[0]) > threshold) ||
+ (std::abs(pt0[1] - pt1[1]) > threshold) ||
+ (std::abs(pt0[2] - pt1[2]) > threshold);
+
+}
+
+bool SegmentSelfContact::findSegSegContact(const Math::SegmentMeshShape& segmentShape,
+ const std::list<std::shared_ptr<Contact>>& contacts,
+ double t, Collision::CollisionDetectionType collisionType, size_t segId1, double s1,
+ size_t segId2, double s2, double timeEpsilon) const
+{
+ for (const auto& contact : contacts)
+ {
+ auto existingSegId1 = contact->penetrationPoints.first.elementMeshLocalCoordinate.getValue().index;
+ auto existingSegId2 = contact->penetrationPoints.second.elementMeshLocalCoordinate.getValue().index;
+ auto existingS1 = contact->penetrationPoints.first.elementMeshLocalCoordinate.getValue().coordinate[1];
+ auto existingS2 = contact->penetrationPoints.second.elementMeshLocalCoordinate.getValue().coordinate[1];
+
+ // Check for same object type and same time. The test for same objectID was
+ // removed as it did not appear to be used (all find segment calls set that
+ // variable to -1) and because it is not available here.
+ if (contact->type == collisionType &&
+ std::abs(contact->time - t) < timeEpsilon)
+ {
+ // Check that it is the same contact point on both segments
+ if (isSameSegContactPoint(segmentShape, segId1, s1, existingSegId1, existingS1))
+ {
+ // Colliding against the same segment (no need to check the abscissa)
+ // Then check for end point cases, which can have different segIDs
+ if ((existingSegId2 == segId2) ||
+ isSameSegContactPoint(segmentShape, segId2, s2, existingSegId2, existingS2))
+ {
+ return true;
+ }
+
+ }
+ }
+ }
+ return false;
+}
+
+bool SegmentSelfContact::isSameSegContactPoint(const Math::SegmentMeshShape& segmentShape,
+ size_t segId1, double s1, size_t segId2, double s2) const
+{
+
+ auto& verticesA = segmentShape.getEdge(segId1).verticesId;
+ auto& verticesB = segmentShape.getEdge(segId2).verticesId;
+
+ if (segId1 == segId2 && // Same segment?
+ std::fabs(s1 - s2) < m_distanceEpsilon) // Same abscissa?
+ {
+ return true;
+ }
+
+ // Check for segment extremities, which may be shared with another segment. Unlike the previous version,
+ // which used ordering of the segments to determine "sharedness" this version cannot rely on ordering. Instead,
+ // we need to determine the vertex ids and compare them. Assuming a vertex may exhibit either a branching or
+ // a linear structure, this gives us 4 cases (1a, 1b, 2a, 2b). 1 and 2 represent vertices where the segId
+ // is at the beginning of the segment (s==0), or at the end of the segment (s==1); while a and b represent
+ // the equivalent position on the test vertex.
+
+ // Test for segId at the start of the segment (1a and 1b)
+ if (s1 < m_distanceEpsilon)
+ {
+ return ((s2 < m_distanceEpsilon) && verticesA[0] == verticesB[0]) ||
+ ((s2 > (1.0 - m_distanceEpsilon) && verticesA[0] == verticesB[1]));
+ }
+
+ // Test for segId at the end of the segment (2a and 2b)
+ if (s1 > (1.0 - m_distanceEpsilon))
+ {
+ return ((s2 < m_distanceEpsilon) && verticesA[1] == verticesB[0]) ||
+ ((s2 > (1.0 - m_distanceEpsilon) && verticesA[1] == verticesB[1]));
+ }
+
+ return false;
+}
+
+double SegmentSelfContact::maxTimePrecision(
+ const std::array<Math::Vector3d, 2>& pt0Positions,
+ const std::array<Math::Vector3d, 2>& pt1Positions,
+ const std::array<Math::Vector3d, 2>& qt0Positions,
+ const std::array<Math::Vector3d, 2>& qt1Positions,
+ double effectiveThickness) const
+{
+ SURGSIM_ASSERT(effectiveThickness > 0.0) << "Attempting CCD with segment thickness <= 0.";
+
+ // Find the displacement of the endpoints during the time period
+ auto p0Displacement = pt1Positions[0] - pt0Positions[0];
+ auto p1Displacement = pt1Positions[1] - pt0Positions[1];
+ auto q1Displacement = qt1Positions[0] - qt0Positions[0];
+ auto q2Displacement = qt1Positions[1] - qt0Positions[1];
+
+ // Get the relative displacement between the segments, so we know the relative movement
+ double p1q1RelativeDisplacementNorm2 = (p0Displacement - q1Displacement).squaredNorm();
+ double p1q2RelativeDisplacementNorm2 = (p0Displacement - q2Displacement).squaredNorm();
+ double p2q1RelativeDisplacementNorm2 = (p1Displacement - q1Displacement).squaredNorm();
+ double p2q2RelativeDisplacementNorm2 = (p1Displacement - q2Displacement).squaredNorm();
+
+ // Now calculate the maximum relative displacement
+ double maxP1RelativeDisplacementNorm2 = std::max(p1q1RelativeDisplacementNorm2, p1q2RelativeDisplacementNorm2);
+ double maxP2RelativeDisplacementNorm2 = std::max(p2q1RelativeDisplacementNorm2, p2q2RelativeDisplacementNorm2);
+ double maxRelativeDisplacement =
+ std::sqrt(std::max(maxP1RelativeDisplacementNorm2, maxP2RelativeDisplacementNorm2));
+
+ // The time precision should be set low enough so that we don't miss any segment collision.
+ double timePrecision = effectiveThickness / maxRelativeDisplacement;
+
+ if (timePrecision < m_timeMinPrecisionEpsilon)
+ {
+ SURGSIM_LOG_WARNING(m_logger) <<
+ "Minimum time precision(" << m_timeMinPrecisionEpsilon <<
+ ") needs to be smaller(" << timePrecision <<
+ ") to guarantee no missed self - collisions!";
+ timePrecision = m_timeMinPrecisionEpsilon;
+ }
+ timePrecision = std::min(timePrecision, m_timeMaxPrecisionEpsilon);
+
+ return timePrecision;
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/SegmentSelfContact.h b/SurgSim/Collision/SegmentSelfContact.h
new file mode 100644
index 0000000..9acf23d
--- /dev/null
+++ b/SurgSim/Collision/SegmentSelfContact.h
@@ -0,0 +1,211 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_SEGMENTSELFCONTACT_H
+#define SURGSIM_COLLISION_SEGMENTSELFCONTACT_H
+
+#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
+#include "SurgSim/DataStructures/AabbTree.h"
+#include "SurgSim/Framework/Logger.h"
+#include "SurgSim/Math/SegmentMeshShape.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// SegmentSelfContact computes the self collisions among a SegmentMesh under motion at two
+/// time points parametrized over the time interval [0,1]. An initial phase uses the AABB tree to
+/// select a set of potentially colliding segments from the SegmentMesh. For each of these
+/// candidate segment pairs, the goal is to determine the point of earliest contact should any exist.
+///
+/// At the highest level the actual collision detection of candidate segment pairs is a two phase
+/// algorithm. First determine if there is contact at the start of an interval and report the contact if
+/// found. If no contact is found at the start, subdivide the interval, determine which of the resulting
+/// candidate subintervals may have collisions, and then recursively check those promising subintervals.
+/// Note that a simple algorithm based on interval arithmetic (including the Interval, LinearMotion and
+/// Polynomial interval classes) allows for a quick determination of promising subintervals allowing many
+/// of the subintervals to be pruned during the subdivision step without forcing the recursion to bottom out.
+///
+/// \sa Interval, LinearMotion, Polynomial, SegmentSegmentCcdIntervalCheck
+///
+class SegmentSelfContact : public ShapeShapeContactCalculation<Math::SegmentMeshShape, Math::SegmentMeshShape>
+{
+public:
+ /// Constructor.
+ SegmentSelfContact();
+
+ std::list<std::shared_ptr<Contact>> calculateCcdContact(
+ const Math::SegmentMeshShape& segmentShape1AtTime0, const Math::RigidTransform3d& segmentPose1AtTime0,
+ const Math::SegmentMeshShape& segmentShape1AtTime1, const Math::RigidTransform3d& segmentPose1AtTime1,
+ const Math::SegmentMeshShape& segmentShape2AtTime0, const Math::RigidTransform3d& segmentPose2AtTime0,
+ const Math::SegmentMeshShape& segmentShape2AtTime1, const Math::RigidTransform3d& segmentPose2AtTime1)
+ const override;
+
+ /// Set the minimum time precision allowed when deciding on the depth of recursion.
+ /// \param precision the desired minimum time precision
+ void setTimeMinPrecisionEpsilon(double precision);
+
+ /// \return the minimum time precision allowed when deciding on the depth of recursion.
+ double getTimeMinPrecisionEpsilon();
+
+ /// Set the maximum time precision allowed when deciding on the depth of recursion.
+ /// \param precision the desired maximum time precision
+ void setTimeMaxPrecisionEpsilon(double precision);
+
+ /// \return the maximum time precision allowed when deciding on the depth of recursion.
+ double getTimeMaxPrecisionEpsilon();
+
+ /// Set the maximum separation for which two points are considered the same.
+ /// \param precision the desired maximum separation for which two points are considered the same.
+ void setDistanceEpsilon(double precision);
+
+ /// \return the maximum separation for which two points are considered the same.
+ double distanceEpsilon();
+
+ /// Function that returns the shapes between which this class performs collision detection.
+ /// \return int std::pair containing the shape types.
+ std::pair<int, int> getShapeTypes() override;
+
+protected:
+ /// Detect if two segments actually collide either at time t=0 (Fixed case) or within a movement phase.
+ /// \param pt0Positions are the segment endpoints for the first segment at time t=0.
+ /// \param pt1Positions are the segment endpoints for the first segment at time t=1.
+ /// \param qt0Positions are the segment endpoints for the second segment at time t=0.
+ /// \param qt1Positions are the segment endpoints for the second segment at time t=1.
+ /// \param segmentRadius1 is the radius of the first segment.
+ /// \param segmentRadius2 is the radius of the second segment.
+ /// \param timePrecision is the minimum time delta. Recursion terminates at or below the timePrecision.
+ /// \param r [out] parametric location of the collision point (if any) on segment 1.
+ /// \param s [out] parametric location of the collision point (if any) on segment 2.
+ /// \param t [out] parametric location of the time of any collision in the interval [0, 1].
+ /// \param pToQDir [out] direction from p(s) to q(r)
+ /// \param contactPtP [out] location of the contact point along segment 1.
+ /// \param contactPtQ [out] location of the contact point along segment 2.
+ /// \return true if a collision is detected between segment 1 and segment 2; false otherwise.
+ bool detectCollision(
+ const std::array<SurgSim::Math::Vector3d, 2>& pt0Positions,
+ const std::array<SurgSim::Math::Vector3d, 2>& pt1Positions,
+ const std::array<SurgSim::Math::Vector3d, 2>& qt0Positions,
+ const std::array<SurgSim::Math::Vector3d, 2>& qt1Positions,
+ double segmentRadius1, double segmentRadius2,
+ double timePrecision,
+ double* r, double* s, double* t,
+ SurgSim::Math::Vector3d* pToQDir,
+ SurgSim::Math::Vector3d* contactPtP,
+ SurgSim::Math::Vector3d* contactPtQ) const;
+
+ /// Given a list of potentially intersecting AABB nodes, cull the list of any duplicates
+ /// and return the uniques candidates as synchronized pairs.
+ /// \param intersectionList list of potentially intersecting AABB node.
+ /// \param segmentIds [out] paired unique matches
+ void getUniqueCandidates(
+ const std::list<SurgSim::DataStructures::AabbTree::TreeNodePairType>& intersectionList,
+ std::set<std::pair<size_t, size_t>>* segmentIds) const;
+
+ /// From the initial AABB tree collisions, there are some very simple filtering operations that we can
+ /// do to eliminate a number of false positives. Most notably, we do not want to collide a single segment
+ /// against itself, or against one of the segments with which it shares a vertex. These are trivial collisions
+ /// and will always be present. Less obvious, we want to filter out segments with wild movement vectors.
+ /// These segments cannot be appropriately processed and are likely to represent errors.
+ /// \param segmentA the segment mesh at time t=0.
+ /// \param segmentB the segment mesh at time t=1.
+ /// \param segment1SegID the specific identifier of the candidate segment at time t=0.
+ /// \param segment2SegID the specific identifier of the candidate segment at time t=1.
+ /// \return true if this collision should be discarded, false if it should be processed further.
+ bool removeInvalidCollisions(
+ const Math::SegmentMeshShape& segmentA,
+ const Math::SegmentMeshShape& segmentB,
+ size_t segment1SegID, size_t segment2SegID) const;
+
+ /// Verify the a given point at times t0 and t1 have remained within a reasonable neighborhood. Large
+ /// movements make it impossible to accurately determine collisions.
+ /// \param pt0 vertex coordinates at time t0
+ /// \param pt1 vertex coordinates at time t1
+ /// \param threshold distance threshold
+ /// \return true if any point has exceeded the movement threshold
+ bool detectExcessMovement(const SurgSim::Math::Vector3d& pt0,
+ const SurgSim::Math::Vector3d& pt1,
+ double threshold) const;
+
+ /// Search the list of contacts for a match to the current contact.
+ /// \param segmentShape shape to be interrogated to see if the contacts match
+ /// \param contacts the current list of detected contacts
+ /// \param t time of the contact under consideration.
+ /// \param collisionType type of contact under consideration.
+ /// \param segId1 segment 1 identifier.
+ /// \param s1 parametric location of the contact point along segment 1.
+ /// \param segId2 segment 2 identifier.
+ /// \param s2 parametric location of the contact point along segment 2.
+ /// \param timeEpsilon maximum allowed epsilon for time matches.
+ /// \return true if the contacts match, return false otherwise.
+ bool findSegSegContact(const Math::SegmentMeshShape& segmentShape,
+ const std::list<std::shared_ptr<Contact>>& contacts,
+ double t, Collision::CollisionDetectionType collisionType,
+ size_t segId1, double s1, size_t segId2, double s2, double timeEpsilon) const;
+
+ /// Check for the same location among two parametric location specifications.
+ /// \param segmentShape shape to be interrogated to see if the contacts match
+ /// \param segId1 segment 1 identifier.
+ /// \param s1 parametric location of the contact point along segment 1.
+ /// \param segId2 segment 2 identifier.
+ /// \param s2 parametric location of the contact point along segment 2.
+ /// \return true if the contacts match, return false otherwise.
+ bool isSameSegContactPoint(const Math::SegmentMeshShape& segmentShape,
+ size_t segId1, double s1, size_t segId2, double s2) const;
+
+ /// Calculate the maximum time interval that guarantees that all collisions can be detected given the
+ /// derived motions of the segment end points.
+ /// \param pt0Positions are the segment endpoints for the first segment at time t=0.
+ /// \param pt1Positions are the segment endpoints for the first segment at time t=1.
+ /// \param qt0Positions are the segment endpoints for the second segment at time t=0.
+ /// \param qt1Positions are the segment endpoints for the second segment at time t=1.
+ /// \param effectiveThickness nearness criteria for declaring a contact.
+ /// \return the maximum time interval that will still allow for the detection of all
+ /// contacts within effectveThickness. This value is bounded from below by the value of
+ /// the member variable m_timeMinPrecisionEpsilon and from above by m_timeMaxPrecisionEpsilon.
+ double maxTimePrecision(
+ const std::array<SurgSim::Math::Vector3d, 2>& pt0Positions,
+ const std::array<SurgSim::Math::Vector3d, 2>& pt1Positions,
+ const std::array<SurgSim::Math::Vector3d, 2>& qt0Positions,
+ const std::array<SurgSim::Math::Vector3d, 2>& qt1Positions,
+ double effectiveThickness) const;
+
+private:
+ /// Minimum distance precision epsilon used in continuous collision detection.
+ double m_distanceEpsilon;
+
+ /// Minimum time precision epsilon used in continuous collision detection.
+ double m_timeMinPrecisionEpsilon;
+
+ /// Maximum time precision epsilon used in continuous collision detection.
+ double m_timeMaxPrecisionEpsilon;
+
+ /// Maximum time precision epsilon used in continuous collision detection.
+ const double m_maxMovementThreshold;
+
+ /// Flag to determine if segment thickness is to be used in contact calculations
+ const bool m_useSegmentThickness;
+
+ /// Logger
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif // SURGSIM_COLLISION_SEGMENTSELFCONTACT_H
diff --git a/SurgSim/Collision/ShapeCollisionRepresentation.cpp b/SurgSim/Collision/ShapeCollisionRepresentation.cpp
index bd5bdef..e158e3a 100644
--- a/SurgSim/Collision/ShapeCollisionRepresentation.cpp
+++ b/SurgSim/Collision/ShapeCollisionRepresentation.cpp
@@ -15,6 +15,7 @@
#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/SceneElement.h"
#include "SurgSim/Math/MathConvert.h"
#include "SurgSim/Math/MeshShape.h"
#include "SurgSim/Physics/Representation.h"
@@ -61,17 +62,6 @@ const std::shared_ptr<SurgSim::Math::Shape> ShapeCollisionRepresentation::getSha
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)
diff --git a/SurgSim/Collision/ShapeCollisionRepresentation.h b/SurgSim/Collision/ShapeCollisionRepresentation.h
index ec0101f..36eb9d9 100644
--- a/SurgSim/Collision/ShapeCollisionRepresentation.h
+++ b/SurgSim/Collision/ShapeCollisionRepresentation.h
@@ -44,17 +44,18 @@ public:
SURGSIM_CLASSNAME(SurgSim::Collision::ShapeCollisionRepresentation);
- virtual int getShapeType() const override;
+ int getShapeType() const override;
- virtual void setLocalPose(const SurgSim::Math::RigidTransform3d& pose) override;
+ 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;
+ const std::shared_ptr<SurgSim::Math::Shape> getShape() const override;
+
+protected:
+ bool doInitialize() override;
private:
// Shape used by this representation
@@ -64,4 +65,4 @@ private:
}; // namespace Collision
}; // namespace SurgSim
-#endif // SURGSIM_COLLISION_SHAPECOLLISIONREPRESENTATION_H
\ No newline at end of file
+#endif // SURGSIM_COLLISION_SHAPECOLLISIONREPRESENTATION_H
diff --git a/SurgSim/Collision/ShapeShapeContactCalculation.h b/SurgSim/Collision/ShapeShapeContactCalculation.h
new file mode 100644
index 0000000..0321d3c
--- /dev/null
+++ b/SurgSim/Collision/ShapeShapeContactCalculation.h
@@ -0,0 +1,97 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_SHAPESHAPECONTACTCALCULATION_H
+#define SURGSIM_COLLISION_SHAPESHAPECONTACTCALCULATION_H
+
+#include "SurgSim/Collision/ContactCalculation.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+
+namespace Collision
+{
+
+/// Class that can automate the type conversion and provides a consistent interface to the typed call
+/// Takes the shapes to convert as template parameters
+template <class Shape1, class Shape2>
+class ShapeShapeContactCalculation : public ContactCalculation
+{
+ /// Virtual function to be overridden, this provides the typed contact calculation between two shapes
+ /// it takes two shapes and their respective poses, and will return the contacts between those two shapes
+ /// for shapes that return true for isTransformable(), the calculation may ignore the pose passed in the
+ /// function.
+ virtual std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const Shape1& shape1, const Math::RigidTransform3d& pose1,
+ const Shape2& shape2, const Math::RigidTransform3d& pose2) const
+ {
+ SURGSIM_FAILURE() << "Not implemented";
+ return std::list<std::shared_ptr<Contact>>();
+ }
+
+ virtual std::list<std::shared_ptr<Contact>> calculateCcdContact(
+ const Shape1& shape1AtTime0, const Math::RigidTransform3d& pose1AtTime0,
+ const Shape1& shape1AtTime1, const Math::RigidTransform3d& pose1AtTime1,
+ const Shape2& shape2AtTime0, const Math::RigidTransform3d& pose2AtTime0,
+ const Shape2& shape2AtTime1, const Math::RigidTransform3d& pose2AtTime1) const
+ {
+ SURGSIM_FAILURE() << "Not implemented";
+ return std::list<std::shared_ptr<Contact>>();
+ }
+
+ /// Overrides the dcd contact calculation to go from untyped shapes to the typed shapes
+ std::list<std::shared_ptr<Contact>> doCalculateDcdContact(
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape1,
+ const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape2) override
+ {
+ auto one = std::static_pointer_cast<Shape1>(posedShape1.getShape());
+ auto two = std::static_pointer_cast<Shape2>(posedShape2.getShape());
+
+ SURGSIM_ASSERT(one->getType() == posedShape1.getShape()->getType()) << "Invalid Shape 1";
+ SURGSIM_ASSERT(two->getType() == posedShape2.getShape()->getType()) << "Invalid Shape 2";
+
+ return calculateDcdContact(*one, posedShape1.getPose(), *two, posedShape2.getPose());
+ }
+
+ /// Overrides the ccd contact calculation to go from untyped shapes to the typed shapes
+ std::list<std::shared_ptr<Contact>> doCalculateCcdContact(
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& posedShapeMotion1,
+ const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& posedShapeMotion2) override
+ {
+ auto oneAtTime0 = std::static_pointer_cast<Shape1>(posedShapeMotion1.first.getShape());
+ auto oneAtTime1 = std::static_pointer_cast<Shape1>(posedShapeMotion1.second.getShape());
+ auto twoAtTime0 = std::static_pointer_cast<Shape2>(posedShapeMotion2.first.getShape());
+ auto twoAtTime1 = std::static_pointer_cast<Shape2>(posedShapeMotion2.second.getShape());
+
+ SURGSIM_ASSERT(oneAtTime0->getType() == posedShapeMotion1.first.getShape()->getType()) <<
+ "Invalid Shape 1 detected at time 0";
+ SURGSIM_ASSERT(oneAtTime1->getType() == posedShapeMotion1.second.getShape()->getType()) <<
+ "Invalid Shape 1 detected at time 1";
+ SURGSIM_ASSERT(twoAtTime0->getType() == posedShapeMotion2.first.getShape()->getType()) <<
+ "Invalid Shape 2 detected at time 0";
+ SURGSIM_ASSERT(twoAtTime1->getType() == posedShapeMotion2.second.getShape()->getType()) <<
+ "Invalid Shape 2 detected at time 1";
+
+ return calculateCcdContact(
+ *oneAtTime0, posedShapeMotion1.first.getPose(), *oneAtTime1, posedShapeMotion1.second.getPose(),
+ *twoAtTime0, posedShapeMotion2.first.getPose(), *twoAtTime1, posedShapeMotion2.second.getPose());
+ }
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Collision/SphereDoubleSidedPlaneContact.cpp b/SurgSim/Collision/SphereDoubleSidedPlaneContact.cpp
new file mode 100644
index 0000000..1aabf26
--- /dev/null
+++ b/SurgSim/Collision/SphereDoubleSidedPlaneContact.cpp
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/SphereDoubleSidedPlaneContact.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
+{
+
+std::list<std::shared_ptr<Contact>> SphereDoubleSidedPlaneContact::calculateDcdContact(
+ const Math::SphereShape& sphere,
+ const Math::RigidTransform3d& spherePose,
+ const Math::DoubleSidedPlaneShape& plane,
+ const Math::RigidTransform3d& planePose) const
+{
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ Vector3d sphereCenter = spherePose.translation();
+
+ // Move into Plane coordinate system
+ Vector3d planeLocalSphereCenter = planePose.inverse() * sphereCenter;
+
+ Vector3d result;
+ double dist = 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 =
+ (planePose.linear() * plane.getNormal()) * ((dist < 0.0) ? -1.0 : 1.0);
+
+ std::pair<Location, Location> penetrationPoints;
+ penetrationPoints.first.rigidLocalPosition.setValue(
+ spherePose.inverse() * (sphereCenter - normal * sphere.getRadius()));
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ planePose.inverse() * (sphereCenter - normal * distAbsolute));
+
+ contacts.emplace_back(std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, depth, 1.0,
+ Vector3d::Zero(), normal, penetrationPoints));
+ }
+
+ return contacts;
+}
+
+std::pair<int, int> SphereDoubleSidedPlaneContact::getShapeTypes()
+{
+ return std::pair<int, int>(SurgSim::Math::SHAPE_TYPE_SPHERE, SurgSim::Math::SHAPE_TYPE_DOUBLESIDEDPLANE);
+}
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/SphereDoubleSidedPlaneContact.h b/SurgSim/Collision/SphereDoubleSidedPlaneContact.h
new file mode 100644
index 0000000..4b61110
--- /dev/null
+++ b/SurgSim/Collision/SphereDoubleSidedPlaneContact.h
@@ -0,0 +1,51 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_SPHEREDOUBLESIDEDPLANECONTACT_H
+#define SURGSIM_COLLISION_SPHEREDOUBLESIDEDPLANECONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between Spheres and DoubleSidedPlanes
+class SphereDoubleSidedPlaneContact :
+ public ShapeShapeContactCalculation<Math::SphereShape, Math::DoubleSidedPlaneShape>
+{
+public:
+ using ContactCalculation::calculateDcdContact;
+
+ std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const Math::SphereShape& sphere,
+ const Math::RigidTransform3d& spherePose,
+ const Math::DoubleSidedPlaneShape& plane,
+ const Math::RigidTransform3d& planePose) const override;
+
+ std::pair<int, int> getShapeTypes() override;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/SphereDoubleSidedPlaneDcdContact.cpp b/SurgSim/Collision/SphereDoubleSidedPlaneDcdContact.cpp
deleted file mode 100644
index 4b92af7..0000000
--- a/SurgSim/Collision/SphereDoubleSidedPlaneDcdContact.cpp
+++ /dev/null
@@ -1,85 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index e7fb713..0000000
--- a/SurgSim/Collision/SphereDoubleSidedPlaneDcdContact.h
+++ /dev/null
@@ -1,52 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/SpherePlaneContact.cpp b/SurgSim/Collision/SpherePlaneContact.cpp
new file mode 100644
index 0000000..a136bb2
--- /dev/null
+++ b/SurgSim/Collision/SpherePlaneContact.cpp
@@ -0,0 +1,78 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/SpherePlaneContact.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
+{
+
+std::list<std::shared_ptr<Contact>> SpherePlaneContact::calculateDcdContact(
+ const Math::SphereShape& sphere,
+ const Math::RigidTransform3d& spherePose,
+ const Math::PlaneShape& plane,
+ const Math::RigidTransform3d& planePose) const
+{
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ Vector3d sphereCenter = spherePose.translation();
+
+ // Move into Plane coordinate system
+ Vector3d planeLocalSphereCenter = planePose.inverse() * sphereCenter;
+
+ Vector3d result;
+ double dist = 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 = planePose.linear() * plane.getNormal();
+
+ std::pair<Location, Location> penetrationPoints;
+ penetrationPoints.first.rigidLocalPosition.setValue(
+ spherePose.inverse() * (sphereCenter - normal * sphere.getRadius()));
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ planePose.inverse() * (sphereCenter - normal * dist));
+
+ contacts.emplace_back(std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, depth, 1.0,
+ Vector3d::Zero(), normal, penetrationPoints));
+ }
+
+ return contacts;
+}
+
+std::pair<int, int> SpherePlaneContact::getShapeTypes()
+{
+ return std::pair<int, int>(SurgSim::Math::SHAPE_TYPE_SPHERE, SurgSim::Math::SHAPE_TYPE_PLANE);
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/SpherePlaneContact.h b/SurgSim/Collision/SpherePlaneContact.h
new file mode 100644
index 0000000..25bf857
--- /dev/null
+++ b/SurgSim/Collision/SpherePlaneContact.h
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_SPHEREPLANECONTACT_H
+#define SURGSIM_COLLISION_SPHEREPLANECONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/PlaneShape.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between Spheres and Planes
+class SpherePlaneContact : public ShapeShapeContactCalculation<Math::SphereShape, Math::PlaneShape>
+{
+public:
+ using ContactCalculation::calculateDcdContact;
+
+ std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const Math::SphereShape& sphere,
+ const Math::RigidTransform3d& spherePose,
+ const Math::PlaneShape& plane,
+ const Math::RigidTransform3d& planePose) const override;
+
+ std::pair<int, int> getShapeTypes() override;
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/SpherePlaneDcdContact.cpp b/SurgSim/Collision/SpherePlaneDcdContact.cpp
deleted file mode 100644
index a5b49a8..0000000
--- a/SurgSim/Collision/SpherePlaneDcdContact.cpp
+++ /dev/null
@@ -1,82 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 15ae6f0..0000000
--- a/SurgSim/Collision/SpherePlaneDcdContact.h
+++ /dev/null
@@ -1,52 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/SphereSphereContact.cpp b/SurgSim/Collision/SphereSphereContact.cpp
new file mode 100644
index 0000000..3005fe3
--- /dev/null
+++ b/SurgSim/Collision/SphereSphereContact.cpp
@@ -0,0 +1,70 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/SphereSphereContact.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
+{
+
+std::list<std::shared_ptr<Contact>> SphereSphereContact::calculateDcdContact(
+ const Math::SphereShape& sphere1,
+ const Math::RigidTransform3d& sphere1Pose,
+ const Math::SphereShape& sphere2,
+ const Math::RigidTransform3d& sphere2Pose) const
+{
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ Vector3d center1 = sphere1Pose.translation();
+ Vector3d center2 = sphere2Pose.translation();
+
+ Vector3d normal = center1 - center2;
+ double dist = normal.norm();
+ double maxDist = sphere1.getRadius() + sphere2.getRadius();
+ if (dist < maxDist)
+ {
+ std::pair<Location, Location> penetrationPoints;
+ normal.normalize();
+ penetrationPoints.first.rigidLocalPosition.setValue(
+ (sphere1Pose.linear().inverse() * -normal) * sphere1.getRadius());
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ (sphere2Pose.linear().inverse() * normal) * sphere2.getRadius());
+
+ contacts.emplace_back(std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, maxDist - dist, 1.0,
+ Vector3d::Zero(), normal, penetrationPoints));
+ }
+
+ return contacts;
+}
+
+std::pair<int, int> SphereSphereContact::getShapeTypes()
+{
+ return std::pair<int, int>(SurgSim::Math::SHAPE_TYPE_SPHERE, SurgSim::Math::SHAPE_TYPE_SPHERE);
+}
+
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/SphereSphereContact.h b/SurgSim/Collision/SphereSphereContact.h
new file mode 100644
index 0000000..ae96420
--- /dev/null
+++ b/SurgSim/Collision/SphereSphereContact.h
@@ -0,0 +1,50 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_SPHERESPHERECONTACT_H
+#define SURGSIM_COLLISION_SPHERESPHERECONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
+#include "SurgSim/Math/SphereShape.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between spheres
+class SphereSphereContact : public ShapeShapeContactCalculation<Math::SphereShape, Math::SphereShape>
+{
+public:
+ using ContactCalculation::calculateDcdContact;
+
+ std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const Math::SphereShape& sphere1,
+ const Math::RigidTransform3d& sphere1Pose,
+ const Math::SphereShape& sphere2,
+ const Math::RigidTransform3d& sphere2Pose) const override;
+
+ std::pair<int, int> getShapeTypes() override;
+
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Collision/SphereSphereDcdContact.cpp b/SurgSim/Collision/SphereSphereDcdContact.cpp
deleted file mode 100644
index 8ad3cfb..0000000
--- a/SurgSim/Collision/SphereSphereDcdContact.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index ef02ed2..0000000
--- a/SurgSim/Collision/SphereSphereDcdContact.h
+++ /dev/null
@@ -1,50 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/TriangleMeshParticlesContact.cpp b/SurgSim/Collision/TriangleMeshParticlesContact.cpp
new file mode 100644
index 0000000..9c1b045
--- /dev/null
+++ b/SurgSim/Collision/TriangleMeshParticlesContact.cpp
@@ -0,0 +1,97 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/TriangleMeshParticlesContact.h"
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/DataStructures/AabbTree.h"
+#include "SurgSim/DataStructures/AabbTreeNode.h"
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+using SurgSim::DataStructures::Location;
+using SurgSim::Math::barycentricCoordinates;
+using SurgSim::Math::distancePointTriangle;
+using SurgSim::Math::Vector;
+using SurgSim::Math::Vector3d;
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+std::pair<int, int> TriangleMeshParticlesContact::getShapeTypes()
+{
+ return std::pair<int, int>(Math::SHAPE_TYPE_MESH, Math::SHAPE_TYPE_PARTICLES);
+}
+
+std::list<std::shared_ptr<Contact>> TriangleMeshParticlesContact::calculateDcdContact(
+ const Math::MeshShape& mesh,
+ const Math::RigidTransform3d&,
+ const Math::ParticlesShape& particles,
+ const Math::RigidTransform3d&) const
+{
+
+ std::list<std::shared_ptr<Contact>> contacts;
+ Vector3d closestPoint;
+ Vector3d coordinates;
+ const double particleRadius = particles.getRadius();
+
+ auto intersections = mesh.getAabbTree()->spatialJoin(*particles.getAabbTree());
+ for (auto& intersection : intersections)
+ {
+ std::list<size_t> candidateTriangles;
+ std::list<size_t> candidateParticles;
+ intersection.first->getIntersections(intersection.second->getAabb(), &candidateTriangles);
+ intersection.second->getIntersections(intersection.first->getAabb(), &candidateParticles);
+ for (auto& triangle : candidateTriangles)
+ {
+ const Vector3d& normal = mesh.getNormal(triangle);
+ if (normal.isZero())
+ {
+ continue;
+ }
+
+ for (auto& particle : candidateParticles)
+ {
+ const Vector3d& particlePosition = particles.getVertexPosition(particle);
+ auto vertices = mesh.getTrianglePositions(triangle);
+ double distance = distancePointTriangle(particlePosition, vertices[0], vertices[1], vertices[2],
+ &closestPoint);
+ if (distance < particleRadius)
+ {
+ double depth = particleRadius - normal.dot(particlePosition - closestPoint);
+ barycentricCoordinates(closestPoint, vertices[0], vertices[1], vertices[2], normal, &coordinates);
+ auto penetrationPoints = std::make_pair(
+ Location(IndexedLocalCoordinate(triangle, coordinates),
+ DataStructures::Location::TRIANGLE),
+ Location(particle));
+ contacts.push_back(std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, depth, 1.0,
+ Vector3d::Zero(), -normal, penetrationPoints));
+ }
+ }
+ }
+ }
+ return contacts;
+}
+
+};
+};
+
diff --git a/SurgSim/Collision/TriangleMeshParticlesContact.h b/SurgSim/Collision/TriangleMeshParticlesContact.h
new file mode 100644
index 0000000..2301bf5
--- /dev/null
+++ b/SurgSim/Collision/TriangleMeshParticlesContact.h
@@ -0,0 +1,53 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_TRIANGLEMESHPARTICLESCONTACT_H
+#define SURGSIM_COLLISION_TRIANGLEMESHPARTICLESCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/ParticlesShape.h"
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between a triangle mesh and particles
+class TriangleMeshParticlesContact : public ShapeShapeContactCalculation<Math::MeshShape, Math::ParticlesShape>
+{
+public:
+ using ContactCalculation::calculateDcdContact;
+
+ /// \note poses are ignored for this calculation
+ std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const Math::MeshShape& mesh,
+ const Math::RigidTransform3d&,
+ const Math::ParticlesShape& particles,
+ const Math::RigidTransform3d&) const override;
+
+ std::pair<int, int> getShapeTypes() override;
+
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif // SURGSIM_COLLISION_TRIANGLEMESHPARTICLESCONTACT_H
diff --git a/SurgSim/Collision/TriangleMeshPlaneContact.cpp b/SurgSim/Collision/TriangleMeshPlaneContact.cpp
new file mode 100644
index 0000000..58495fb
--- /dev/null
+++ b/SurgSim/Collision/TriangleMeshPlaneContact.cpp
@@ -0,0 +1,75 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/TriangleMeshPlaneContact.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
+{
+std::pair<int, int> TriangleMeshPlaneContact::getShapeTypes()
+{
+ return std::pair<int, int> (SurgSim::Math::SHAPE_TYPE_MESH, SurgSim::Math::SHAPE_TYPE_PLANE);
+}
+
+std::list<std::shared_ptr<Contact>> TriangleMeshPlaneContact::calculateDcdContact(
+ const Math::MeshShape& mesh,
+ const Math::RigidTransform3d& meshPose,
+ const Math::PlaneShape& plane, const Math::RigidTransform3d& planePose) const
+{
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ // Transform the plane normal to Mesh co-ordinate system.
+ Vector3d planeNormal = planePose.linear() * plane.getNormal();
+ Vector3d planeNormalScaled = plane.getNormal() * -plane.getD();
+ Vector3d planePoint = planePose * planeNormalScaled;
+ double planeD = -planeNormal.dot(planePoint);
+
+ double d;
+ Vector3d normal;
+ for (auto& vertex : mesh.getVertices())
+ {
+ d = planeNormal.dot(vertex.position) + planeD;
+ if (d < SurgSim::Math::Geometry::DistanceEpsilon)
+ {
+ // Create the contact
+ normal = planePose.linear() * plane.getNormal();
+ std::pair<Location, Location> penetrationPoints;
+ penetrationPoints.first.rigidLocalPosition.setValue(
+ meshPose.inverse() * vertex.position);
+ penetrationPoints.second.rigidLocalPosition.setValue(
+ planePose.inverse() * (vertex.position - normal * d));
+
+ contacts.emplace_back(std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, -d, 1.0,
+ Vector3d::Zero(), normal, penetrationPoints));
+ }
+ }
+ return contacts;
+}
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Collision/TriangleMeshPlaneContact.h b/SurgSim/Collision/TriangleMeshPlaneContact.h
new file mode 100644
index 0000000..40770af
--- /dev/null
+++ b/SurgSim/Collision/TriangleMeshPlaneContact.h
@@ -0,0 +1,48 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_TRIANGLEMESHPLANECONTACT_H
+#define SURGSIM_COLLISION_TRIANGLEMESHPLANECONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/PlaneShape.h"
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+/// Class to calculate intersections between a triangle mesh and a plane
+class TriangleMeshPlaneContact : public ShapeShapeContactCalculation<Math::MeshShape, Math::PlaneShape>
+{
+public:
+ using ContactCalculation::calculateDcdContact;
+
+ std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const Math::MeshShape& mesh,
+ const Math::RigidTransform3d& meshPose,
+ const Math::PlaneShape& plane,
+ const Math::RigidTransform3d& planePose) const override;
+
+ std::pair<int, int> getShapeTypes() override;
+};
+
+};
+};
+
+#endif
diff --git a/SurgSim/Collision/TriangleMeshPlaneDcdContact.cpp b/SurgSim/Collision/TriangleMeshPlaneDcdContact.cpp
deleted file mode 100644
index 6f20121..0000000
--- a/SurgSim/Collision/TriangleMeshPlaneDcdContact.cpp
+++ /dev/null
@@ -1,91 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 498c666..0000000
--- a/SurgSim/Collision/TriangleMeshPlaneDcdContact.h
+++ /dev/null
@@ -1,52 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/TriangleMeshSurfaceMeshContact.cpp b/SurgSim/Collision/TriangleMeshSurfaceMeshContact.cpp
new file mode 100644
index 0000000..2a4b435
--- /dev/null
+++ b/SurgSim/Collision/TriangleMeshSurfaceMeshContact.cpp
@@ -0,0 +1,142 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/TriangleMeshSurfaceMeshContact.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
+{
+
+std::pair<int, int> TriangleMeshSurfaceMeshContact::getShapeTypes()
+{
+ return std::pair<int, int>(SurgSim::Math::SHAPE_TYPE_MESH, SurgSim::Math::SHAPE_TYPE_SURFACEMESH);
+}
+
+std::list<std::shared_ptr<Contact>> TriangleMeshSurfaceMeshContact::calculateDcdContact(
+ const Math::MeshShape& meshA,
+ const Math::RigidTransform3d& meshAPose,
+ const Math::SurfaceMeshShape& meshB,
+ const Math::RigidTransform3d& meshBPose) const
+{
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ std::list<SurgSim::DataStructures::AabbTree::TreeNodePairType> intersectionList
+ = meshA.getAabbTree()->spatialJoin(*(meshB.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 = meshA.getNormal(*i);
+ if (normalA.isZero())
+ {
+ continue;
+ }
+
+ auto verticesA = meshA.getTrianglePositions(*i);
+
+ for (auto j = triangleListB.begin(); j != triangleListB.end(); ++j)
+ {
+ const Vector3d& normalB = meshB.getNormal(*j);
+ if (normalB.isZero())
+ {
+ continue;
+ }
+
+ auto verticesB = meshB.getTrianglePositions(*j);
+
+ bool trianglesInContact = false;
+ // Check if the triangle A is on the bottom side of triangle B
+ if ((((verticesA[0] + verticesA[1] + verticesA[2]) / 3.0) - verticesB[0]).dot(normalB) < 0.0)
+ {
+ // The centroid of triangle A is below the plane of triangle B.
+ // Reverse the normal of the triangle B
+ trianglesInContact =
+ Math::calculateContactTriangleTriangle(verticesA[0], verticesA[1], verticesA[2],
+ verticesB[0], verticesB[2], verticesB[1],
+ normalA, (-normalB).eval(), &depth,
+ &penetrationPointA, &penetrationPointB,
+ &normal);
+ }
+ else
+ {
+ trianglesInContact =
+ Math::calculateContactTriangleTriangle(verticesA[0], verticesA[1], verticesA[2],
+ verticesB[0], verticesB[1], verticesB[2],
+ normalA, normalB, &depth,
+ &penetrationPointA, &penetrationPointB,
+ &normal);
+ }
+
+ // Check if the triangles intersect.
+ if (trianglesInContact)
+ {
+ // Create the contact.
+ std::pair<Location, Location> penetrationPoints;
+ Vector3d barycentricCoordinate;
+ Math::barycentricCoordinates(penetrationPointA, verticesA[0], verticesA[1], verticesA[2],
+ normalA, &barycentricCoordinate);
+ penetrationPoints.first.triangleMeshLocalCoordinate.setValue(
+ DataStructures::IndexedLocalCoordinate(*i, barycentricCoordinate));
+ Math::barycentricCoordinates(penetrationPointB, verticesB[0], verticesB[1], verticesB[2],
+ normalB, &barycentricCoordinate);
+ penetrationPoints.second.triangleMeshLocalCoordinate.setValue(
+ DataStructures::IndexedLocalCoordinate(*j, barycentricCoordinate));
+
+ penetrationPoints.first.rigidLocalPosition.setValue(meshAPose.inverse() * penetrationPointA);
+ penetrationPoints.second.rigidLocalPosition.setValue(meshBPose.inverse() * penetrationPointB);
+
+ contacts.emplace_back(std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, std::abs(depth), 1.0,
+ Vector3d::Zero(), normal, penetrationPoints));
+ }
+ }
+ }
+ }
+ return contacts;
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/TriangleMeshSurfaceMeshContact.h b/SurgSim/Collision/TriangleMeshSurfaceMeshContact.h
new file mode 100644
index 0000000..64a9144
--- /dev/null
+++ b/SurgSim/Collision/TriangleMeshSurfaceMeshContact.h
@@ -0,0 +1,53 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_TRIANGLEMESHSURFACEMESHCONTACT_H
+#define SURGSIM_COLLISION_TRIANGLEMESHSURFACEMESHCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/SurfaceMeshShape.h"
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between a surface mesh and a triangle/surface mesh
+class TriangleMeshSurfaceMeshContact : public ShapeShapeContactCalculation<Math::MeshShape, Math::SurfaceMeshShape>
+{
+public:
+ using ContactCalculation::calculateDcdContact;
+
+ /// \note this expects the posed version of the shape to be passed
+ std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const Math::MeshShape& mesh1,
+ const Math::RigidTransform3d& mesh1Pose,
+ const Math::SurfaceMeshShape& mesh2,
+ const Math::RigidTransform3d& mesh2Pose) const override;
+
+ std::pair<int, int> getShapeTypes() override;
+
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif // SURGSIM_COLLISION_TRIANGLEMESHSURFACEMESHCONTACT_H
diff --git a/SurgSim/Collision/TriangleMeshTriangleMeshContact.cpp b/SurgSim/Collision/TriangleMeshTriangleMeshContact.cpp
new file mode 100644
index 0000000..dbae37e
--- /dev/null
+++ b/SurgSim/Collision/TriangleMeshTriangleMeshContact.cpp
@@ -0,0 +1,231 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/TriangleMeshTriangleMeshContact.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
+{
+
+std::pair<int, int> TriangleMeshTriangleMeshContact::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
+
+std::list<std::shared_ptr<Contact>> TriangleMeshTriangleMeshContact::calculateDcdContact(
+ const Math::MeshShape& meshA,
+ const Math::RigidTransform3d& meshAPose,
+ const Math::MeshShape& meshB,
+ const Math::RigidTransform3d& meshBPose) const
+{
+
+ std::list<std::shared_ptr<Contact>> contacts;
+
+ std::list<SurgSim::DataStructures::AabbTree::TreeNodePairType> intersectionList
+ = meshA.getAabbTree()->spatialJoin(*(meshB.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 = meshA.getNormal(*i);
+ if (normalA.isZero())
+ {
+ continue;
+ }
+
+ auto verticesA = meshA.getTrianglePositions(*i);
+
+ for (auto j = triangleListB.begin(); j != triangleListB.end(); ++j)
+ {
+ const Vector3d& normalB = meshB.getNormal(*j);
+ if (normalB.isZero())
+ {
+ continue;
+ }
+
+ auto verticesB = meshB.getTrianglePositions(*j);
+
+ // Check if the triangles intersect.
+ if (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;
+ Math::barycentricCoordinates(penetrationPointA, verticesA[0], verticesA[1], verticesA[2],
+ normalA, &barycentricCoordinate);
+ penetrationPoints.first.triangleMeshLocalCoordinate.setValue(
+ DataStructures::IndexedLocalCoordinate(*i, barycentricCoordinate));
+ Math::barycentricCoordinates(penetrationPointB, verticesB[0], verticesB[1], verticesB[2],
+ normalB, &barycentricCoordinate);
+ penetrationPoints.second.triangleMeshLocalCoordinate.setValue(
+ DataStructures::IndexedLocalCoordinate(*j, barycentricCoordinate));
+
+ penetrationPoints.first.rigidLocalPosition.setValue(meshAPose.inverse() * penetrationPointA);
+ penetrationPoints.second.rigidLocalPosition.setValue(meshBPose.inverse() * penetrationPointB);
+
+ contacts.emplace_back(std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, std::abs(depth), 1.0,
+ Vector3d::Zero(), normal, penetrationPoints));
+ }
+ }
+ }
+ }
+ return contacts;
+}
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/TriangleMeshTriangleMeshContact.h b/SurgSim/Collision/TriangleMeshTriangleMeshContact.h
new file mode 100644
index 0000000..083d72c
--- /dev/null
+++ b/SurgSim/Collision/TriangleMeshTriangleMeshContact.h
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_TRIANGLEMESHTRIANGLEMESHCONTACT_H
+#define SURGSIM_COLLISION_TRIANGLEMESHTRIANGLEMESHCONTACT_H
+
+#include <memory>
+
+#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
+#include "SurgSim/Math/MeshShape.h"
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class CollisionPair;
+
+/// Class to calculate intersections between a triangle mesh and a triangle mesh
+class TriangleMeshTriangleMeshContact : public ShapeShapeContactCalculation<Math::MeshShape, Math::MeshShape>
+{
+public:
+ using ContactCalculation::calculateDcdContact;
+
+ /// \note this expects the posed version of the shape to be passed
+ std::list<std::shared_ptr<Contact>> calculateDcdContact(
+ const Math::MeshShape& mesh1,
+ const Math::RigidTransform3d& mesh1Pose,
+ const Math::MeshShape& mesh2,
+ const Math::RigidTransform3d& mesh2Pose) const override;
+
+ std::pair<int, int> getShapeTypes() override;
+
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
+
+#endif // SURGSIM_COLLISION_TRIANGLEMESHTRIANGLEMESHCONTACT_H
diff --git a/SurgSim/Collision/TriangleMeshTriangleMeshDcdContact.cpp b/SurgSim/Collision/TriangleMeshTriangleMeshDcdContact.cpp
deleted file mode 100644
index 87b8579..0000000
--- a/SurgSim/Collision/TriangleMeshTriangleMeshDcdContact.cpp
+++ /dev/null
@@ -1,229 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index a927f89..0000000
--- a/SurgSim/Collision/TriangleMeshTriangleMeshDcdContact.h
+++ /dev/null
@@ -1,50 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
index 472f438..e8f37fa 100644
--- a/SurgSim/Collision/UnitTests/BoxCapsuleContactCalculationTests.cpp
+++ b/SurgSim/Collision/UnitTests/BoxCapsuleContactCalculationTests.cpp
@@ -16,7 +16,7 @@
#include <gtest/gtest.h>
#include <memory>
-#include "SurgSim/Collision/BoxCapsuleDcdContact.h"
+#include "SurgSim/Collision/BoxCapsuleContact.h"
#include "SurgSim/Collision/Representation.h"
#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
#include "SurgSim/Math/BoxShape.h"
@@ -55,7 +55,7 @@ void doBoxCapsuleTest(std::shared_ptr<BoxShape> box,
capsuleRep->setLocalPose(makeRigidTransform(capsuleQuat, capsuleTrans));
// Perform collision detection.
- BoxCapsuleDcdContact calcContact;
+ BoxCapsuleContact calcContact;
std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(boxRep, capsuleRep);
calcContact.calculateContact(pair);
diff --git a/SurgSim/Collision/UnitTests/BoxDoubleSidedPlaneContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/BoxDoubleSidedPlaneContactCalculationTests.cpp
index 59225d7..c4977bb 100644
--- a/SurgSim/Collision/UnitTests/BoxDoubleSidedPlaneContactCalculationTests.cpp
+++ b/SurgSim/Collision/UnitTests/BoxDoubleSidedPlaneContactCalculationTests.cpp
@@ -14,7 +14,7 @@
// limitations under the License.
#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
-#include "SurgSim/Collision/BoxDoubleSidedPlaneDcdContact.h"
+#include "SurgSim/Collision/BoxDoubleSidedPlaneContact.h"
using SurgSim::Math::BoxShape;
using SurgSim::Math::DoubleSidedPlaneShape;
@@ -54,7 +54,7 @@ void doBoxDoubleSidedPlaneTest(std::shared_ptr<BoxShape> box,
}
// Perform collision detection.
- BoxDoubleSidedPlaneDcdContact calcContact;
+ BoxDoubleSidedPlaneContact calcContact;
std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(boxRep, planeRep);
calcContact.calculateContact(pair);
@@ -143,8 +143,8 @@ TEST(BoxDoubleSidedPlaneContactCalculationTests, UnitTests)
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)));
+ boxQuat = globalQuat * (SurgSim::Math::makeRotationQuaternion(angle, Vector3d(0.0,1.0,0.0)) *
+ SurgSim::Math::makeRotationQuaternion(-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);
@@ -159,8 +159,8 @@ TEST(BoxDoubleSidedPlaneContactCalculationTests, UnitTests)
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)));
+ boxQuat = globalQuat * Quaterniond(SurgSim::Math::makeRotationQuaternion(angle, Vector3d(0.0,1.0,0.0)) *
+ SurgSim::Math::makeRotationQuaternion(-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);
diff --git a/SurgSim/Collision/UnitTests/BoxPlaneContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/BoxPlaneContactCalculationTests.cpp
index 22f6045..ebf74b9 100644
--- a/SurgSim/Collision/UnitTests/BoxPlaneContactCalculationTests.cpp
+++ b/SurgSim/Collision/UnitTests/BoxPlaneContactCalculationTests.cpp
@@ -14,7 +14,7 @@
// limitations under the License.
#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
-#include "SurgSim/Collision/BoxPlaneDcdContact.h"
+#include "SurgSim/Collision/BoxPlaneContact.h"
#include "SurgSim/Math/Geometry.h"
using SurgSim::Math::BoxShape;
@@ -54,7 +54,7 @@ void doBoxPlaneTest(std::shared_ptr<BoxShape> box,
}
// Perform collision detection.
- BoxPlaneDcdContact calcContact;
+ BoxPlaneContact calcContact;
std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(boxRep, planeRep);
calcContact.calculateContact(pair);
@@ -131,8 +131,8 @@ TEST(BoxPlaneContactCalculationTests, UnitTests)
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)));
+ boxQuat = globalQuat * (SurgSim::Math::makeRotationQuaternion(angle, Vector3d(0.0,1.0,0.0)) *
+ SurgSim::Math::makeRotationQuaternion(-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);
@@ -146,8 +146,8 @@ TEST(BoxPlaneContactCalculationTests, UnitTests)
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)));
+ boxQuat = globalQuat * (SurgSim::Math::makeRotationQuaternion(angle, Vector3d(0.0,1.0,0.0)) *
+ SurgSim::Math::makeRotationQuaternion(-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);
diff --git a/SurgSim/Collision/UnitTests/BoxSphereContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/BoxSphereContactCalculationTests.cpp
index bbbf512..61f9356 100644
--- a/SurgSim/Collision/UnitTests/BoxSphereContactCalculationTests.cpp
+++ b/SurgSim/Collision/UnitTests/BoxSphereContactCalculationTests.cpp
@@ -14,7 +14,7 @@
// limitations under the License.
#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
-#include "SurgSim/Collision/BoxSphereDcdContact.h"
+#include "SurgSim/Collision/BoxSphereContact.h"
#include "SurgSim/Math/BoxShape.h"
#include "SurgSim/Math/SphereShape.h"
@@ -52,7 +52,7 @@ void doBoxSphereTest(std::shared_ptr<BoxShape> box,
sphereRep->setLocalPose(SurgSim::Math::makeRigidTransform(sphereQuat, sphereTrans));
// Perform collision detection.
- BoxSphereDcdContact calcContact;
+ BoxSphereContact calcContact;
std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(boxRep, sphereRep);
calcContact.calculateContact(pair);
diff --git a/SurgSim/Collision/UnitTests/CMakeLists.txt b/SurgSim/Collision/UnitTests/CMakeLists.txt
index 715a530..a363033 100644
--- a/SurgSim/Collision/UnitTests/CMakeLists.txt
+++ b/SurgSim/Collision/UnitTests/CMakeLists.txt
@@ -25,17 +25,26 @@ set(UNIT_TEST_SOURCES
BoxSphereContactCalculationTests.cpp
CapsuleSphereContactCalculationTests.cpp
CollisionPairTests.cpp
+ CompoundShapeContactCalculationTests.cpp
ContactCalculationTests.cpp
ContactCalculationTestsCommon.cpp
DefaultContactCalculationTests.cpp
+ ElementContactFilterTests.cpp
OctreeContactCalculationTests.cpp
RepresentationTest.cpp
RepresentationUtilities.cpp
+ SegmentMeshTriangleMeshContactCalculationTests.cpp
+ SegmentSegmentCcdIntervalCheckTests.cpp
+ SegmentSegmentCcdMovingContactTests.cpp
+ SegmentSegmentCcdStaticContactTests.cpp
+ SegmentSelfContactTests.cpp
ShapeCollisionRepresentationTest.cpp
SphereDoubleSidedPlaneContactCalculationTests.cpp
SpherePlaneContactCalculationTests.cpp
SphereSphereContactCalculationTests.cpp
+ TriangleMeshParticlesContactCalculationTests.cpp
TriangleMeshPlaneContactCalculationTests.cpp
+ TriangleMeshSurfaceMeshContactCalculationTests.cpp
TriangleMeshTriangleMeshContactCalculationTests.cpp
)
@@ -52,8 +61,6 @@ set(LIBS
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"
diff --git a/SurgSim/Collision/UnitTests/CapsuleSphereContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/CapsuleSphereContactCalculationTests.cpp
index 704a180..c459828 100644
--- a/SurgSim/Collision/UnitTests/CapsuleSphereContactCalculationTests.cpp
+++ b/SurgSim/Collision/UnitTests/CapsuleSphereContactCalculationTests.cpp
@@ -14,7 +14,7 @@
// limitations under the License.
#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
-#include "SurgSim/Collision/CapsuleSphereDcdContact.h"
+#include "SurgSim/Collision/CapsuleSphereContact.h"
#include "SurgSim/Math/CapsuleShape.h"
#include "SurgSim/Math/SphereShape.h"
@@ -37,7 +37,7 @@ void doCapsuleSphereTest(double capsuleHeight, double capsuleRadius,
makeCapsuleRepresentation(capsuleHeight, capsuleRadius, capsuleQuat, capsulePosition),
makeSphereRepresentation(sphereRadius, sphereQuat, spherePosition));
- CapsuleSphereDcdContact calc;
+ CapsuleSphereContact calc;
calc.calculateContact(pair);
EXPECT_EQ(hasContacts, pair->hasContacts());
diff --git a/SurgSim/Collision/UnitTests/CollisionPairTests.cpp b/SurgSim/Collision/UnitTests/CollisionPairTests.cpp
index 8526be0..639e3c0 100644
--- a/SurgSim/Collision/UnitTests/CollisionPairTests.cpp
+++ b/SurgSim/Collision/UnitTests/CollisionPairTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -15,24 +15,23 @@
#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/Collision/UnitTests/RepresentationUtilities.h"
#include "SurgSim/DataStructures/BufferedValue.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Physics/RigidState.h"
using SurgSim::Collision::ContactMapType;
using SurgSim::DataStructures::Location;
-using SurgSim::Math::Vector3d;
using SurgSim::Math::Quaterniond;
using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
namespace SurgSim
{
@@ -47,13 +46,13 @@ TEST(CollisionPairTests, InitTest)
std::shared_ptr<Representation> rep0 = makeSphereRepresentation(1.0);
std::shared_ptr<Representation> rep1 = makeSphereRepresentation(2.0);
- EXPECT_ANY_THROW({CollisionPair pair(rep0, rep0);});
+ EXPECT_NO_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);
+ CollisionPair pair(rep0, rep1);
EXPECT_EQ(rep0, pair.getFirst());
EXPECT_EQ(rep1, pair.getSecond());
@@ -63,7 +62,7 @@ TEST(CollisionPairTests, InitTest)
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);
+ pair.addDcdContact(1.0, Vector3d(1.0, 0.0, 0.0), penetrationPoints);
EXPECT_TRUE(pair.hasContacts());
}
@@ -72,10 +71,10 @@ TEST(CollisionPairTests, SwapTest)
std::shared_ptr<Representation> rep0 = makeSphereRepresentation(1.0);
std::shared_ptr<Representation> rep1 = makeSphereRepresentation(2.0);
- CollisionPair pair(rep0,rep1);
+ CollisionPair pair(rep0, rep1);
EXPECT_FALSE(pair.isSwapped());
- EXPECT_EQ(rep0.get(),pair.getRepresentations().first.get());
- EXPECT_EQ(rep1.get(),pair.getRepresentations().second.get());
+ EXPECT_EQ(rep0.get(), pair.getRepresentations().first.get());
+ EXPECT_EQ(rep1.get(), pair.getRepresentations().second.get());
pair.swapRepresentations();
EXPECT_TRUE(pair.isSwapped());
pair.swapRepresentations();
@@ -85,7 +84,7 @@ TEST(CollisionPairTests, SwapTest)
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);
+ pair.addDcdContact(1.0, Vector3d(1.0, 0.0, 0.0), penetrationPoints);
EXPECT_TRUE(pair.hasContacts());
EXPECT_ANY_THROW(pair.swapRepresentations());
@@ -98,61 +97,137 @@ TEST(CollisionPairTests, setRepresentationsTest)
std::shared_ptr<Representation> repA = makeSphereRepresentation(99.0);
std::shared_ptr<Representation> repB = makeSphereRepresentation(100.0);
- CollisionPair pair(repA,repB);
+ CollisionPair pair(repA, repB);
EXPECT_FALSE(pair.isSwapped());
pair.swapRepresentations();
- pair.setRepresentations(rep0,rep1);
+ 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, CollisionDetectionTypeTest)
+{
+ std::shared_ptr<Representation> repCCD1 = makeSphereRepresentation(1.0);
+ repCCD1->setCollisionDetectionType(COLLISION_DETECTION_TYPE_CONTINUOUS);
+ std::shared_ptr<Representation> repCCD2 = makeSphereRepresentation(2.0);
+ repCCD2->setCollisionDetectionType(COLLISION_DETECTION_TYPE_CONTINUOUS);
+ std::shared_ptr<Representation> repDCD1 = makeSphereRepresentation(99.0);
+ repDCD1->setCollisionDetectionType(COLLISION_DETECTION_TYPE_DISCRETE);
+ std::shared_ptr<Representation> repDCD2 = makeSphereRepresentation(100.0);
+ repDCD2->setCollisionDetectionType(COLLISION_DETECTION_TYPE_DISCRETE);
+ std::shared_ptr<Representation> repNone = makeSphereRepresentation(300.0);
+ repNone->setCollisionDetectionType(COLLISION_DETECTION_TYPE_NONE);
+
+ {
+ CollisionPair pair(repCCD1, repCCD2);
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_CONTINUOUS, pair.getType());
+ }
+ {
+ CollisionPair pair(repDCD1, repDCD2);
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_DISCRETE, pair.getType());
+ }
+ {
+ CollisionPair pair(repCCD1, repDCD1);
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_DISCRETE, pair.getType());
+ }
+ {
+ CollisionPair pair(repDCD2, repCCD2);
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_DISCRETE, pair.getType());
+ }
+ {
+ CollisionPair pair(repNone, repDCD1);
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_NONE, pair.getType());
+ }
+ {
+ CollisionPair pair(repCCD1, repNone);
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_NONE, pair.getType());
+ }
+}
+
+TEST(CollisionPairTests, SelfCollisionDetectionTypeTest)
+{
+ std::shared_ptr<Representation> repCCD = makeSphereRepresentation(1.0);
+ repCCD->setSelfCollisionDetectionType(COLLISION_DETECTION_TYPE_CONTINUOUS);
+ std::shared_ptr<Representation> repDCD = makeSphereRepresentation(99.0);
+ repDCD->setSelfCollisionDetectionType(COLLISION_DETECTION_TYPE_DISCRETE);
+ std::shared_ptr<Representation> repNone = makeSphereRepresentation(300.0);
+ repNone->setSelfCollisionDetectionType(COLLISION_DETECTION_TYPE_NONE);
+
+ {
+ CollisionPair pair(repCCD, repCCD);
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_CONTINUOUS, pair.getType());
+ }
+ {
+ CollisionPair pair(repDCD, repDCD);
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_DISCRETE, pair.getType());
+ }
+ {
+ CollisionPair pair(repNone, repNone);
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_NONE, pair.getType());
+ }
+}
+
TEST(CollisionPairTests, addContactTest)
{
- std::shared_ptr<Representation> rep0 = makeSphereRepresentation(1.0);
- std::shared_ptr<Representation> rep1 = makeSphereRepresentation(2.0);
+ auto rep0 = makeSphereRepresentation(1.0);
+ auto rep1 = makeSphereRepresentation(2.0);
+ auto other = makeSphereRepresentation(3.0);
- ContactMapType& rep0Collisions = rep0->getCollisions().unsafeGet();
- ContactMapType& rep1Collisions = rep1->getCollisions().unsafeGet();
+ auto rep0Collisions = rep0->getCollisions().safeGet();
+ auto rep1Collisions = rep1->getCollisions().safeGet();
- EXPECT_TRUE(rep0Collisions.empty());
- EXPECT_TRUE(rep1Collisions.empty());
+ 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);
+ pair.addDcdContact(1.0, Vector3d::UnitY(), penetrationPoints);
+ pair.updateRepresentations();
rep0->update(0.0);
+ rep0->getCollisions().publish();
rep1->update(0.0);
+ rep1->getCollisions().publish();
+
+ rep0Collisions = rep0->getCollisions().safeGet();
+ rep1Collisions = rep1->getCollisions().safeGet();
- EXPECT_EQ(1u, rep0Collisions.size());
- auto rep0CollisionContacts = rep0Collisions.find(rep1);
- EXPECT_NE(rep0Collisions.end(), rep0CollisionContacts);
+ 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)));
+ Vector3d(0.1, 0.2, 0.3)));
EXPECT_TRUE(rep0FirstContact->penetrationPoints.second.rigidLocalPosition.getValue().isApprox(
- Vector3d(0.4, 0.5, 0.6)));
+ Vector3d(0.4, 0.5, 0.6)));
+ EXPECT_TRUE(rep0->collidedWith(rep1));
+ EXPECT_FALSE(rep0->collidedWith(other));
- EXPECT_EQ(1u, rep1Collisions.size());
- auto rep1CollisionContacts = rep1Collisions.find(rep0);
- EXPECT_NE(rep1Collisions.end(), rep1CollisionContacts);
+ 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)));
+ Vector3d(0.4, 0.5, 0.6)));
EXPECT_TRUE(rep1FirstContact->penetrationPoints.second.rigidLocalPosition.getValue().isApprox(
- Vector3d(0.1, 0.2, 0.3)));
+ Vector3d(0.1, 0.2, 0.3)));
+ EXPECT_TRUE(rep1->collidedWith(rep0));
+ EXPECT_FALSE(rep0->collidedWith(other));
+
+ rep0->retire();
+ rep1->retire();
}
}; // namespace Collision
diff --git a/SurgSim/Collision/UnitTests/CompoundShapeContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/CompoundShapeContactCalculationTests.cpp
new file mode 100644
index 0000000..ef7fdb5
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/CompoundShapeContactCalculationTests.cpp
@@ -0,0 +1,124 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/BoxPlaneContact.h"
+#include "SurgSim/Collision/BoxSphereContact.h"
+#include "SurgSim/Collision/CompoundShapeContact.h"
+#include "SurgSim/Collision/ContactCalculation.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/Math/Shapes.h"
+
+namespace SurgSim
+{
+
+typedef Math::PosedShape<std::shared_ptr<Math::Shape>> PosedShape;
+
+namespace Collision
+{
+
+class CompoundShapeDcdContactTest : public ::testing::Test
+{
+ void SetUp() override
+ {
+ ContactCalculation::registerDcdContactCalculation(std::make_shared<BoxSphereContact>());
+ ContactCalculation::registerDcdContactCalculation(std::make_shared<BoxPlaneContact>());
+ ContactCalculation::registerDcdContactCalculation(std::make_shared<CompoundShapeContact>(
+ std::make_pair(Math::SHAPE_TYPE_COMPOUNDSHAPE, Math::SHAPE_TYPE_SPHERE)));
+ ContactCalculation::registerDcdContactCalculation(std::make_shared<CompoundShapeContact>(
+ std::make_pair(Math::SHAPE_TYPE_COMPOUNDSHAPE, Math::SHAPE_TYPE_PLANE)));
+
+ }
+
+ void TearDown() override
+ {
+ }
+};
+
+// The basic principle here is that colliding a set of shapes with each other should give the same
+// results as colliding a compound representation
+TEST_F(CompoundShapeDcdContactTest, SingleCube)
+{
+ auto box = std::make_shared<Math::BoxShape>(1.0, 1.0, 1.0);
+ auto sphere = std::make_shared<Math::SphereShape>(1.0);
+
+ auto compoundShape = std::make_shared<Math::CompoundShape>();
+
+ Math::RigidTransform3d identity = Math::RigidTransform3d::Identity();
+ Math::RigidTransform3d transform = Math::makeRigidTranslation(Math::Vector3d(0.25, 0.25, 0.25));
+
+ compoundShape->addShape(box);
+
+ auto calc1 = ContactCalculation::getDcdContactTable()[Math::SHAPE_TYPE_BOX][Math::SHAPE_TYPE_SPHERE];
+ auto calc2 = ContactCalculation::getDcdContactTable()[Math::SHAPE_TYPE_COMPOUNDSHAPE][Math::SHAPE_TYPE_SPHERE];
+
+ auto expected = calc1->calculateDcdContact(PosedShape(box, identity), PosedShape(sphere, transform));
+ auto result = calc2->calculateDcdContact(PosedShape(compoundShape, identity), PosedShape(sphere, transform));
+ ASSERT_EQ(1, expected.size());
+
+ contactsInfoEqualityTest(expected, result);
+}
+
+TEST_F(CompoundShapeDcdContactTest, MultipleShapes)
+{
+ auto box = std::make_shared<Math::BoxShape>(1.0, 1.0, 1.0);
+ auto plane = std::make_shared<Math::PlaneShape>();
+
+ auto planePose = Math::makeRigidTransform(Math::makeRotationQuaternion(0.01, Vector3d::UnitX().eval()),
+ Vector3d::Zero());
+ auto basePose = Math::makeRigidTranslation(Math::Vector3d(0.0, 0.01, 0.0));
+ auto box1Pose = Math::makeRigidTransform(
+ Math::makeRotationQuaternion(0.01, Vector3d(0.01, 0.01, 0.01)),
+ Vector3d(0.1, 0.1, 0.1));
+
+ auto box2Pose = Math::makeRigidTransform(
+ Math::makeRotationQuaternion(-0.01, Vector3d(0.01, 0.01, 0.01)),
+ Vector3d(0.0, -1.0, 0.0));
+
+ auto compoundShape = std::make_shared<Math::CompoundShape>();
+
+ compoundShape->addShape(box, box1Pose);
+ compoundShape->addShape(box, box2Pose);
+
+ auto calc1 = ContactCalculation::getDcdContactTable()[Math::SHAPE_TYPE_BOX][Math::SHAPE_TYPE_PLANE];
+ auto calc2 = ContactCalculation::getDcdContactTable()[Math::SHAPE_TYPE_COMPOUNDSHAPE][Math::SHAPE_TYPE_PLANE];
+
+ auto result = calc2->calculateDcdContact(PosedShape(compoundShape->getTransformed(basePose), basePose),
+ PosedShape(plane, planePose));
+
+ std::list<std::shared_ptr<Contact>> expected =
+ calc1->calculateDcdContact(PosedShape(box, basePose * box1Pose), PosedShape(plane, planePose));
+ for (auto& contact : expected)
+ {
+ contact->penetrationPoints.first.rigidLocalPosition.setValue(box1Pose *
+ contact->penetrationPoints.first.rigidLocalPosition.getValue());
+ }
+
+ auto box2Expected = calc1->calculateDcdContact(PosedShape(box, basePose * box2Pose), PosedShape(plane, planePose));
+ for (auto& contact : box2Expected)
+ {
+ contact->penetrationPoints.first.rigidLocalPosition.setValue(box2Pose *
+ contact->penetrationPoints.first.rigidLocalPosition.getValue());
+ }
+ expected.splice(expected.end(), box2Expected);
+ ASSERT_EQ(12, expected.size());
+
+ contactsInfoEqualityTest(expected, result);
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Collision/UnitTests/ContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/ContactCalculationTests.cpp
index 0b80960..563d9fe 100644
--- a/SurgSim/Collision/UnitTests/ContactCalculationTests.cpp
+++ b/SurgSim/Collision/UnitTests/ContactCalculationTests.cpp
@@ -17,7 +17,7 @@
#include <gtest/gtest.h>
#include "SurgSim/Collision/ContactCalculation.h"
#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
-#include "SurgSim/Collision/SpherePlaneDcdContact.h"
+#include "SurgSim/Collision/SpherePlaneContact.h"
#include "SurgSim/Math/SphereShape.h"
#include "SurgSim/Math/PlaneShape.h"
@@ -37,6 +37,9 @@ using SurgSim::Math::SphereShape;
namespace SurgSim
{
+
+typedef Math::PosedShape<std::shared_ptr<Math::Shape>> PosedShape;
+
namespace Collision
{
@@ -48,7 +51,7 @@ 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);
+ Vector3d trans(0.0, 0.0, 0.0);
Quaterniond quat = Quaterniond::Identity();
std::shared_ptr<ShapeCollisionRepresentation> planeRep =
@@ -64,12 +67,50 @@ TEST(ContactCalculationTests, SwappedPairTest)
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>();
+ std::shared_ptr<SpherePlaneContact> calc = std::make_shared<SpherePlaneContact>();
EXPECT_NO_THROW(calc->calculateContact(pair1));
EXPECT_NO_THROW(calc->calculateContact(pair2));
}
+TEST(ContactCalculationTests, SwappedShapeTest)
+{
+ std::shared_ptr<PlaneShape> plane = std::make_shared<PlaneShape>();
+ std::shared_ptr<SphereShape> sphere = std::make_shared<SphereShape>(1.0);
+ std::shared_ptr<SpherePlaneContact> calc = std::make_shared<SpherePlaneContact>();
+
+ auto transform = Math::RigidTransform3d::Identity();
+
+ ASSERT_NO_THROW(calc->calculateDcdContact(PosedShape(sphere, transform), PosedShape(plane, transform)));
+ ASSERT_NO_THROW(calc->calculateDcdContact(PosedShape(plane, transform), PosedShape(sphere, transform)));
+
+ auto planeRep = std::make_shared<ShapeCollisionRepresentation>("Plane Shape");
+ planeRep->setShape(plane);
+ planeRep->setLocalPose(transform);
+
+ auto sphereRep = std::make_shared<ShapeCollisionRepresentation>("Sphere Shape");
+ sphereRep->setShape(sphere);
+ sphereRep->setLocalPose(transform);
+
+ std::shared_ptr<CollisionPair> pair1 = std::make_shared<CollisionPair>(sphereRep, planeRep);
+
+ calc->calculateContact(pair1);
+
+ auto contacts1 = calc->calculateDcdContact(PosedShape(sphere, transform), PosedShape(plane, transform));
+ auto contacts2 = calc->calculateDcdContact(PosedShape(plane, transform), PosedShape(sphere, transform));
+
+ contactsInfoEqualityTest(pair1->getContacts(), contacts1);
+
+ // Contacts2 should be flipped from contacts1
+ for (auto& contact : contacts2)
+ {
+ contact->normal = -contact->normal;
+ std::swap(contact->penetrationPoints.first, contact->penetrationPoints.second);
+ }
+
+ contactsInfoEqualityTest(contacts1 , contacts2);
+}
+
}; // namespace Collision
}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.cpp b/SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.cpp
index bde50f1..9d875f0 100644
--- a/SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.cpp
+++ b/SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -15,6 +15,9 @@
#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+
+
namespace SurgSim
{
namespace Collision
@@ -33,24 +36,27 @@ namespace Collision
}
}
-void checkContactInfo(std::shared_ptr<Contact> contact, double expectedDepth,
- Vector3d &expectedNormal, Vector3d &expectedPenetrationPointFirst,
- Vector3d &expectedPenetrationPointSecond)
+void checkContactInfo(std::shared_ptr<Contact> contact, CollisionDetectionType expectedType,
+ double expectedDepth, double expectedTime,
+ const Vector3d& expectedNormal, const Vector3d& expectedPenetrationPointFirst,
+ const Vector3d& expectedPenetrationPointSecond)
{
+ EXPECT_EQ(expectedType, contact->type);
EXPECT_NEAR(expectedDepth, contact->depth, SurgSim::Math::Geometry::DistanceEpsilon);
+ EXPECT_NEAR(expectedTime, contact->time, 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()));
+ contact->penetrationPoints.first.rigidLocalPosition.getValue()));
EXPECT_TRUE(eigenEqual(expectedPenetrationPointSecond,
- contact->penetrationPoints.second.rigidLocalPosition.getValue()));
+ contact->penetrationPoints.second.rigidLocalPosition.getValue()));
}
bool checkMeshLocalCoordinate(
- SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::IndexedLocalCoordinate>& actualLocalCoordinate,
+ const SurgSim::DataStructures::OptionalValue<IndexedLocalCoordinate>& actualLocalCoordinate,
const std::array<SurgSim::Math::Vector3d, 3>& vertices,
- SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::IndexedLocalCoordinate>& expectedLocalCoordinate,
+ const SurgSim::DataStructures::OptionalValue<IndexedLocalCoordinate>& expectedLocalCoordinate,
const SurgSim::Math::Vector3d& expectedLocalPosition)
{
bool isEqual = true;
@@ -69,8 +75,8 @@ bool checkMeshLocalCoordinate(
}
::testing::AssertionResult isContactPresentInList(std::shared_ptr<Contact> expected,
- const std::list<std::shared_ptr<Contact>>& contactsList,
- bool expectedHasTriangleContactObject)
+ const std::list<std::shared_ptr<Contact>>& contactsList,
+ bool expectedHasTriangleContactObject)
{
using SurgSim::Math::Geometry::ScalarEpsilon;
@@ -87,6 +93,10 @@ bool checkMeshLocalCoordinate(
it->get()->penetrationPoints.second.rigidLocalPosition.getValue());
// Compare the depth.
contactPresent &= std::abs(expected->depth - it->get()->depth) <= ScalarEpsilon;
+ // Compare the time.
+ contactPresent &= std::abs(expected->time - it->get()->time) <= ScalarEpsilon;
+ // Compare the contact types.
+ contactPresent &= (expected->type == it->get()->type);
// Check if the optional 'meshLocalCoordinate' are the same.
std::shared_ptr<SurgSim::Collision::TriangleContact> triangleContact;
std::shared_ptr<SurgSim::Collision::Contact> contact;
@@ -101,15 +111,15 @@ bool checkMeshLocalCoordinate(
contact = expected;
}
contactPresent &= checkMeshLocalCoordinate(
- contact->penetrationPoints.first.meshLocalCoordinate,
- triangleContact->firstVertices,
- triangleContact->penetrationPoints.first.meshLocalCoordinate,
- expected->penetrationPoints.first.rigidLocalPosition.getValue());
+ contact->penetrationPoints.first.triangleMeshLocalCoordinate,
+ triangleContact->firstVertices,
+ triangleContact->penetrationPoints.first.triangleMeshLocalCoordinate,
+ expected->penetrationPoints.first.rigidLocalPosition.getValue());
contactPresent &= checkMeshLocalCoordinate(
- contact->penetrationPoints.second.meshLocalCoordinate,
- triangleContact->secondVertices,
- triangleContact->penetrationPoints.second.meshLocalCoordinate,
- expected->penetrationPoints.second.rigidLocalPosition.getValue());
+ contact->penetrationPoints.second.triangleMeshLocalCoordinate,
+ triangleContact->secondVertices,
+ triangleContact->penetrationPoints.second.triangleMeshLocalCoordinate,
+ expected->penetrationPoints.second.rigidLocalPosition.getValue());
}
if (contactPresent)
@@ -166,8 +176,9 @@ void generateBoxPlaneContact(std::list<std::shared_ptr<Contact>>* expectedContac
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));
+ expectedContacts->push_back(std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, depth, 1.0,
+ Vector3d::Zero(), collisionNormal, penetrationPoint));
}
}
@@ -196,8 +207,9 @@ void generateBoxDoubleSidedPlaneContact(std::list<std::shared_ptr<Contact>>* exp
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));
+ expectedContacts->push_back(std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, std::abs(depth),
+ 1.0, Vector3d::Zero(), collisionNormal, penetrationPoint));
}
}
diff --git a/SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h b/SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h
index cdfc782..906ae7c 100644
--- a/SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h
+++ b/SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -30,6 +30,7 @@
#include "SurgSim/Math/PlaneShape.h"
#include "SurgSim/Math/DoubleSidedPlaneShape.h"
#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/SegmentMeshShape.h"
#include "SurgSim/Collision/Representation.h"
#include "SurgSim/Collision/ContactCalculation.h"
@@ -48,6 +49,7 @@ using SurgSim::Math::BoxShape;
using SurgSim::Math::PlaneShape;
using SurgSim::Math::DoubleSidedPlaneShape;
using SurgSim::Math::MeshShape;
+using SurgSim::Math::SegmentMeshShape;
namespace SurgSim
{
@@ -57,11 +59,13 @@ 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)
+ TriangleContact(const CollisionDetectionType newCollisionDetectionType,
+ const double& newDepth,
+ const double& newTime,
+ const SurgSim::Math::Vector3d& newContact,
+ const SurgSim::Math::Vector3d& newNormal,
+ const std::pair<Location, Location>& newPenetrationPoints)
+ : Contact(newCollisionDetectionType, newDepth, newTime, newContact, newNormal, newPenetrationPoints)
{
}
@@ -82,9 +86,10 @@ struct TriangleContact : public Contact
/// \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);
+void checkContactInfo(std::shared_ptr<Contact> contact, CollisionDetectionType expectedType,
+ double expectedDepth, double expectedTime,
+ const Vector3d& expectedNormal, const Vector3d& expectedPenetrationPointFirst,
+ const Vector3d& expectedPenetrationPointSecond);
/// Function that checks if a given contact is present in the list of given contacts.
/// \param expected The expected contact.
@@ -92,8 +97,8 @@ void checkContactInfo(std::shared_ptr<Contact> contact, double expectedDepth,
/// \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);
+ 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.
diff --git a/SurgSim/Collision/UnitTests/ElementContactFilterTests.cpp b/SurgSim/Collision/UnitTests/ElementContactFilterTests.cpp
new file mode 100644
index 0000000..63bdadb
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/ElementContactFilterTests.cpp
@@ -0,0 +1,289 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 <memory>
+
+#include "SurgSim/Collision/ElementContactFilter.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/SegmentMeshShape.h"
+
+namespace
+{
+
+std::shared_ptr<SurgSim::Collision::CollisionPair> makePair()
+{
+ auto rep1 = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("rep1");
+ rep1->setShape(std::make_shared<SurgSim::Math::MeshShape>());
+
+ auto rep2 = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("rep2");
+ rep2->setShape(std::make_shared<SurgSim::Math::SegmentMeshShape>());
+
+ return std::make_shared <SurgSim::Collision::CollisionPair>(rep1, rep2);
+}
+
+
+std::shared_ptr<SurgSim::Collision::Contact> makeContact(
+ size_t triMesh1, size_t elementMesh1,
+ size_t triMesh2, size_t elementMesh2)
+{
+ std::pair<SurgSim::DataStructures::Location, SurgSim::DataStructures::Location> penetrationPoints;
+
+ penetrationPoints.first.triangleMeshLocalCoordinate =
+ SurgSim::DataStructures::IndexedLocalCoordinate(triMesh1, SurgSim::Math::Vector3d::Zero());
+ penetrationPoints.first.elementMeshLocalCoordinate =
+ SurgSim::DataStructures::IndexedLocalCoordinate(elementMesh1, SurgSim::Math::Vector3d::Zero());
+
+ penetrationPoints.second.triangleMeshLocalCoordinate =
+ SurgSim::DataStructures::IndexedLocalCoordinate(triMesh2, SurgSim::Math::Vector3d::Zero());
+ penetrationPoints.second.elementMeshLocalCoordinate =
+ SurgSim::DataStructures::IndexedLocalCoordinate(elementMesh2, SurgSim::Math::Vector3d::Zero());
+
+ return std::make_shared<SurgSim::Collision::Contact>(
+ SurgSim::Collision::COLLISION_DETECTION_TYPE_DISCRETE, 0.0, 1.0,
+ SurgSim::Math::Vector3d::Zero(), SurgSim::Math::Vector3d::Zero(), penetrationPoints);
+
+}
+
+
+
+}
+
+
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+class ElementContactFilterTest : public testing::Test
+{
+
+public:
+ void SetUp()
+ {
+ filter = std::make_shared<ElementContactFilter>("filter");
+ pair = makePair();
+ }
+
+ std::shared_ptr<Physics::PhysicsManagerState> state;
+ std::shared_ptr<ElementContactFilter> filter;
+ std::shared_ptr<SurgSim::Collision::CollisionPair> pair;
+};
+
+
+TEST_F(ElementContactFilterTest, Accessors)
+{
+
+ {
+ std::shared_ptr<Framework::Component> rep = std::make_shared<Physics::RigidCollisionRepresentation>("rep");
+ filter->setValue("Representation", rep);
+ auto result = filter->getValue <std::shared_ptr<Collision::Representation>>("Representation");
+ EXPECT_EQ(rep.get(), result.get());
+ }
+
+ {
+
+ typedef std::vector<std::pair<std::shared_ptr<SurgSim::Framework::Component>, std::vector<size_t>>>
+ FilterMapType;
+
+ FilterMapType empty;
+
+ EXPECT_NO_THROW(filter->setValue("FilterElements", empty));
+ filter->update(0.0);
+ auto result = filter->getValue<FilterMapType>("FilterElements");
+ EXPECT_EQ(0u, result.size());
+
+ FilterMapType filters;
+ filters.emplace_back(pair->getRepresentations().first, std::vector<size_t>(3, 1));
+ filters.emplace_back(pair->getRepresentations().second, std::vector<size_t>(5, 1));
+ filter->setValue("FilterElements", filters);
+ filter->update(0.0);
+ result = filter->getValue<FilterMapType>("FilterElements");
+
+ EXPECT_EQ(2u, result.size());
+ for (const auto& item : result)
+ {
+ if (item.first.get() == pair->getRepresentations().first.get())
+ {
+ EXPECT_EQ(3u, item.second.size());
+ }
+ else if (item.first.get() == pair->getRepresentations().second.get())
+ {
+ EXPECT_EQ(5u, item.second.size());
+ }
+ else
+ {
+ ADD_FAILURE() << "Found invalid representation in filters.";
+ }
+ }
+ }
+
+}
+
+TEST_F(ElementContactFilterTest, SetGetFilter)
+{
+ auto result = filter->getFilter(nullptr);
+
+ EXPECT_EQ(0u, result.size());
+
+ auto rep = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("representation");
+ rep->setShape(std::make_shared<SurgSim::Math::SegmentMeshShape>());
+
+ result = filter->getFilter(rep);
+ EXPECT_EQ(0u, result.size());
+
+ std::vector<size_t> ignores(2, 1);
+ filter->setFilter(rep, ignores);
+ filter->update(0.0);
+
+ result = filter->getFilter(rep);
+ EXPECT_EQ(2u, result.size());
+
+ std::vector<size_t> empty;
+ filter->setFilter(rep, empty);
+ filter->update(0.0);
+
+ result = filter->getFilter(rep);
+ EXPECT_EQ(0u, result.size());
+
+ filter->setFilter(pair->getRepresentations().second, ignores);
+ filter->update(0.0);
+
+ result = filter->getFilter(rep);
+ EXPECT_EQ(0u, result.size());
+
+ result = filter->getFilter(pair->getRepresentations().second);
+ EXPECT_EQ(2u, result.size());
+
+}
+
+TEST_F(ElementContactFilterTest, Noop)
+{
+ EXPECT_NO_THROW(filter->filterContacts(state, pair));
+ std::vector<size_t> ignores(1, 1);
+ filter->setRepresentation(pair->getRepresentations().first);
+ filter->setFilter(pair->getRepresentations().second, ignores);
+ filter->update(0.0);
+ EXPECT_NO_THROW(filter->filterContacts(state, pair));
+
+ pair->addContact(makeContact(0, 0, 0, 0));
+ EXPECT_NO_THROW(filter->filterContacts(state, pair));
+}
+
+TEST_F(ElementContactFilterTest, OnlyRemoveMatches)
+{
+ auto rep3 = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("rep3");
+ rep3->setShape(std::make_shared<SurgSim::Math::SegmentMeshShape>());
+
+ std::vector<size_t> ignores;
+ ignores.push_back(0);
+ ignores.push_back(2);
+ filter->setRepresentation(pair->getRepresentations().first);
+ filter->setFilter(rep3, ignores);
+
+
+ pair->addContact(makeContact(2, 10, 10, 10));
+ pair->addContact(makeContact(0, 10, 10, 10));
+ pair->addContact(makeContact(1, 10, 10, 10));
+ pair->addContact(makeContact(1, 10, 10, 10));
+ pair->addContact(makeContact(2, 10, 10, 10));
+ pair->addContact(makeContact(2, 10, 10, 10));
+
+ EXPECT_EQ(6u, pair->getContacts().size());
+ EXPECT_NO_THROW(filter->filterContacts(state, pair));
+ EXPECT_EQ(6u, pair->getContacts().size());
+ filter->update(0.0);
+ EXPECT_NO_THROW(filter->filterContacts(state, pair));
+ EXPECT_EQ(6u, pair->getContacts().size());
+}
+
+TEST_F(ElementContactFilterTest, RemoveOnTriangleMesh)
+{
+
+ std::vector<size_t> ignores;
+ ignores.push_back(0);
+ ignores.push_back(2);
+ filter->setRepresentation(pair->getRepresentations().first);
+ filter->setFilter(pair->getRepresentations().second, ignores);
+
+ pair->addContact(makeContact(2, 10, 10, 10));
+ pair->addContact(makeContact(0, 10, 10, 10));
+ pair->addContact(makeContact(1, 10, 10, 10));
+ pair->addContact(makeContact(1, 10, 10, 10));
+ pair->addContact(makeContact(2, 10, 10, 10));
+ pair->addContact(makeContact(2, 10, 10, 10));
+
+ EXPECT_EQ(6u, pair->getContacts().size());
+ EXPECT_NO_THROW(filter->filterContacts(state, pair));
+ EXPECT_EQ(6u, pair->getContacts().size());
+ filter->update(0.0);
+ EXPECT_NO_THROW(filter->filterContacts(state, pair));
+ EXPECT_EQ(2u, pair->getContacts().size());
+}
+
+TEST_F(ElementContactFilterTest, RemoveOnTriangleMeshSwapped)
+{
+ std::vector<size_t> ignores;
+ ignores.push_back(0);
+ ignores.push_back(2);
+ filter->setRepresentation(pair->getRepresentations().first);
+ filter->setFilter(pair->getRepresentations().second, ignores);
+ filter->update(0.0);
+
+ pair->swapRepresentations();
+
+ pair->addContact(makeContact(10, 10, 2, 10));
+ pair->addContact(makeContact(10, 10, 0, 10));
+ pair->addContact(makeContact(10, 10, 1, 10));
+ pair->addContact(makeContact(10, 10, 1, 10));
+ pair->addContact(makeContact(10, 10, 2, 10));
+ pair->addContact(makeContact(10, 10, 2, 10));
+
+
+ EXPECT_EQ(6u, pair->getContacts().size());
+ EXPECT_NO_THROW(filter->filterContacts(state, pair));
+ EXPECT_EQ(2u, pair->getContacts().size());
+}
+
+TEST_F(ElementContactFilterTest, RemoveOnSegmentMesh)
+{
+ std::vector<size_t> ignores;
+ ignores.push_back(0);
+ ignores.push_back(2);
+ filter->setRepresentation(pair->getRepresentations().second);
+ filter->setFilter(pair->getRepresentations().first, ignores);
+ filter->update(0.0);
+
+ pair->addContact(makeContact(10, 10, 10, 0));
+ pair->addContact(makeContact(10, 10, 10, 1));
+ pair->addContact(makeContact(10, 10, 10, 1));
+ pair->addContact(makeContact(10, 10, 10, 2));
+ pair->addContact(makeContact(10, 10, 10, 2));
+ pair->addContact(makeContact(10, 10, 10, 2));
+
+ EXPECT_EQ(6u, pair->getContacts().size());
+ filter->filterContacts(state, pair);
+ EXPECT_EQ(2u, pair->getContacts().size());
+}
+
+}
+}
diff --git a/SurgSim/Collision/UnitTests/OctreeContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/OctreeContactCalculationTests.cpp
index 1cebbfa..a572c42 100644
--- a/SurgSim/Collision/UnitTests/OctreeContactCalculationTests.cpp
+++ b/SurgSim/Collision/UnitTests/OctreeContactCalculationTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,11 +16,14 @@
#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/CcdDcdCollision.h"
+#include "SurgSim/Collision/OctreeCapsuleContact.h"
+#include "SurgSim/Collision/OctreeDoubleSidedPlaneContact.h"
+#include "SurgSim/Collision/OctreePlaneContact.h"
+#include "SurgSim/Collision/OctreeSphereContact.h"
#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/DataStructures/OctreeNode.h"
#include "SurgSim/Math/Geometry.h"
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/RigidTransform.h"
@@ -28,10 +31,10 @@
#include "SurgSim/Math/Shapes.h"
#include "SurgSim/Math/Vector.h"
+using SurgSim::Collision::ShapeCollisionRepresentation;
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;
@@ -44,23 +47,26 @@ using SurgSim::Math::Shape;
using SurgSim::Math::SphereShape;
using SurgSim::Math::Vector3d;
-namespace SurgSim
-{
-namespace Collision
-{
-
struct OctreeData
{
std::string name;
};
+template<>
+std::string SurgSim::DataStructures::OctreeNode<OctreeData>::m_className = "OctreeNode<OctreeData>";
+
+namespace SurgSim
+{
+namespace Collision
+{
+
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)
+ 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");
@@ -74,13 +80,13 @@ std::list<std::shared_ptr<Contact>> doCollision(std::shared_ptr<Shape> octree,
// Perform collision detection.
std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(octreeRep, shapeRep);
- calculator.calculateContact(pair);
+ 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)
+ for (auto contact = contacts.cbegin(); contact != contacts.cend(); ++contact)
{
Location& location = (*contact)->penetrationPoints.first;
ASSERT_TRUE(location.octreeNodePath.hasValue());
@@ -93,9 +99,9 @@ void checkContacts(const std::list<std::shared_ptr<Contact>>& contacts, std::sha
}
bool nodeInContacts(const std::string& name, const std::list<std::shared_ptr<Contact>>& contacts,
- std::shared_ptr<OctreeNode<OctreeData>> octree)
+ std::shared_ptr<OctreeNode<OctreeData>> octree)
{
- for(auto contact = contacts.cbegin(); contact != contacts.cend(); ++contact)
+ 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);
@@ -115,35 +121,35 @@ std::shared_ptr<OctreeNode<OctreeData>> buildTestOctree()
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);
+ 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);
+ rootNode->addData(Vector3d(8.5, 8.5, 8.5), numLevels, data);
data.name = "corner0";
- rootNode->addData(Vector3d( 0.5, 0.5, 0.5), data, numLevels);
+ rootNode->addData(Vector3d(0.5, 0.5, 0.5), numLevels, data);
data.name = "corner1";
- rootNode->addData(Vector3d(15.5, 0.5, 0.5), data, numLevels);
+ rootNode->addData(Vector3d(15.5, 0.5, 0.5), numLevels, data);
data.name = "corner2";
- rootNode->addData(Vector3d( 0.5, 15.5, 0.5), data, numLevels);
+ rootNode->addData(Vector3d(0.5, 15.5, 0.5), numLevels, data);
data.name = "corner3";
- rootNode->addData(Vector3d(15.5, 15.5, 0.5), data, numLevels);
+ rootNode->addData(Vector3d(15.5, 15.5, 0.5), numLevels, data);
data.name = "corner4";
- rootNode->addData(Vector3d( 0.5, 0.5, 15.5), data, numLevels);
+ rootNode->addData(Vector3d(0.5, 0.5, 15.5), numLevels, data);
data.name = "corner5";
- rootNode->addData(Vector3d(15.5, 0.5, 15.5), data, numLevels);
+ rootNode->addData(Vector3d(15.5, 0.5, 15.5), numLevels, data);
data.name = "corner6";
- rootNode->addData(Vector3d( 0.5, 15.5, 15.5), data, numLevels);
+ rootNode->addData(Vector3d(0.5, 15.5, 15.5), numLevels, data);
data.name = "corner7";
- rootNode->addData(Vector3d(15.5, 15.5, 15.5), data, numLevels);
+ rootNode->addData(Vector3d(15.5, 15.5, 15.5), numLevels, data);
return rootNode;
}
@@ -153,45 +159,45 @@ 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>());
+ OctreeCapsuleContact calculator;
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);
+ 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);
+ 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);
+ 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));
}
@@ -199,13 +205,13 @@ TEST(OctreeContactCalculationTests, Capsule)
{
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);
+ 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));
@@ -214,13 +220,13 @@ TEST(OctreeContactCalculationTests, Capsule)
{
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);
+ 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));
}
@@ -231,32 +237,32 @@ 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>());
+ OctreePlaneContact calculator;
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);
+ 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);
+ 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));
@@ -267,13 +273,13 @@ TEST(OctreeContactCalculationTests, Plane)
{
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);
+ 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));
@@ -282,13 +288,13 @@ TEST(OctreeContactCalculationTests, Plane)
{
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);
+ 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));
@@ -303,32 +309,32 @@ 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>());
+ OctreeDoubleSidedPlaneContact calculator;
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);
+ 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);
+ 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));
@@ -339,13 +345,13 @@ TEST(OctreeContactCalculationTests, DoubleSidedPlane)
{
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);
+ 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));
@@ -360,32 +366,32 @@ 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>());
+ OctreeSphereContact calculator;
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);
+ 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);
+ 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));
}
@@ -393,13 +399,13 @@ TEST(OctreeContactCalculationTests, Sphere)
{
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);
+ 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));
@@ -411,17 +417,43 @@ TEST(OctreeContactCalculationTests, Sphere)
{
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);
+ 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());
}
}
+TEST(OctreeContactCalculationTests, CheckNumberOfContacts)
+{
+ 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);
+ OctreeSphereContact calculator;
+
+ std::shared_ptr<ShapeCollisionRepresentation> octreeRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Octree 0");
+ octreeRep->setShape(octreeShape);
+
+ std::shared_ptr<ShapeCollisionRepresentation> shapeRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision sphere 0");
+ shapeRep->setShape(sphereShape);
+ shapeRep->setLocalPose(SurgSim::Math::makeRigidTranslation(Vector3d(8.0, 8.0, 8.0)));
+
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(octreeRep, shapeRep);
+ calculator.calculateContact(pair);
+ pair->updateRepresentations();
+
+ EXPECT_EQ(1, shapeRep->getCollisions().unsafeGet().size());
+ EXPECT_EQ(1, shapeRep->getCollisions().unsafeGet().count(octreeRep));
+
+ octreeRep->retire();
+ shapeRep->retire();
+}
}; // namespace Collision
}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/RepresentationTest.cpp b/SurgSim/Collision/UnitTests/RepresentationTest.cpp
index 2ff5aa1..3961a64 100644
--- a/SurgSim/Collision/UnitTests/RepresentationTest.cpp
+++ b/SurgSim/Collision/UnitTests/RepresentationTest.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -15,11 +15,16 @@
#include <gtest/gtest.h>
+#include <yaml-cpp/yaml.h>
#include "SurgSim/Collision/CollisionPair.h"
#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
#include "SurgSim/DataStructures/BufferedValue.h"
#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/ThreadPool.h"
#include "SurgSim/Math/PlaneShape.h"
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/RigidTransform.h"
@@ -40,7 +45,7 @@ using SurgSim::Math::Vector3d;
namespace
{
- const double epsilon = 1e-10;
+const double epsilon = 1e-10;
};
namespace SurgSim
@@ -52,6 +57,8 @@ struct RepresentationTest : public ::testing::Test
{
virtual void SetUp()
{
+ runtime = std::make_shared<Framework::Runtime>();
+ scene = runtime->getScene();
element = std::make_shared<BasicSceneElement>("Element");
plane = std::make_shared<PlaneShape>();
sphere = std::make_shared<SphereShape>(1.0);
@@ -66,7 +73,7 @@ struct RepresentationTest : public ::testing::Test
element->addComponent(planeRep);
element->addComponent(sphereRep);
- element->initialize();
+ scene->addSceneElement(element);
planeRep->wakeUp();
sphereRep->wakeUp();
}
@@ -75,6 +82,9 @@ struct RepresentationTest : public ::testing::Test
{
}
+ std::shared_ptr<Framework::Runtime> runtime;
+ std::shared_ptr<Framework::Scene> scene;
+
std::shared_ptr<BasicSceneElement> element;
std::shared_ptr<PlaneShape> plane;
std::shared_ptr<SphereShape> sphere;
@@ -87,6 +97,18 @@ TEST_F(RepresentationTest, InitTest)
EXPECT_NO_THROW(ShapeCollisionRepresentation("Plane"));
}
+TEST_F(RepresentationTest, CollisionDetectionType)
+{
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_NONE, sphereRep->getSelfCollisionDetectionType());
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_DISCRETE, sphereRep->getCollisionDetectionType());
+
+ sphereRep->setCollisionDetectionType(COLLISION_DETECTION_TYPE_NONE);
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_NONE, sphereRep->getCollisionDetectionType());
+
+ sphereRep->setSelfCollisionDetectionType(COLLISION_DETECTION_TYPE_CONTINUOUS);
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_CONTINUOUS, sphereRep->getSelfCollisionDetectionType());
+}
+
TEST_F(RepresentationTest, PoseTest)
{
RigidTransform3d initialPose = makeRigidTransform(Quaterniond::Identity(), Vector3d(1.0, 2.0, 3.0));
@@ -127,7 +149,9 @@ TEST_F(RepresentationTest, CollisionTest)
EXPECT_TRUE(safePlaneCollisions->empty());
std::shared_ptr<Contact> dummyContact =
- std::make_shared<Contact>(0.0, Vector3d::Zero(), Vector3d::Zero(), std::make_pair(Location(), Location()));
+ std::make_shared<Contact>(COLLISION_DETECTION_TYPE_DISCRETE,
+ 0.0, 1.0, Vector3d::Zero(), Vector3d::Zero(),
+ std::make_pair(Location(), Location()));
unsafeSphereCollisions[planeRep].push_back(dummyContact);
auto spherePlanePair = unsafeSphereCollisions.find(planeRep);
@@ -146,6 +170,142 @@ TEST_F(RepresentationTest, CollisionTest)
EXPECT_EQ(unsafePlaneCollisions, *planeRep->getCollisions().safeGet());
}
+// addContact method thread-safety test case.
+// WARNING: Due to the nature of multi-threaded environment, a successful test does not imply thread-safety
+// also note the lack of reproducibility.
+TEST_F(RepresentationTest, AddContactsInParallelTest)
+{
+ auto rep = std::make_shared<ShapeCollisionRepresentation>("collisionRepReference");
+ auto contact = std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, 0.1, 1.0,
+ Math::Vector3d::Zero(), Math::Vector3d::Zero(),
+ std::make_pair(DataStructures::Location(), DataStructures::Location()));
+ auto threadPool = Framework::Runtime::getThreadPool();
+ std::vector<std::future<void>> tasks;
+ const size_t numContacts = 500;
+
+ for (size_t i = 0; i < numContacts; i++)
+ {
+ tasks.push_back(threadPool->enqueue<void>([&rep, &contact]()
+ {
+ rep->addContact(rep, contact);
+ }));
+ }
+
+ std::for_each(tasks.begin(), tasks.end(), [](std::future<void>& p)
+ {
+ p.get();
+ });
+ ASSERT_EQ(numContacts, rep->getCollisions().unsafeGet()[rep].size());
+ rep->retire();
+}
+
+TEST_F(RepresentationTest, Ignoring)
+{
+ EXPECT_TRUE(planeRep->ignore("Test"));
+ EXPECT_FALSE(planeRep->ignore("Test"));
+ EXPECT_TRUE(planeRep->ignore(sphereRep));
+
+ EXPECT_TRUE(planeRep->isIgnoring("Test"));
+ EXPECT_TRUE(planeRep->isIgnoring("Element/SphereShape"));
+ EXPECT_FALSE(planeRep->isIgnoring("Invalid"));
+ EXPECT_FALSE(planeRep->isAllowing("Test"));
+ EXPECT_FALSE(planeRep->isAllowing("Element/SphereShape"));
+ EXPECT_TRUE(planeRep->isAllowing("Invalid"));
+
+ // Remove and add Test back from the ignoring list
+ EXPECT_TRUE(planeRep->allow("Test"));
+ EXPECT_FALSE(planeRep->isIgnoring("Test"));
+ EXPECT_FALSE(planeRep->allow("Test"));
+ EXPECT_TRUE(planeRep->ignore("Test"));
+ EXPECT_TRUE(planeRep->isIgnoring("Test"));
+
+
+ std::vector<std::string> newExclusions;
+ newExclusions.push_back("Test");
+ newExclusions.push_back("Element/PlaneShape");
+ sphereRep->setIgnoring(newExclusions);
+ EXPECT_TRUE(sphereRep->isIgnoring("Test"));
+ EXPECT_TRUE(sphereRep->isIgnoring("Element/PlaneShape"));
+ EXPECT_FALSE(sphereRep->isIgnoring("Invalid"));
+ EXPECT_FALSE(sphereRep->isAllowing("Test"));
+ EXPECT_FALSE(sphereRep->isAllowing("Element/PlaneShape"));
+ EXPECT_TRUE(sphereRep->isAllowing("Invalid"));
+
+ std::vector<std::string> allowing;
+ allowing.push_back("Invalid");
+ sphereRep->setAllowing(allowing);
+ EXPECT_FALSE(sphereRep->allow("Invalid"));
+ EXPECT_TRUE(sphereRep->isIgnoring("Test"));
+ EXPECT_TRUE(sphereRep->isIgnoring("Element/PlaneShape"));
+ EXPECT_FALSE(sphereRep->isIgnoring("Invalid"));
+ EXPECT_FALSE(sphereRep->isAllowing("Test"));
+ EXPECT_FALSE(sphereRep->isAllowing("Element/PlaneShape"));
+ EXPECT_TRUE(sphereRep->isAllowing("Invalid"));
+}
+
+TEST_F(RepresentationTest, Allowing)
+{
+ EXPECT_FALSE(sphereRep->isIgnoring("Other"));
+ EXPECT_FALSE(sphereRep->isIgnoring("CollideWith1"));
+ EXPECT_FALSE(sphereRep->isIgnoring("CollideWith2"));
+ EXPECT_TRUE(sphereRep->isAllowing("Other"));
+ EXPECT_TRUE(sphereRep->isAllowing("CollideWith1"));
+ EXPECT_TRUE(sphereRep->isAllowing("CollideWith2"));
+
+ std::vector<std::string> allowing;
+ allowing.push_back("CollideWith1");
+ allowing.push_back("CollideWith2");
+ sphereRep->setAllowing(allowing);
+ sphereRep->allow("CollideWith3");
+
+ EXPECT_TRUE(sphereRep->isIgnoring("Other"));
+ EXPECT_FALSE(sphereRep->isIgnoring("CollideWith1"));
+ EXPECT_FALSE(sphereRep->isIgnoring("CollideWith2"));
+ EXPECT_FALSE(sphereRep->isIgnoring("CollideWith3"));
+ EXPECT_FALSE(sphereRep->isAllowing("Other"));
+ EXPECT_TRUE(sphereRep->isAllowing("CollideWith1"));
+ EXPECT_TRUE(sphereRep->isAllowing("CollideWith2"));
+ EXPECT_TRUE(sphereRep->isAllowing("CollideWith3"));
+
+
+ /// CollideWith1 is will be removed from the 'allowing'
+ EXPECT_TRUE(sphereRep->ignore("CollideWith1"));
+ EXPECT_TRUE(sphereRep->isIgnoring("CollideWith1"));
+ EXPECT_FALSE(sphereRep->isAllowing("CollideWith1"));
+ EXPECT_TRUE(sphereRep->allow("CollideWith1"));
+ EXPECT_TRUE(sphereRep->isAllowing("CollideWith1"));
+
+}
+
+TEST_F(RepresentationTest, SerializationTest)
+{
+ std::vector<std::string> ignoring;
+ ignoring.push_back("Test");
+ ignoring.push_back("Element/PlaneShape");
+ EXPECT_NO_THROW(sphereRep->setValue("Ignore", ignoring));
+
+ EXPECT_NO_THROW(sphereRep->setValue("CollisionDetectionType", COLLISION_DETECTION_TYPE_CONTINUOUS));
+ EXPECT_NO_THROW(sphereRep->setValue("SelfCollisionDetectionType", COLLISION_DETECTION_TYPE_DISCRETE));
+
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<Framework::Component>::encode(*sphereRep));
+ EXPECT_TRUE(node.IsMap());
+
+ std::shared_ptr<Representation> decodedSphereRep;
+ ASSERT_NO_THROW(decodedSphereRep = std::dynamic_pointer_cast<Representation>(
+ node.as<std::shared_ptr<Framework::Component>>()));
+
+ ASSERT_NE(nullptr, decodedSphereRep);
+ EXPECT_TRUE(decodedSphereRep->isIgnoring("Test"));
+ EXPECT_TRUE(decodedSphereRep->isIgnoring("Element/PlaneShape"));
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_CONTINUOUS,
+ sphereRep->getValue<CollisionDetectionType>("CollisionDetectionType"));
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_CONTINUOUS, sphereRep->getCollisionDetectionType());
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_DISCRETE,
+ sphereRep->getValue<CollisionDetectionType>("SelfCollisionDetectionType"));
+ EXPECT_EQ(COLLISION_DETECTION_TYPE_DISCRETE, sphereRep->getSelfCollisionDetectionType());
+}
}; // namespace Collision
}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/SegmentMeshTriangleMeshContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/SegmentMeshTriangleMeshContactCalculationTests.cpp
new file mode 100644
index 0000000..a27d330
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/SegmentMeshTriangleMeshContactCalculationTests.cpp
@@ -0,0 +1,905 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <sstream>
+
+#include "SurgSim/Collision/SegmentMeshTriangleMeshContact.h"
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/DataStructures/SegmentMesh.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::SegmentMeshPlain;
+using SurgSim::DataStructures::TriangleMeshPlain;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+class SegmentMeshTriangleMeshContactCalculationTests : public ::testing::Test
+{
+protected:
+ RigidTransform3d buildRigidTransform(double angle, double axisX, double axisY, double axisZ,
+ double translationX, double translationY, double translationZ)
+ {
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::makeRotationQuaternion;
+ return makeRigidTransform(makeRotationQuaternion(angle, Vector3d(axisX, axisY, axisZ).normalized()),
+ Vector3d(translationX, translationY, translationZ));
+ }
+
+ void SetUp() override
+ {
+ m_cubeSize = 1.0;
+ m_segmentRadius = 0.1;
+
+ m_transforms.push_back(std::pair<RigidTransform3d, std::string>(
+ RigidTransform3d::Identity(), "Identity"));
+ m_transforms.push_back(std::pair<RigidTransform3d, std::string>(
+ buildRigidTransform(1.234, 17.04, 2.047, 3.052, 23.34, 42.45, 83.68), "Transform 1"));
+ m_transforms.push_back(std::pair<RigidTransform3d, std::string>(
+ buildRigidTransform(-5.34, 41.03, -2.52, -3.84, -3.45, 66.47, 29.34), "Transform 2"));
+ m_transforms.push_back(std::pair<RigidTransform3d, std::string>(
+ buildRigidTransform(0.246, -9.42, -4.86, 2.469, 37.68, -34.6, -17.1), "Transform 3"));
+ m_transforms.push_back(std::pair<RigidTransform3d, std::string>(
+ buildRigidTransform(-0.85, 3.344, 8.329, -97.4, 9.465, 0.275, -95.9), "Transform 4"));
+ }
+
+ /// \return The SegmentMeshShape with a line mesh.
+ std::shared_ptr<SegmentMeshShape> createSegmentMeshShape()
+ {
+ static const int segmentNumPoints = 3;
+ static const double segmentPoints[3][3] =
+ {
+ {0.0, -0.5, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.5, 0.0}
+ };
+
+ static const int segmentNumEdges = 2;
+ static const int segmentEdges[2][2] =
+ {
+ {0, 1}, {1, 2}
+ };
+
+ auto mesh = std::make_shared<SegmentMeshPlain>();
+ for (int i = 0; i < segmentNumPoints; ++i)
+ {
+ Vector3d p;
+ p[0] = segmentPoints[i][0];
+ p[1] = segmentPoints[i][1];
+ p[2] = segmentPoints[i][2];
+ SegmentMeshPlain::VertexType v(p);
+ mesh->addVertex(v);
+ }
+ for (int i = 0; i < segmentNumEdges; ++i)
+ {
+ std::array<size_t, 2> edgePoints;
+ for (int j = 0; j < 2; j++)
+ {
+ edgePoints[j] = segmentEdges[i][j];
+ }
+ SegmentMeshPlain::EdgeType e(edgePoints);
+ mesh->addEdge(e);
+ }
+
+ return std::make_shared<SegmentMeshShape>(*mesh, m_segmentRadius);
+ }
+
+ /// \return The MeshShape with a cube mesh.
+ std::shared_ptr<MeshShape> createCubeMeshShape()
+ {
+ // Create a Mesh Cube
+ static const int cubeNumPoints = 8;
+ static const double cubePoints[8][3] =
+ {
+ {-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}
+ };
+
+ 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]
+ };
+
+ auto 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);
+ }
+
+ return std::make_shared<MeshShape>(*mesh);
+ }
+
+ void testSegmentMeshTriangleMeshDCD(std::string scenario, std::shared_ptr<SegmentMeshShape> segmentMeshShape,
+ RigidTransform3d segmentMeshShapeTransform, std::shared_ptr<MeshShape> meshShape,
+ RigidTransform3d meshShapeTransform, int expectedNumContacts = 0)
+ {
+ for (const auto& transform : m_transforms)
+ {
+ SCOPED_TRACE("Scenario: " + scenario + ", for transform: " + transform.second);
+
+ std::shared_ptr<ShapeCollisionRepresentation> segmentMeshRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Mesh - Segment");
+ segmentMeshRep->setShape(segmentMeshShape);
+ segmentMeshRep->setLocalPose(transform.first * segmentMeshShapeTransform);
+
+ std::shared_ptr<ShapeCollisionRepresentation> triangleMeshRep =
+ std::make_shared<ShapeCollisionRepresentation>("Collision Mesh - Triangle");
+ triangleMeshRep->setShape(meshShape);
+ triangleMeshRep->setLocalPose(transform.first * meshShapeTransform);
+
+ // Perform collision detection.
+ SegmentMeshTriangleMeshContact calcContact;
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(segmentMeshRep, triangleMeshRep);
+ calcContact.calculateContact(pair);
+
+ // Verify for correct contact generation.
+ auto contacts = pair->getContacts();
+
+ // Check if the number of generated contacts was the expected number.
+ EXPECT_EQ(expectedNumContacts, contacts.size());
+
+ if (contacts.size() > 0)
+ {
+ // Find correction that need to be applied to remove all intersections.
+ Vector3d correction = Vector3d::Zero();
+ for (auto &contact : contacts)
+ {
+ Vector3d intersection = contact->normal * contact->depth;
+ // Project intersection on correction and subtract that component from intersection.
+ if (!correction.isZero())
+ {
+ Vector3d correctionNormalized = correction.normalized();
+ intersection -= intersection.dot(correctionNormalized) * correctionNormalized;
+ }
+ // Add the unique component of intersection to overall correction.
+ correction += intersection;
+ }
+
+ // Change this correction to a RigidTransform3d
+ RigidTransform3d correctionTransform = Math::makeRigidTranslation(correction);
+
+ // Applying this correction to the two shapes should remove all intersections.
+ segmentMeshRep->setLocalPose(correctionTransform * transform.first * segmentMeshShapeTransform);
+
+ // Perform collision detection.
+ std::shared_ptr<CollisionPair> pair2 = std::make_shared<CollisionPair>(segmentMeshRep, triangleMeshRep);
+ calcContact.calculateContact(pair2);
+
+ // There should be no intersections.
+ EXPECT_EQ(0, pair2->getContacts().size());
+ }
+ }
+ }
+
+ // Cube size
+ double m_cubeSize;
+
+ // Segment radius
+ double m_segmentRadius;
+
+private:
+ // List of random transformations and a string to identify it.
+ std::vector<std::pair<RigidTransform3d, std::string>> m_transforms;
+};
+
+
+TEST_F(SegmentMeshTriangleMeshContactCalculationTests, NonintersectionTest)
+{
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::makeRigidTranslation;
+ using SurgSim::Math::makeRotationQuaternion;
+
+ double epsilonTrans = 0.001;
+
+ auto segmentMeshShape = createSegmentMeshShape();
+ auto meshShape = createCubeMeshShape();
+
+ RigidTransform3d segmentMeshShapeTransform;
+ RigidTransform3d meshShapeTransform;
+
+ // No rotations
+ {
+ segmentMeshShapeTransform =
+ makeRigidTranslation(Vector3d(0.0, m_cubeSize + m_segmentRadius + epsilonTrans, 0.0));
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (segment mesh above box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ segmentMeshShapeTransform =
+ makeRigidTranslation(Vector3d(0.0, -m_cubeSize - m_segmentRadius - epsilonTrans, 0.0));
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (segment mesh below box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ segmentMeshShapeTransform =
+ makeRigidTranslation(Vector3d(-m_cubeSize * 0.5 - m_segmentRadius - epsilonTrans, 0.0, 0.0));
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (segment mesh left of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ segmentMeshShapeTransform =
+ makeRigidTranslation(Vector3d(m_cubeSize * 0.5 + m_segmentRadius + epsilonTrans, 0.0, 0.0));
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (segment mesh right of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ segmentMeshShapeTransform =
+ makeRigidTranslation(Vector3d(0.0, 0.0, -m_cubeSize * 0.5 - m_segmentRadius - epsilonTrans));
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (segment mesh behind box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ segmentMeshShapeTransform =
+ makeRigidTranslation(Vector3d(0.0, 0.0, m_cubeSize * 0.5 + m_segmentRadius + epsilonTrans));
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (segment mesh in front of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ // Rotation about X
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0, 0.0, offset + m_segmentRadius + epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about x-axis) segment mesh above box)",
+ createSegmentMeshShape(), segmentMeshShapeTransform,
+ createCubeMeshShape(), meshShapeTransform);
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0, 0.0, -offset - m_segmentRadius - epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about x-axis) segment mesh below box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0, m_cubeSize * 0.5 + m_segmentRadius + epsilonTrans, 0.0, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about x-axis) segment mesh right of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0, -m_cubeSize * 0.5 - m_segmentRadius - epsilonTrans, 0.0, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about x-axis) segment mesh left of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0,0.0, 0.0, offset + m_segmentRadius + epsilonTrans);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about x-axis) segment mesh in front of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0,0.0, 0.0, -offset - m_segmentRadius - epsilonTrans);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about x-axis) segment mesh behind box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ // Rotation about Y
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 1.0, 0.0, 0.0, m_cubeSize + m_segmentRadius + epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about y-axis) segment mesh above box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 1.0, 0.0, 0.0, -m_cubeSize - m_segmentRadius - epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about y-axis) segment mesh below box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 1.0, 0.0, m_cubeSize * 0.5 + m_segmentRadius + epsilonTrans, 0.0, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about y-axis) segment mesh right of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 1.0, 0.0, -m_cubeSize * 0.5 - m_segmentRadius - epsilonTrans, 0.0, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about y-axis) segment mesh left of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 1.0, 0.0, 0.0, 0.0, m_cubeSize * 0.5 + m_segmentRadius + epsilonTrans);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about y-axis) segment mesh in front of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 1.0, 0.0, 0.0, 0.0, -m_cubeSize * 0.5 - m_segmentRadius - epsilonTrans);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about y-axis) segment mesh behind box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ // Rotation about Z
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, 0.0, offset + m_segmentRadius + epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about z-axis) segment mesh above box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, 0.0, -offset - m_segmentRadius - epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about z-axis) segment mesh below box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, offset + m_segmentRadius + epsilonTrans, 0.0, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about z-axis) segment mesh right of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, -offset - m_segmentRadius - epsilonTrans, 0.0, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about z-axis) segment mesh left of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, 0.0, 0.0, m_cubeSize * 0.5 + m_segmentRadius + epsilonTrans);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about z-axis) segment mesh in front of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, 0.0, 0.0, -m_cubeSize * 0.5 - m_segmentRadius - epsilonTrans);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("No intersection (rotated (about z-axis) segment mesh behind box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform);
+ }
+}
+
+TEST_F(SegmentMeshTriangleMeshContactCalculationTests, IntersectionTest)
+{
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::makeRigidTranslation;
+ using SurgSim::Math::makeRotationQuaternion;
+
+ double epsilonTrans = -0.001;
+
+ auto segmentMeshShape = createSegmentMeshShape();
+ auto meshShape = createCubeMeshShape();
+
+ RigidTransform3d segmentMeshShapeTransform;
+ RigidTransform3d meshShapeTransform;
+
+ // No rotations
+ {
+ segmentMeshShapeTransform =
+ makeRigidTranslation(Vector3d(0.0, m_cubeSize + m_segmentRadius + epsilonTrans, 0.0));
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (segment mesh above box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 2); // bottom segment intersects top two triangles
+ }
+
+ {
+ segmentMeshShapeTransform =
+ makeRigidTranslation(Vector3d(0.0, -m_cubeSize - m_segmentRadius - epsilonTrans, 0.0));
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (segment mesh below box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 2); // top segment intersects bottom two triangles
+ }
+
+ {
+ segmentMeshShapeTransform =
+ makeRigidTranslation(Vector3d(-m_cubeSize * 0.5 - m_segmentRadius - epsilonTrans, 0.0, 0.0));
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (segment mesh left of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 6); // both segments intersect left two, one top and one bottom triangles
+ }
+
+ {
+ segmentMeshShapeTransform =
+ makeRigidTranslation(Vector3d(m_cubeSize * 0.5 + m_segmentRadius + epsilonTrans, 0.0, 0.0));
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (segment mesh right of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 6); // both segments intersect right two, one top and one bottom triangles
+ }
+
+ {
+ segmentMeshShapeTransform =
+ makeRigidTranslation(Vector3d(0.0, 0.0, -m_cubeSize * 0.5 - m_segmentRadius - epsilonTrans));
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (segment mesh behind box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 6); // both segments intersect farthest two, one top and one bottom triangles
+ }
+
+ {
+ segmentMeshShapeTransform =
+ makeRigidTranslation(Vector3d(0.0, 0.0, m_cubeSize * 0.5 + m_segmentRadius + epsilonTrans));
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (segment mesh in front of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 6); // both segments intersect nearest two, one top and one bottom triangles
+ }
+
+ // Rotation about X
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0, 0.0, offset + m_segmentRadius + epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about x-axis) segment mesh above box)",
+ createSegmentMeshShape(), segmentMeshShapeTransform,
+ createCubeMeshShape(), meshShapeTransform,
+ 1); // bottom segment intersect one top triangle
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0, 0.0, -offset - m_segmentRadius - epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about x-axis) segment mesh below box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // top segment intersect one bottom triangle
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0, m_cubeSize * 0.5 + m_segmentRadius + epsilonTrans, 0.0, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about x-axis) segment mesh right of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 4); // both segments intersect two right side triangles
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0, -m_cubeSize * 0.5 - m_segmentRadius - epsilonTrans, 0.0, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about x-axis) segment mesh left of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 4); // both segments intersect two left side triangles
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0,0.0, 0.0, offset + m_segmentRadius + epsilonTrans);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about x-axis) segment mesh in front of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // top segment intersects one of the nearer triangle
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0,0.0, 0.0, -offset - m_segmentRadius - epsilonTrans);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about x-axis) segment mesh behind box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // bottom segment intersects one of the farther triangle
+ }
+
+ // Rotation about Y
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 1.0, 0.0, 0.0, m_cubeSize + m_segmentRadius + epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about y-axis) segment mesh above box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 2); // bottom segment intersects top two triangles
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 1.0, 0.0, 0.0, -m_cubeSize - m_segmentRadius - epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about y-axis) segment mesh below box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 2); // top segment intersects bottom two triangles
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 1.0, 0.0, m_cubeSize * 0.5 + m_segmentRadius + epsilonTrans, 0.0, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about y-axis) segment mesh right of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 6); // both segments intersect left two, one top and one bottom triangles
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 1.0, 0.0, -m_cubeSize * 0.5 - m_segmentRadius - epsilonTrans, 0.0, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about y-axis) segment mesh left of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 6); // both segments intersect right two, one top and one bottom triangles
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 1.0, 0.0, 0.0, 0.0, m_cubeSize * 0.5 + m_segmentRadius + epsilonTrans);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about y-axis) segment mesh in front of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 6); // both segments intersect nearest two, one top and one bottom triangles
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 1.0, 0.0, 0.0, 0.0, -m_cubeSize * 0.5 - m_segmentRadius - epsilonTrans);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about y-axis) segment mesh behind box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 6); // both segments intersect farthest two, one top and one bottom triangles
+ }
+
+ // Rotation about Z
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, 0.0, offset + m_segmentRadius + epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about z-axis) segment mesh above box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // bottom segment intersect one of the top triangles
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, 0.0, -offset - m_segmentRadius - epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about z-axis) segment mesh below box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // top segment intersects one of the bottom triangles
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, offset + m_segmentRadius + epsilonTrans, 0.0, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about z-axis) segment mesh right of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // bottom segment intersects one of the right side triangles
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, -offset - m_segmentRadius - epsilonTrans, 0.0, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about z-axis) segment mesh left of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // top segment intersects one of the left side triangles
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, 0.0, 0.0, m_cubeSize * 0.5 + m_segmentRadius + epsilonTrans);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about z-axis) segment mesh in front of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 4); // both segments intersect the two nearest triangles
+ }
+
+ {
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, 0.0, 0.0, -m_cubeSize * 0.5 - m_segmentRadius - epsilonTrans);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection (rotated (about z-axis) segment mesh behind box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 4); // both segments intersect the two farthest triangles
+ }
+}
+
+TEST_F(SegmentMeshTriangleMeshContactCalculationTests, IntersectionWithSegmentAxisTest)
+{
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::makeRigidTranslation;
+ using SurgSim::Math::makeRotationQuaternion;
+
+ double epsilonTrans = -0.001;
+
+ auto segmentMeshShape = createSegmentMeshShape();
+ auto meshShape = createCubeMeshShape();
+
+ RigidTransform3d segmentMeshShapeTransform;
+ RigidTransform3d meshShapeTransform;
+
+ // No rotations
+ {
+ double offset = m_segmentRadius + 0.001;
+ segmentMeshShapeTransform =
+ makeRigidTranslation(Vector3d(offset, m_cubeSize + epsilonTrans, -offset));
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection with axis (segment mesh above box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // bottom segment intersects one of the top two triangles
+ }
+
+ {
+ double offset = m_segmentRadius + 0.001;
+ segmentMeshShapeTransform =
+ makeRigidTranslation(Vector3d(offset, -m_cubeSize - epsilonTrans, -offset));
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection with axis (segment mesh below box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // top segment intersects one of the bottom two triangles
+ }
+
+ // Rotation about X
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0, 0.0, offset + epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection with axis (rotated (about x-axis) segment mesh above box)",
+ createSegmentMeshShape(), segmentMeshShapeTransform,
+ createCubeMeshShape(), meshShapeTransform,
+ 1); // bottom segment intersects one of the top triangles
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0, 0.0, -offset - epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection with axis (rotated (about x-axis) segment mesh below box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // top segment intersect one bottom triangle
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0,0.0, 0.0, offset + epsilonTrans);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection with axis (rotated (about x-axis) segment mesh in front of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // top segment intersects one of the nearer triangle
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 1.0, 0.0, 0.0,0.0, 0.0, -offset - epsilonTrans);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection with axis (rotated (about x-axis) segment mesh behind box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // bottom segment intersects one of the farther triangle
+ }
+
+ // Rotation about Y
+ {
+ double offset = m_segmentRadius + 0.001;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 1.0, 0.0, offset, m_cubeSize + epsilonTrans, -offset);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection with axis (rotated (about y-axis) segment mesh above box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // bottom segment intersects one of the top two triangles
+ }
+
+ {
+ double offset = m_segmentRadius + 0.001;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 1.0, 0.0, offset, -m_cubeSize - epsilonTrans, -offset);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection with axis (rotated (about y-axis) segment mesh below box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // top segment intersects one of the bottom two triangles
+ }
+
+ // Rotation about Z
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, 0.0, offset + epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection with axis (rotated (about z-axis) segment mesh above box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // bottom segment intersect one of the top triangles
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, 0.0, -offset - epsilonTrans, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection with axis (rotated (about z-axis) segment mesh below box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // top segment intersects one of the bottom triangles
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, offset + epsilonTrans, 0.0, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection with axis (rotated (about z-axis) segment mesh right of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // bottom segment intersects one of the right side triangles
+ }
+
+ {
+ double offset = (1.0 + std::sin(M_PI_4)) * m_cubeSize * 0.5;
+ segmentMeshShapeTransform =
+ buildRigidTransform(M_PI_4, 0.0, 0.0, 1.0, -offset - epsilonTrans, 0.0, 0.0);
+ meshShapeTransform.setIdentity();
+ testSegmentMeshTriangleMeshDCD("Intersection with axis (rotated (about z-axis) segment mesh left of box)",
+ segmentMeshShape, segmentMeshShapeTransform,
+ meshShape, meshShapeTransform,
+ 1); // top segment intersects one of the left side triangles
+ }
+}
+
+
+}; // namespace Collision
+}; // namespace Surgsim
diff --git a/SurgSim/Collision/UnitTests/SegmentSegmentCcdIntervalCheckTests.cpp b/SurgSim/Collision/UnitTests/SegmentSegmentCcdIntervalCheckTests.cpp
new file mode 100644
index 0000000..1c39809
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/SegmentSegmentCcdIntervalCheckTests.cpp
@@ -0,0 +1,558 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 SegmentSegmentCcdCheck functions.
+
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Collision/SegmentSegmentCcdIntervalCheck.h"
+#include "SurgSim/Math/LinearMotionArithmetic.h"
+
+namespace SurgSim
+{
+
+namespace Collision
+{
+
+namespace
+{
+double epsilon = 1.0e-10;
+}
+
+template <typename T>
+class SegmentSegmentCcdIntervalCheckTests : public ::testing::Test
+{
+};
+
+TEST(SegmentSegmentCcdIntervalCheckTests, Initialization)
+{
+ // Set up some arbitrary locations to check the class initialization.
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, 1.0, 0.0), Math::Vector3d(-0.5, 1.5, 1.5)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, 1.0, 1.5), Math::Vector3d(-0.5, 1.5, 3.0)};
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(2.0, 1.0, 4.0), Math::Vector3d(1.0, 3.5, 2.5)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(0.0, 1.0, 2.0), Math::Vector3d(-2.0, 1.5, 3.5)};
+
+ ASSERT_NO_THROW(SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd,
+ 1.0e-06, 2.0e-06, 3.0e-06, 4.0e-06));
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 1.0e-06, 2.0e-06, 3.0e-06, 4.0e-06);
+
+ Math::Vector3d p11 = status.p1T0();
+ Math::Vector3d p12 = status.p1T1();
+ Math::Vector3d p21 = status.p2T0();
+ Math::Vector3d p22 = status.p2T1();
+ Math::Vector3d q11 = status.q1T0();
+ Math::Vector3d q12 = status.q1T1();
+ Math::Vector3d q21 = status.q2T0();
+ Math::Vector3d q22 = status.q2T1();
+
+ EXPECT_TRUE(p11.isApprox(Math::Vector3d(-1.0, 1.0, 0.0), epsilon));
+ EXPECT_TRUE(p12.isApprox(Math::Vector3d(-1.0, 1.0, 1.5), epsilon));
+ EXPECT_TRUE(p21.isApprox(Math::Vector3d(-0.5, 1.5, 1.5), epsilon));
+ EXPECT_TRUE(p22.isApprox(Math::Vector3d(-0.5, 1.5, 3.0), epsilon));
+ EXPECT_TRUE(q11.isApprox(Math::Vector3d(2.0, 1.0, 4.0), epsilon));
+ EXPECT_TRUE(q12.isApprox(Math::Vector3d(0.0, 1.0, 2.0), epsilon));
+ EXPECT_TRUE(q21.isApprox(Math::Vector3d(1.0, 3.5, 2.5), epsilon));
+ EXPECT_TRUE(q22.isApprox(Math::Vector3d(-2.0, 1.5, 3.5), epsilon));
+
+ EXPECT_TRUE((Math::LinearMotionND<double, 3>(p11, p12)).isApprox(status.motionP1(), epsilon));
+ EXPECT_TRUE((Math::LinearMotionND<double, 3>(p21, p22)).isApprox(status.motionP2(), epsilon));
+ EXPECT_TRUE((Math::LinearMotionND<double, 3>(q11, q12)).isApprox(status.motionQ1(), epsilon));
+ EXPECT_TRUE((Math::LinearMotionND<double, 3>(q21, q22)).isApprox(status.motionQ2(), epsilon));
+
+ EXPECT_DOUBLE_EQ(1.0e-06, status.thicknessP());
+ EXPECT_DOUBLE_EQ(2.0e-06, status.thicknessQ());
+ EXPECT_DOUBLE_EQ(3.0e-06, status.timePrecisionEpsilon());
+ EXPECT_DOUBLE_EQ(4.0e-06, status.distanceEpsilon());
+ EXPECT_DOUBLE_EQ(0.0, status.tripleProductEpsilon());
+ EXPECT_DOUBLE_EQ(0.0, status.muNuEpsilon());
+
+
+ auto p1q1 = status.motionQ1() - status.motionP1();
+ auto p1p2 = status.motionP2() - status.motionP1();
+ auto q1q2 = status.motionQ2() - status.motionQ1();
+ EXPECT_TRUE(analyticTripleProduct(p1q1, p1p2, q1q2).isApprox(status.P1Q1_P1P2_Q1Q2().getPolynomial(), epsilon));
+ EXPECT_TRUE(analyticDotProduct(p1p2, p1q1).isApprox(status.P1P2_P1Q1().getPolynomial(), epsilon));
+ EXPECT_TRUE(analyticDotProduct(q1q2, p1q1).isApprox(status.Q1Q2_P1Q1().getPolynomial(), epsilon));
+ EXPECT_TRUE(analyticDotProduct(p1p2, q1q2).isApprox(status.P1P2_Q1Q2().getPolynomial(), epsilon));
+ EXPECT_TRUE(analyticMagnitudeSquared(p1p2).isApprox(status.P1P2_sq().getPolynomial(), epsilon));
+ EXPECT_TRUE(analyticMagnitudeSquared(q1q2).isApprox(status.Q1Q2_sq().getPolynomial(), epsilon));
+
+ Math::Interval<double> range(0.5, 0.75);
+ Math::Polynomial<double, 2> axisX;
+ Math::Polynomial<double, 2> axisY;
+ Math::Polynomial<double, 2> axisZ;
+
+ Math::analyticCrossProduct(p1p2, q1q2, &axisX, &axisY, &axisZ);
+ Math::PolynomialValues<double, 2> valueX(axisX);
+ Math::PolynomialValues<double, 2> valueY(axisY);
+ Math::PolynomialValues<double, 2> valueZ(axisZ);
+ Math::Interval<double> cross = valueX.valuesOverInterval(range).square() +
+ valueY.valuesOverInterval(range).square() +
+ valueZ.valuesOverInterval(range).square();
+ EXPECT_TRUE(cross.isApprox(status.crossValueOnInterval(range), epsilon));
+};
+
+TEST(SegmentSegmentCcdIntervalCheckTests, SetGetTests)
+{
+ // Set up some arbitrary locations to check the class initialization.
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, 1.0, 0.0), Math::Vector3d(-0.5, 1.5, 1.5)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, 1.0, 1.5), Math::Vector3d(-0.5, 1.5, 3.0)};
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(2.0, 1.0, 4.0), Math::Vector3d(1.0, 3.5, 2.5)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(0.0, 1.0, 2.0), Math::Vector3d(-2.0, 1.5, 3.5)};
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 1.0e-06, 2.0e-06, 3.0e-06, 4.0e-06);
+
+ status.setTimePrecisionEpsilon(5.0e-06);
+ status.setDistanceEpsilon(6.0e-06);
+ status.setTripleProductEpsilon(7.0e-06);
+ status.setMuNuEpsilon(8.0e-06);
+
+ EXPECT_DOUBLE_EQ(5.0e-06, status.timePrecisionEpsilon());
+ EXPECT_DOUBLE_EQ(6.0e-06, status.distanceEpsilon());
+ EXPECT_DOUBLE_EQ(7.0e-06, status.tripleProductEpsilon());
+ EXPECT_DOUBLE_EQ(8.0e-06, status.muNuEpsilon());
+};
+
+TEST(SegmentSegmentCcdIntervalCheckTests, CollisionChecksWithThickness)
+{
+ {
+ // Two crossed segments intersecting at X = Y = 0 when the distance between is <= 0.75. This
+ // should occur when t >= 0.75
+ SCOPED_TRACE("Testing simple crossed segments moving towards each other.");
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(1.0, -1.0, 1.0), Math::Vector3d(-1.0, 1.0, 1.0)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(1.0, -1.0, 0.0), Math::Vector3d(-1.0, 1.0, 0.0)};
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.0, 1.0)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.0, 0.7500000001)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.0, 0.7499999999)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.0, 0.5)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.5, 0.7499999999)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.5, 0.7500000001)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.7499999999, 1.0)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.7500000001, 1.0)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.9, 0.95)));
+ }
+ {
+ // Two crossed segments intersecting at X = Y = 0 when the distance between is <= 0.75. This
+ // should occur when t <= 0.25
+ SCOPED_TRACE("Testing simple crossed segments moving away from each other.");
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(1.0, -1.0, 0.0), Math::Vector3d(-1.0, 1.0, 0.0)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(1.0, -1.0, 1.0), Math::Vector3d(-1.0, 1.0, 1.0)};
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.0, 1.0)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.2499999999, 1.0)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.2499999999, 0.2500000001)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.2500000001, 1.0)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.5, 1.0)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.5, 0.7499999999)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around Q1. Segments never get close enough.");
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+ std::array<Math::Vector3d, 2> qStart =
+ {
+ Math::Vector3d(0.0, 0.7500000001, 1.0),
+ Math::Vector3d(0.0, 1.7500000001, 1.0)
+ };
+ std::array<Math::Vector3d, 2> qEnd =
+ {
+ Math::Vector3d(0.0, 0.7500000001, 0.0),
+ Math::Vector3d(0.0, 1.7500000001, 0.0)
+ };
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionEndpoints,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around Q1. Segments get close enough right around time = 1.0.");
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+ std::array<Math::Vector3d, 2> qStart =
+ {
+ Math::Vector3d(0.0, 0.7499999999, 1.0),
+ Math::Vector3d(0.0, 1.7499999999, 1.0)
+ };
+ std::array<Math::Vector3d, 2> qEnd =
+ {
+ Math::Vector3d(0.0, 0.7499999999, 0.0),
+ Math::Vector3d(0.0, 1.7499999999, 0.0)
+ };
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around Q2. Segments never get close enough.");
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+ std::array<Math::Vector3d, 2> qStart =
+ {
+ Math::Vector3d(0.0, -1.7500000001, 1.0),
+ Math::Vector3d(0.0, -0.7500000001, 1.0)
+ };
+ std::array<Math::Vector3d, 2> qEnd =
+ {
+ Math::Vector3d(0.0, -1.7500000001, 0.0),
+ Math::Vector3d(0.0, -0.7500000001, 0.0)
+ };
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionEndpoints,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around Q2. Segments get close enough right around time = 1.0.");
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+ std::array<Math::Vector3d, 2> qStart =
+ {
+ Math::Vector3d(0.0, -1.7499999999, 1.0),
+ Math::Vector3d(0.0, -0.7499999999, 1.0)
+ };
+ std::array<Math::Vector3d, 2> qEnd =
+ {
+ Math::Vector3d(0.0, -1.7499999999, 0.0),
+ Math::Vector3d(0.0, -0.7499999999, 0.0)
+ };
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around P1. Segments never get close enough.");
+ std::array<Math::Vector3d, 2> pStart =
+ {
+ Math::Vector3d(0.0, 0.7500000001, 1.0),
+ Math::Vector3d(0.0, 1.7500000001, 1.0)
+ };
+ std::array<Math::Vector3d, 2> pEnd =
+ {
+ Math::Vector3d(0.0, 0.7500000001, 0.0),
+ Math::Vector3d(0.0, 1.7500000001, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionEndpoints,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around P1. Segments get close enough right around time = 1.0.");
+ std::array<Math::Vector3d, 2> pStart =
+ {
+ Math::Vector3d(0.0, 0.7499999999, 1.0),
+ Math::Vector3d(0.0, 1.7499999999, 1.0)
+ };
+ std::array<Math::Vector3d, 2> pEnd =
+ {
+ Math::Vector3d(0.0, 0.7499999999, 0.0),
+ Math::Vector3d(0.0, 1.7499999999, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around P2. Segments never get close enough.");
+ std::array<Math::Vector3d, 2> pStart =
+ {
+ Math::Vector3d(0.0, -1.7500000001, 1.0),
+ Math::Vector3d(0.0, -0.7500000001, 1.0)
+ };
+ std::array<Math::Vector3d, 2> pEnd =
+ {
+ Math::Vector3d(0.0, -1.7500000001, 0.0),
+ Math::Vector3d(0.0, -0.7500000001, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionEndpoints,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around P2. Segments get close enough right around time = 1.0.");
+ std::array<Math::Vector3d, 2> pStart =
+ {
+ Math::Vector3d(0.0, -1.7499999999, 1.0),
+ Math::Vector3d(0.0, -0.7499999999, 1.0)
+ };
+ std::array<Math::Vector3d, 2> pEnd =
+ {
+ Math::Vector3d(0.0, -1.7499999999, 0.0),
+ Math::Vector3d(0.0, -0.7499999999, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestWithThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+};
+
+TEST(SegmentSegmentCcdIntervalCheckTests, CollisionChecksWithoutThickness)
+{
+ {
+ // Two crossed segments intersecting at X = Y = 0 when the distance between is = 0.0. This
+ // should occur when t >= 0.5
+ SCOPED_TRACE("Testing simple crossed segments moving towards each other.");
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, -1.0, 2.0), Math::Vector3d(1.0, 1.0, 2.0)};
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(1.0, -1.0, 1.0), Math::Vector3d(-1.0, 1.0, 1.0)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(1.0, -1.0, -1.0), Math::Vector3d(-1.0, 1.0, -1.0)};
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.0, 1.0)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.0, 0.5000000001)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.0, 0.4999999999)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.0, 0.25)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.25, 0.4999999999)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.4999999999, 0.5000000001)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.4999999999, 1.0)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.5000000001, 1.0)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.9, 0.95)));
+ }
+ {
+ // Two crossed segments intersecting at X = Y = 0 when the distance between is = 0.0. This
+ // should occur at t = 0.0
+ SCOPED_TRACE("Testing simple crossed segments moving away from each other.");
+ std::array<Math::Vector3d, 2> pStart =
+ {
+ Math::Vector3d(-1.0, -1.0, 0.0000000001), Math::Vector3d(1.0, 1.0, 0.0000000001)
+ };
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> qStart =
+ {
+ Math::Vector3d(1.0, -1.0, -0.0000000001), Math::Vector3d(-1.0, 1.0, -0.0000000001)
+ };
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(1.0, -1.0, 1.0), Math::Vector3d(-1.0, 1.0, 1.0)};
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.0, 1.0)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.25, 1.0)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.0, 0.1)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.000001, 1.0)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.5, 1.0)));
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionVolume,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.5, 0.7499999999)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around Q1. Segments never get close enough.");
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+ std::array<Math::Vector3d, 2> qStart =
+ {
+ Math::Vector3d(0.0, 0.0000000001, 1.0),
+ Math::Vector3d(0.0, 1.0000000001, 1.0)
+ };
+ std::array<Math::Vector3d, 2> qEnd =
+ {
+ Math::Vector3d(0.0, 0.0000000001, 0.0),
+ Math::Vector3d(0.0, 1.0000000001, 0.0)
+ };
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionEndpoints,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around Q1. Segments get close enough right around time = 1.0.");
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+ std::array<Math::Vector3d, 2> qStart =
+ {
+ Math::Vector3d(0.0, -0.0000000001, 1.0),
+ Math::Vector3d(0.0, 1.0000000001, 1.0)
+ };
+ std::array<Math::Vector3d, 2> qEnd =
+ {
+ Math::Vector3d(0.0, -0.0000000001, 0.0),
+ Math::Vector3d(0.0, 1.0000000001, 0.0)
+ };
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around Q2. Segments never get close enough.");
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+ std::array<Math::Vector3d, 2> qStart =
+ {
+ Math::Vector3d(0.0, -1.0000000001, 1.0),
+ Math::Vector3d(0.0, -0.0000000001, 1.0)
+ };
+ std::array<Math::Vector3d, 2> qEnd =
+ {
+ Math::Vector3d(0.0, -1.0000000001, 0.0),
+ Math::Vector3d(0.0, -0.0000000001, 0.0)
+ };
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionEndpoints,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around Q2. Segments get close enough right around time = 1.0.");
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+ std::array<Math::Vector3d, 2> qStart =
+ {
+ Math::Vector3d(0.0, -1.0000000001, 1.0),
+ Math::Vector3d(0.0, 0.0000000001, 1.0)
+ };
+ std::array<Math::Vector3d, 2> qEnd =
+ {
+ Math::Vector3d(0.0, -1.0000000001, 0.0),
+ Math::Vector3d(0.0, 0.0000000001, 0.0)
+ };
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around P1. Segments never get close enough.");
+ std::array<Math::Vector3d, 2> pStart =
+ {
+ Math::Vector3d(0.0, 0.0000000001, 1.0),
+ Math::Vector3d(0.0, 1.0000000001, 1.0)
+ };
+ std::array<Math::Vector3d, 2> pEnd =
+ {
+ Math::Vector3d(0.0, 0.0000000001, 0.0),
+ Math::Vector3d(0.0, 1.0000000001, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionEndpoints,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around P1. Segments get close enough right around time = 1.0.");
+ std::array<Math::Vector3d, 2> pStart =
+ {
+ Math::Vector3d(0.0, -0.0000000001, 1.0),
+ Math::Vector3d(0.0, 1.0000000001, 1.0)
+ };
+ std::array<Math::Vector3d, 2> pEnd =
+ {
+ Math::Vector3d(0.0, -0.0000000001, 0.0),
+ Math::Vector3d(0.0, 1.0000000001, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around P2. Segments never get close enough.");
+ std::array<Math::Vector3d, 2> pStart =
+ {
+ Math::Vector3d(0.0, -1.0000000001, 1.0),
+ Math::Vector3d(0.0, -0.0000000001, 1.0)
+ };
+ std::array<Math::Vector3d, 2> pEnd =
+ {
+ Math::Vector3d(0.0, -1.0000000001, 0.0),
+ Math::Vector3d(0.0, -0.0000000001, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckNoCollisionEndpoints,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+ {
+ SCOPED_TRACE("Slanted T formation around P2. Segments get close enough right around time = 1.0.");
+ std::array<Math::Vector3d, 2> pStart =
+ {
+ Math::Vector3d(0.0, -1.0000000001, 1.0),
+ Math::Vector3d(0.0, 0.0000000001, 1.0)
+ };
+ std::array<Math::Vector3d, 2> pEnd =
+ {
+ Math::Vector3d(0.0, -1.0000000001, 0.0),
+ Math::Vector3d(0.0, 0.0000000001, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_EQ(SegmentSegmentCcdIntervalCheck::IntervalCheckPossibleCollision,
+ status.possibleCollisionTestNoThickness(Math::Interval<double> (0.0, 1.0)));
+ }
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/SegmentSegmentCcdMovingContactTests.cpp b/SurgSim/Collision/UnitTests/SegmentSegmentCcdMovingContactTests.cpp
new file mode 100644
index 0000000..0f0af9b
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/SegmentSegmentCcdMovingContactTests.cpp
@@ -0,0 +1,581 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 SegmentSegmentCcdMovingContact functions.
+
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Collision/SegmentSegmentCcdMovingContact.h"
+
+namespace SurgSim
+{
+
+namespace Collision
+{
+
+namespace
+{
+double epsilon = 1.0e-05;
+}
+
+template <typename T>
+class SegmentSegmentCcdMovingContactTests : public ::testing::Test
+{
+};
+
+class MockSegmentSegmentCcdMovingContact : public SurgSim::Collision::SegmentSegmentCcdMovingContact
+{
+public:
+ void normalizeSegmentsConsistently(Math::Vector3d* t0, Math::Vector3d* t1, double epsilon) const
+ {
+ SegmentSegmentCcdMovingContact::normalizeSegmentsConsistently(t0, t1, epsilon);
+ }
+
+ bool collideSegmentSegmentBaseCase(
+ const std::array<Math::Vector3d, 2>& pT0,
+ const std::array<Math::Vector3d, 2>& pT1,
+ const std::array<Math::Vector3d, 2>& qT0,
+ const std::array<Math::Vector3d, 2>& qT1,
+ double thicknessP,
+ double thicknessQ,
+ double timePrecisionEpsilon,
+ double* t, double* r, double* s)
+ {
+ return SegmentSegmentCcdMovingContact::collideSegmentSegmentBaseCase(pT0, pT1, qT0, qT1,
+ thicknessP, thicknessQ, timePrecisionEpsilon,
+ t, r, s);
+ }
+
+ bool collideSegmentSegmentGeneralCase(
+ const SegmentSegmentCcdIntervalCheck& state,
+ double a, double b, // Interval boundaries
+ double* t, double* r, double* s,
+ int depth = 0)
+ {
+ return SegmentSegmentCcdMovingContact::collideSegmentSegmentGeneralCase(state, a, b, t, r, s, depth);
+ }
+
+ bool collideSegmentSegmentCoplanarCase(
+ const std::array<Math::Vector3d, 2>& pT0, /* Segment 1 at t=0 */
+ const std::array<Math::Vector3d, 2>& pT1, /* Segment 1 at t=1 */
+ const std::array<Math::Vector3d, 2>& qT0, /* Segment 2 at t=0 */
+ const std::array<Math::Vector3d, 2>& qT1, /* Segment 2 at t=1 */
+ double a, double b, /* Interval boundaries */
+ double timePrecisionEpsilon,
+ double thicknessP, double thicknessQ,
+ double* t, double* r, double* s,
+ int depth = 0)
+ {
+ m_staticTest.collideStaticSegmentSegment(pT0, qT0, thicknessP, thicknessQ, r, s);
+ return SegmentSegmentCcdMovingContact::collideSegmentSegmentCoplanarCase(
+ pT0, pT1, qT0, qT1, a, b, timePrecisionEpsilon, thicknessP, thicknessQ, t, r, s, depth);
+ }
+
+ bool collideSegmentSegmentParallelCase(
+ const std::array<Math::Vector3d, 2>& pT0,
+ const std::array<Math::Vector3d, 2>& pT1,
+ const std::array<Math::Vector3d, 2>& qT0,
+ const std::array<Math::Vector3d, 2>& qT1,
+ double a, double b,
+ double thicknessP, double thicknessQ,
+ double timePrecisionEpsilon,
+ double* t, double* r, double* s, int depth = 0)
+ {
+ m_staticTest.collideStaticSegmentSegment(pT0, qT0, thicknessP, thicknessQ, r, s);
+ return SegmentSegmentCcdMovingContact::collideSegmentSegmentParallelCase(
+ pT0, pT1, qT0, qT1, a, b, thicknessP, thicknessQ, timePrecisionEpsilon, t, r, s, depth);
+
+ }
+
+ Collision::SegmentSegmentCcdStaticContact m_staticTest;
+};
+
+TEST(SegmentSegmentCcdMovingContactTests, Initialization)
+{
+ ASSERT_NO_THROW(SegmentSegmentCcdMovingContact movingTest);
+ SegmentSegmentCcdMovingContact movingTest;
+};
+
+TEST(SegmentSegmentCcdMovingContactTests, TestSafeNormalization)
+{
+ MockSegmentSegmentCcdMovingContact movingTest;
+
+ {
+ SCOPED_TRACE("Normalizing 2 good vectors.");
+ Math::Vector3d vector1(0.0, 0.1, 0.2);
+ Math::Vector3d vector2(0.2, 0.1, 0.0);
+ movingTest.normalizeSegmentsConsistently(&vector1, &vector2, epsilon);
+ EXPECT_TRUE(vector1.isApprox(Math::Vector3d(0.0, 1.0 / std::sqrt(5.0), 2 / std::sqrt(5.0)), epsilon));
+ EXPECT_TRUE(vector2.isApprox(Math::Vector3d(2 / std::sqrt(5.0), 1.0 / std::sqrt(5.0), 0.0), epsilon));
+ }
+ {
+ SCOPED_TRACE("Vector 1 is good. Vector 2 is bad.");
+ Math::Vector3d vector1(0.0, 0.1, 0.2);
+ Math::Vector3d vector2(2.0e-11, 1.0e-11, 0.0);
+ movingTest.normalizeSegmentsConsistently(&vector1, &vector2, epsilon);
+ EXPECT_TRUE(vector1.isApprox(Math::Vector3d(0.0, 1.0 / std::sqrt(5.0), 2 / std::sqrt(5.0)), epsilon));
+ EXPECT_TRUE(vector2.isApprox(Math::Vector3d(0.0, 1.0 / std::sqrt(5.0), 2 / std::sqrt(5.0)), epsilon));
+ }
+ {
+ SCOPED_TRACE("Vector 1 is bad. Vector 2 is good.");
+ Math::Vector3d vector1(0.0, 1.0e-11, 2.0e-11);
+ Math::Vector3d vector2(0.2, 0.1, 0.0);
+ movingTest.normalizeSegmentsConsistently(&vector1, &vector2, epsilon);
+ EXPECT_TRUE(vector1.isApprox(Math::Vector3d(2 / std::sqrt(5.0), 1.0 / std::sqrt(5.0), 0.0), epsilon));
+ EXPECT_TRUE(vector2.isApprox(Math::Vector3d(2 / std::sqrt(5.0), 1.0 / std::sqrt(5.0), 0.0), epsilon));
+ }
+ {
+ SCOPED_TRACE("Normalizing 2 bad vectors.");
+ Math::Vector3d vector1(0.0, 1.0e-11, 2.0e-11);
+ Math::Vector3d vector2(2.0e-11, 1.0e-11, 0.0);
+ movingTest.normalizeSegmentsConsistently(&vector1, &vector2, epsilon);
+ EXPECT_TRUE(vector1.isApprox(Math::Vector3d(0.0, 1.0e-11, 2.0e-11), epsilon));
+ EXPECT_TRUE(vector2.isApprox(Math::Vector3d(2.0e-11, 1.0e-11, 0.0), epsilon));
+ }
+};
+
+TEST(SegmentSegmentCcdMovingContactTests, TestSegmentSegmentGeneralCase)
+{
+ MockSegmentSegmentCcdMovingContact movingTest;
+
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(1.0, -1.0, 1.0), Math::Vector3d(-1.0, 1.0, 1.0)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(1.0, -1.0, 0.0), Math::Vector3d(-1.0, 1.0, 0.0)};
+
+ double t;
+ double r;
+ double s;
+
+ {
+ SCOPED_TRACE("Collision not possible within time interval.");
+ SegmentSegmentCcdIntervalCheck status(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_FALSE(movingTest.collideSegmentSegmentGeneralCase(status, 0.0, 0.5, &t, &r, &s, 0));
+ }
+
+ {
+ // Test where collision occurs at the end of the (small) time interval
+ SCOPED_TRACE("Recursion bottomed out, test collision and no collision at time precision.");
+
+ SegmentSegmentCcdIntervalCheck statusTrue(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_TRUE(movingTest.collideSegmentSegmentGeneralCase(statusTrue, 0.75 - 1.4e-07, 0.75 + 1.5e-07,
+ &t, &r, &s, 0));
+ EXPECT_DOUBLE_EQ(0.75 + 1.5e-07, t);
+ EXPECT_DOUBLE_EQ(0.50, r);
+ EXPECT_DOUBLE_EQ(0.50, s);
+
+ // Increase the interval and time precision so that no collision occurs
+ SegmentSegmentCcdIntervalCheck statusFalse(pStart, pEnd, qStart, qEnd, 1.0e-06, 1.0e-06, 0.5, 4.0e-06);
+ EXPECT_FALSE(movingTest.collideSegmentSegmentGeneralCase(statusFalse, 0.76 - 2.5, 0.75 + 2.4999,
+ &t, &r, &s, 0));
+ }
+
+ {
+ // Test complete recursion
+ SCOPED_TRACE("Full recursion test, test collision and no collision at time precision.");
+
+ SegmentSegmentCcdIntervalCheck statusTrue(pStart, pEnd, qStart, qEnd, 0.5, 0.25, 3.0e-06, 4.0e-06);
+ EXPECT_TRUE(movingTest.collideSegmentSegmentGeneralCase(statusTrue, 0.0, 1.0, &t, &r, &s, 0));
+ EXPECT_GT(epsilon, std::abs(0.75 - t));
+ EXPECT_DOUBLE_EQ(0.50, r);
+ EXPECT_DOUBLE_EQ(0.50, s);
+
+ // Increase the interval and time precision so that no collision occurs
+ SegmentSegmentCcdIntervalCheck statusFalse(pStart, pEnd, qStart, qEnd, 1.0e-06, 1.0e-06, 0.5, 4.0e-06);
+ EXPECT_FALSE(movingTest.collideSegmentSegmentGeneralCase(statusFalse, 0.0, 0.75 + 1.0, &t, &r, &s, 0));
+ }
+};
+
+TEST(SegmentSegmentCcdMovingContactTests, TestSegmentSegmentCoplanarCase)
+{
+ MockSegmentSegmentCcdMovingContact movingTest;
+
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, 1.75, 0.0), Math::Vector3d(1.0, 1.75, 0.0)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(1.0, -0.25, 0.0), Math::Vector3d(3.0, -0.25, 0.0)};
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(-1.0, 0.0, 0.0), Math::Vector3d(-1.0, -1.0, 0.0)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(1.0, 0.0, 0.0), Math::Vector3d(1.0, -1.0, 0.0)};
+
+ double t;
+ double r;
+ double s;
+
+ {
+ // Test where collision occurs at the end of the (small) time interval
+ SCOPED_TRACE("Recursion bottomed out, test collision and no collision at time precision.");
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentCoplanarCase(
+ pStart, pEnd,
+ qStart, qEnd,
+ 0.5 - 1.4e-07, 0.5 + 1.5e-07, 3.0e-06,
+ 0.5, 0.25, &t, &r, &s, 0));
+ EXPECT_GE(3.0e-06, std::abs(0.5 - t));
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+
+ // Increase the interval and time precision so that no collision occurs
+ EXPECT_TRUE(movingTest.collideSegmentSegmentCoplanarCase(
+ pEnd, pStart,
+ qStart, qEnd,
+ 0.5 - .24, 0.5 + .25, .50,
+ 0.5, 0.25, &t, &r, &s, 0));
+ EXPECT_GE(1.0e-02, std::abs(0.5 - t));
+ }
+ {
+ // Test complete recursion
+ SCOPED_TRACE("Full recursion test, test start, middle, and end of interval.");
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentCoplanarCase(
+ pStart, pEnd,
+ qStart, qEnd,
+ 0.5 - 1.5e-06, 1.0, 3.0e-06,
+ 0.5, 0.25, &t, &r, &s, 0));
+ EXPECT_GE(3.0e-06, std::abs(0.5 - t));
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentCoplanarCase(
+ pStart, pEnd,
+ qStart, qEnd,
+ 0.0, 1.0, 3.0e-06,
+ 0.5, 0.25, &t, &r, &s, 0));
+ EXPECT_GE(3.0e-06, std::abs(0.5 - t));
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentCoplanarCase(
+ pStart, pEnd,
+ qStart, qEnd,
+ 0.0, 0.5 + 1.5e-06, 3.0e-06,
+ 0.5, 0.25, &t, &r, &s, 0));
+ EXPECT_GE(3.0e-06, std::abs(0.5 - t));
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+ }
+ {
+ SCOPED_TRACE("Test normal flip at start, middle, and end of interval.");
+
+ std::array<Math::Vector3d, 2> pFlipStart = {Math::Vector3d(0.0625, 1.0, 0.0),
+ Math::Vector3d(-3.5 + 0.0625, -1.0e-03, 0.0)
+ };
+ std::array<Math::Vector3d, 2> pFlipEnd = {Math::Vector3d(0.0625, 1.0, 0.0),
+ Math::Vector3d(3.5, -1.0e-03, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qFlip = {Math::Vector3d(0.0625, -1.0, 0.0),
+ Math::Vector3d(0.0625, 0.0, 0.0)
+ };
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentCoplanarCase(
+ pFlipStart, pFlipEnd,
+ qFlip, qFlip,
+ 0.503, 1.0, 1.0e-10,
+ 1.0e-09, 1.0e-09, &t, &r, &s, 0));
+ EXPECT_GE(2.0e-06, std::abs(0.504504 - t));
+ EXPECT_GT(2.0e-03, 1.0 - r);
+ EXPECT_GT(2.0e-03, 1.0 - s);
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentCoplanarCase(
+ pFlipStart, pFlipEnd,
+ qFlip, qFlip,
+ 0.0, 1.0, 1.0e-10,
+ 1.0e-09, 1.0e-09, &t, &r, &s, 0));
+ EXPECT_GE(2.0e-06, std::abs(0.504504 - t));
+ EXPECT_GT(2.0e-03, 1.0 - r);
+ EXPECT_GT(2.0e-03, 1.0 - s);
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentCoplanarCase(
+ pFlipStart, pFlipEnd,
+ qFlip, qFlip,
+ 0.0, 0.505, 1.0e-10,
+ 1.0e-09, 1.0e-09, &t, &r, &s, 0));
+ EXPECT_GE(2.0e-06, std::abs(0.504504 - t));
+ EXPECT_GT(2.0e-03, 1.0 - r);
+ EXPECT_GT(2.0e-03, 1.0 - s);
+ }
+ {
+ SCOPED_TRACE("Test the no flip combinations.");
+ std::array<Math::Vector3d, 2> pSpinStart = {Math::Vector3d(-0.5 + 1.0e-03, -1.0e-09, 0.0),
+ Math::Vector3d(0.0 + 1.0e-03, 0.5, 0.0)
+ };
+ std::array<Math::Vector3d, 2> pSpinEnd = {Math::Vector3d(0.5 + 1.0e-03, -1.0e-09, 0.0),
+ Math::Vector3d(1.0 + 1.0e-03, -0.5, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qSpin = {Math::Vector3d(0.0, -1.0, 0.0),
+ Math::Vector3d(0.0, 0.0, 0.0)
+ };
+
+ // We don't need to test the endpoints of the region. We verified that
+ // we hit those with the flip tests. Instead, test all possible combinations
+ // of crossing ...
+ EXPECT_TRUE(movingTest.collideSegmentSegmentCoplanarCase(
+ pSpinStart, pSpinEnd,
+ qSpin, qSpin,
+ 0.0, 1.0, 1.0e-10,
+ 1.0e-09, 1.0e-09, &t, &r, &s, 0));
+ EXPECT_GE(3.0e-06, std::abs(0.5 - t - 1.0e-03));
+ EXPECT_GT(1.0e-08, std::abs(3.0e-06 - r));
+ EXPECT_GT(1.0e-08, std::abs(1.0 - s));
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentCoplanarCase(
+ pSpinEnd, pSpinStart,
+ qSpin, qSpin,
+ 0.0, 1.0, 1.0e-10,
+ 1.0e-09, 1.0e-09, &t, &r, &s, 0));
+ EXPECT_GE(3.0e-06, std::abs(0.5 - t + 1.0e-03));
+ EXPECT_GT(1.0e-08, std::abs(r));
+ EXPECT_GT(1.0e-08, std::abs(1.0 - s));
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentCoplanarCase(
+ qSpin, qSpin,
+ pSpinStart, pSpinEnd,
+ 0.0, 1.0, 1.0e-10,
+ 1.0e-09, 1.0e-09, &t, &r, &s, 0));
+ EXPECT_GE(3.0e-06, std::abs(0.5 - t - 1.0e-03));
+ EXPECT_GT(1.0e-08, std::abs(1.0 - r));
+ EXPECT_GT(1.0e-08, std::abs(3.0e-06 - s));
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentCoplanarCase(
+ qSpin, qSpin,
+ pSpinEnd, pSpinStart,
+ 0.0, 1.0, 1.0e-10,
+ 1.0e-09, 1.0e-09, &t, &r, &s, 0));
+ EXPECT_GE(3.0e-06, std::abs(0.5 - t + 1.0e-03));
+ EXPECT_GT(1.0e-08, std::abs(1.0 - r));
+ EXPECT_GT(1.0e-08, s);
+ }
+};
+
+TEST(SegmentSegmentCcdMovingContactTests, TestSegmentSegmentParallelCase)
+{
+ MockSegmentSegmentCcdMovingContact movingTest;
+
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-5.0, -1.0e-09, 0.0),
+ Math::Vector3d(-5.0, 1.0, 0.0)
+ };
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(5.0, -1.0e-09, 0.0),
+ Math::Vector3d(5.0, 1.0, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(0.0, -1.0, 0.0),
+ Math::Vector3d(0.0, 0.0, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(0.0, -1.0, 0.0),
+ Math::Vector3d(0.0, 0.0, 0.0)
+ };
+
+ double t;
+ double r;
+ double s;
+
+ {
+ // Test where collision occurs in one of the intervals
+ SCOPED_TRACE("Test success and failure at end of recursion.");
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentParallelCase(
+ pStart, pEnd,
+ qStart, qEnd,
+ 0.425 - 1.4e-07, 0.425 + 1.4e-07,
+ 0.5, 0.25, 3.0e-06, &t, &r, &s, 0));
+ EXPECT_GE(3.0e-06, std::abs(0.425 - t));
+ EXPECT_GE(1.0e-09, std::abs(5.0e-10 - r));
+ EXPECT_GE(1.0e-09, std::abs(1.0 - 5.0e-10 - s));
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentParallelCase(
+ pStart, pEnd,
+ qStart, qEnd,
+ 0.4 - 1.4e-07, 0.4 + 1.4e-07,
+ 0.5, 0.25, 3.0e-06, &t, &r, &s, 0));
+ EXPECT_GE(5.0e-02, std::abs(0.425 - t));
+ }
+ {
+ // Test where collision occurs in one of the intervals
+ SCOPED_TRACE("Test collision at end of one of the intervals, first, middle, and last.");
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentParallelCase(
+ pStart, pEnd,
+ qStart, qEnd,
+ 0.4, 1.0,
+ 0.5, 0.25, 3.0e-06, &t, &r, &s, 0));
+ EXPECT_GE(3.0e-06, std::abs(0.425 - t));
+ EXPECT_GE(1.0e-09, std::abs(5.0e-10 - r));
+ EXPECT_GE(1.0e-09, std::abs(1.0 - 5.0e-10 - s));
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentParallelCase(
+ pStart, pEnd,
+ qStart, qEnd,
+ 0.1, 1.0,
+ 0.5, 0.25, 3.0e-06, &t, &r, &s, 0));
+ EXPECT_GE(3.0e-06, std::abs(0.425 - t));
+ EXPECT_GE(1.0e-09, std::abs(5.0e-10 - r));
+ EXPECT_GE(1.0e-09, std::abs(1.0 - 5.0e-10 - s));
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentParallelCase(
+ pStart, pEnd,
+ qStart, qEnd,
+ 0.1, 0.43,
+ 0.5, 0.25, 3.0e-06, &t, &r, &s, 0));
+ EXPECT_GE(3.0e-06, std::abs(0.425 - t));
+ EXPECT_GE(1.0e-09, std::abs(5.0e-10 - r));
+ EXPECT_GE(1.0e-09, std::abs(1.0 - 5.0e-10 - s));
+ }
+ {
+ // Test where collision occurs in one of the intervals
+ SCOPED_TRACE("Test within one of the intervals, first, middle, and last.");
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentParallelCase(
+ pStart, pEnd,
+ qStart, qEnd,
+ 0.42, 1.0,
+ 1.0e-03, 1.0e-03, 1.0e-06, &t, &r, &s, 0));
+ EXPECT_GE(2.0e-06, std::abs(0.5 - 2.0e-04 - t));
+ EXPECT_GE(1.0e-09, std::abs(5.0e-10 - r));
+ EXPECT_GE(1.0e-09, std::abs(1.0 - 5.0e-10 - s));
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentParallelCase(
+ pStart, pEnd,
+ qStart, qEnd,
+ 0.0, 1.0,
+ 1.0e-03, 1.0e-03, 1.0e-06, &t, &r, &s, 0));
+ EXPECT_GE(2.0e-06, std::abs(0.5 - 2.0e-04 - t));
+ EXPECT_GE(1.0e-09, std::abs(5.0e-10 - r));
+ EXPECT_GE(1.0e-09, std::abs(1.0 - 5.0e-10 - s));
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentParallelCase(
+ pStart, pEnd,
+ qStart, qEnd,
+ 0.0, 0.51,
+ 1.0e-03, 1.0e-03, 1.0e-06, &t, &r, &s, 0));
+ EXPECT_GE(2.0e-06, std::abs(0.5 - 2.0e-04 - t));
+ EXPECT_GE(1.0e-09, std::abs(5.0e-10 - r));
+ EXPECT_GE(1.0e-09, std::abs(1.0 - 5.0e-10 - s));
+ }
+};
+
+TEST(SegmentSegmentCcdMovingContactTests, TestRouting)
+{
+ // Test the base case for handling traffic to each of the 3 specific cases.
+ MockSegmentSegmentCcdMovingContact movingTest;
+ double r;
+ double s;
+ double t;
+
+ {
+ SCOPED_TRACE("Test the routing of the Parallel case.");
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-5.0, -1.0e-09, 0.0),
+ Math::Vector3d(-5.0, 1.0, 0.0)
+ };
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(5.0, -1.0e-09, 0.0),
+ Math::Vector3d(5.0, 1.0, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(0.0, -1.0, 0.0),
+ Math::Vector3d(0.0, 0.0, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(0.0, -1.0, 0.0),
+ Math::Vector3d(0.0, 0.0, 0.0)
+ };
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentBaseCase(
+ pStart, pEnd,
+ qStart, qEnd,
+ 0.5, 0.25, 3.0e-06, &t, &r, &s));
+ EXPECT_GE(3.0e-06, std::abs(0.425 - t));
+ EXPECT_GE(1.0e-09, std::abs(5.0e-10 - r));
+ EXPECT_GE(1.0e-09, std::abs(1.0 - 5.0e-10 - s));
+ }
+ {
+ SCOPED_TRACE("Test the routing of the Coplanar case.");
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-0.5 + 1.0e-03, -1.0e-09, 0.0),
+ Math::Vector3d(0.0 + 1.0e-03, 0.5, 0.0)
+ };
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(0.5 + 1.0e-03, -1.0e-09, 0.0),
+ Math::Vector3d(1.0 + 1.0e-03, -0.5, 0.0)
+ };
+ std::array<Math::Vector3d, 2> q = {Math::Vector3d(0.0, -1.0, 0.0),
+ Math::Vector3d(0.0, 0.0, 0.0)
+ };
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentBaseCase(pStart, pEnd, q, q,
+ 1.0e-09, 1.0e-09, 1.0e-10, &t, &r, &s));
+ EXPECT_GE(3.0e-06, std::abs(0.5 - t - 1.0e-03));
+ EXPECT_GT(1.0e-08, std::abs(3.0e-06 - r));
+ EXPECT_GT(1.0e-08, std::abs(1.0 - s));
+ }
+ {
+ SCOPED_TRACE("Test the routing of the General case.");
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-1.0, -1.0, -2.0), Math::Vector3d(1.0, 1.0, -2.0)};
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(-1.0, -1.0, 0.0), Math::Vector3d(1.0, 1.0, 0.0)};
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(1.0, -1.0, 1.0), Math::Vector3d(-1.0, 1.0, 1.0)};
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(1.0, -1.0, 0.0), Math::Vector3d(-1.0, 1.0, 0.0)};
+
+ EXPECT_TRUE(movingTest.collideSegmentSegmentBaseCase(pStart, pEnd, qStart, qEnd,
+ 0.5, 0.25, 3.0e-06, &t, &r, &s));
+ EXPECT_GT(epsilon, std::abs(0.75 - t));
+ EXPECT_DOUBLE_EQ(0.50, r);
+ EXPECT_DOUBLE_EQ(0.50, s);
+ }
+};
+
+TEST(SegmentSegmentCcdMovingContactTests, PublicAPI)
+{
+ // Test the base case for handling traffic to each of the 3 specific cases.
+ MockSegmentSegmentCcdMovingContact movingTest;
+
+ std::array<Math::Vector3d, 2> pStart = {Math::Vector3d(-5.0, -1.0e-09, 0.0),
+ Math::Vector3d(-5.0, 1.0, 0.0)
+ };
+ std::array<Math::Vector3d, 2> pEnd = {Math::Vector3d(5.0, -1.0e-09, 0.0),
+ Math::Vector3d(5.0, 1.0, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qStart = {Math::Vector3d(0.0, -1.0, 0.0),
+ Math::Vector3d(0.0, 0.0, 0.0)
+ };
+ std::array<Math::Vector3d, 2> qEnd = {Math::Vector3d(0.0, -1.0, 0.0),
+ Math::Vector3d(0.0, 0.0, 0.0)
+ };
+ Math::Vector3d dir;
+
+ double r;
+ double s;
+ double t;
+
+ {
+ SCOPED_TRACE("Test thick segments.");
+
+ EXPECT_TRUE(movingTest.collideMovingSegmentSegment(
+ pStart, pEnd,
+ qStart, qEnd,
+ 0.5, 0.25, 3.0e-06, &t, &r, &s, &dir));
+ EXPECT_GE(3.0e-06, std::abs(0.425 - t));
+ EXPECT_GE(1.0e-09, std::abs(5.0e-10 - r));
+ EXPECT_GE(1.0e-09, std::abs(1.0 - 5.0e-10 - s));
+ EXPECT_TRUE(dir.normalized().isApprox(Math::Vector3d(1.0, 0.0, 0.0), 1.0e-09));
+ }
+ {
+ SCOPED_TRACE("Test zero thickness segments.");
+
+ EXPECT_TRUE(movingTest.collideMovingSegmentSegment(
+ pStart, pEnd,
+ qStart, qEnd,
+ 2.0e-03, 1.0e-06, &t, &r, &s, &dir));
+ EXPECT_GE(2.0e-06, std::abs(0.5 - 2.0e-04 - t));
+ EXPECT_GE(1.0e-09, std::abs(5.0e-10 - r));
+ EXPECT_GE(1.0e-09, std::abs(1.0 - 5.0e-10 - s));
+ EXPECT_TRUE(dir.normalized().isApprox(Math::Vector3d(1.0, 0.0, 0.0), 1.0e-09));
+ }
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/SegmentSegmentCcdStaticContactTests.cpp b/SurgSim/Collision/UnitTests/SegmentSegmentCcdStaticContactTests.cpp
new file mode 100644
index 0000000..167cbf8
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/SegmentSegmentCcdStaticContactTests.cpp
@@ -0,0 +1,884 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 SegmentSegmentCcdCheck functions.
+
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Collision/SegmentSegmentCcdStaticContact.h"
+
+namespace SurgSim
+{
+
+namespace Collision
+{
+
+namespace
+{
+double epsilon = 1.0e-05;
+}
+
+template <typename T>
+class SegmentSegmentCcdStaticContactTests : public ::testing::Test
+{
+};
+
+class MockSegmentSegmentCcdStaticContact : public SurgSim::Collision::SegmentSegmentCcdStaticContact
+{
+public:
+ bool collideStaticPointSegment(
+ const Math::Vector3d& point, const std::array<SurgSim::Math::Vector3d, 2>& p,
+ double thicknessPoint, double thicknessSegment,
+ double* r)
+ {
+ return SegmentSegmentCcdStaticContact::collideStaticPointSegment(point, p, thicknessPoint, thicknessSegment, r);
+ }
+
+ SegmentCcdEdgeType computeCollisionEdge(double a, double b, double d,
+ double r, double s, double ratio) const
+ {
+ return SegmentSegmentCcdStaticContact::computeCollisionEdge(a, b, d, r, s, ratio);
+ }
+
+ void computeCollisionParametrics(SegmentCcdEdgeType edge, double a, double b, double c, double d, double e,
+ double ratio, double* r, double* s) const
+ {
+ SegmentSegmentCcdStaticContact::computeCollisionParametrics(edge, a, b, c, d, e, ratio, r, s);
+ }
+
+ void computeParallelSegmentParametrics(double a, double b, double d, double* r, double* s)
+ {
+ SegmentSegmentCcdStaticContact::computeParallelSegmentParametrics(a, b, d, r, s);
+ }
+};
+
+TEST(SegmentSegmentCcdStaticContactTests, Initialization)
+{
+ ASSERT_NO_THROW(SegmentSegmentCcdStaticContact staticTest);
+ SegmentSegmentCcdStaticContact staticTest;
+};
+
+TEST(SegmentSegmentCcdStaticContactTests, RegionAndEdgeGenerator)
+{
+ MockSegmentSegmentCcdStaticContact staticTest;
+ const double ratio = 0.5;
+ double a = 1.0;
+ double b = 1.0;
+ double d = 3.0;
+
+ for (double r = -1.0; r <= 0.0; r += 0.1)
+ {
+ for (double s = -1.0; s < 0.0; s += 0.1)
+ {
+ // Region 6
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeR0,
+ staticTest.computeCollisionEdge(a, b, d, r, s, ratio));
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeS0,
+ staticTest.computeCollisionEdge(a, b, -d, r, s, ratio));
+ }
+
+ for (double s = 0.0; s <= ratio; s += 0.1)
+ {
+ // Region 5
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeR0,
+ staticTest.computeCollisionEdge(a, b, d, r, s, ratio));
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeR0,
+ staticTest.computeCollisionEdge(a, b, -d, r, s, ratio));
+ }
+
+ for (double s = ratio + 0.1; s <= 2 * ratio; s += 0.1)
+ {
+ // Region 4
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeR0,
+ staticTest.computeCollisionEdge(a, b, d, r, s, ratio));
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeS1,
+ staticTest.computeCollisionEdge(a, b, -d, r, s, ratio));
+ }
+ }
+
+ for (double r = 0.0; r <= ratio; r += 0.1)
+ {
+ for (double s = -1.0; s < 0.0; s += 0.1)
+ {
+ // Region 7
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeS0,
+ staticTest.computeCollisionEdge(a, b, d, r, s, ratio));
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeS0,
+ staticTest.computeCollisionEdge(a, b, -d, r, s, ratio));
+ }
+
+ for (double s = 0.0; s <= ratio; s += 0.1)
+ {
+ // Region 0
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeEdgeSkip,
+ staticTest.computeCollisionEdge(a, b, d, r, s, ratio));
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeEdgeSkip,
+ staticTest.computeCollisionEdge(a, b, -d, r, s, ratio));
+ }
+
+ for (double s = ratio + 0.1; s <= 2 * ratio; s += 0.1)
+ {
+ // Region 3
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeS1,
+ staticTest.computeCollisionEdge(a, b, d, r, s, ratio));
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeS1,
+ staticTest.computeCollisionEdge(a, b, -d, r, s, ratio));
+ }
+ }
+
+ for (double r = ratio + 0.1; r <= 2 * ratio; r += 0.1)
+ {
+ for (double s = -1.0; s < 0.0; s += 0.1)
+ {
+ // Region 8
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeS0,
+ staticTest.computeCollisionEdge(a, b, d, r, s, ratio));
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeR1,
+ staticTest.computeCollisionEdge(a, b, -d, r, s, ratio));
+ }
+
+ for (double s = 0.0; s <= ratio; s += 0.1)
+ {
+ // Region 1
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeR1,
+ staticTest.computeCollisionEdge(a, b, d, r, s, ratio));
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeR1,
+ staticTest.computeCollisionEdge(a, b, -d, r, s, ratio));
+ }
+
+ for (double s = ratio + 0.1; s <= 2 * ratio; s += 0.1)
+ {
+ // Region 2
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeS1,
+ staticTest.computeCollisionEdge(a, b, d, r, s, ratio));
+ EXPECT_EQ(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeR1,
+ staticTest.computeCollisionEdge(a, b, -d, r, s, ratio));
+ }
+ }
+};
+
+TEST(SegmentSegmentCcdStaticContactTests, CalculateParametrics)
+{
+ MockSegmentSegmentCcdStaticContact staticTest;
+
+ double a = 3.3;
+ double b = 3.2;
+ double c = 3.0;
+ double d = 3.2;
+ double e = -4.2;
+ double ratio = 5.0;
+ double r = 4.5;
+ double s = 4.0;
+ {
+ SCOPED_TRACE("Testing region 0. Nearest point is on both segments.");
+ staticTest.computeCollisionParametrics(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeEdgeSkip,
+ a, b, c, d, e, ratio, &r, &s);
+ EXPECT_DOUBLE_EQ(0.9, r);
+ EXPECT_DOUBLE_EQ(0.8, s);
+ }
+
+ {
+ SCOPED_TRACE("Testing when r = 0. s depends on e and c.");
+ e = 5.0;
+ c = 7.0;
+ staticTest.computeCollisionParametrics(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeR0,
+ a, b, c, d, e, ratio, &r, &s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+
+ e = -5.0;
+ c = 7.0;
+ staticTest.computeCollisionParametrics(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeR0,
+ a, b, c, d, e, ratio, &r, &s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_DOUBLE_EQ(5.0 / 7.0, s);
+
+ e = -5.0;
+ c = 4.0;
+ staticTest.computeCollisionParametrics(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeR0,
+ a, b, c, d, e, ratio, &r, &s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_DOUBLE_EQ(1.0, s);
+ }
+
+ {
+ SCOPED_TRACE("Testing when r = 1. s depends on e, b, and c.");
+ e = 5.0;
+ b = -4.0;
+ c = 4.0;
+ staticTest.computeCollisionParametrics(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeR1,
+ a, b, c, d, e, ratio, &r, &s);
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+
+ e = -5.0;
+ b = 4.0;
+ c = 4.0;
+ staticTest.computeCollisionParametrics(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeR1,
+ a, b, c, d, e, ratio, &r, &s);
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_DOUBLE_EQ(0.25, s);
+
+ e = -5.0;
+ b = -4.0;
+ c = 4.0;
+ staticTest.computeCollisionParametrics(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeR1,
+ a, b, c, d, e, ratio, &r, &s);
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_DOUBLE_EQ(1.0, s);
+ }
+
+ {
+ SCOPED_TRACE("Testing when s = 0. r depends on d and a.");
+ d = 7.0;
+ a = 8.0;
+ staticTest.computeCollisionParametrics(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeS0,
+ a, b, c, d, e, ratio, &r, &s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+
+ d = -7.0;
+ a = 8.0;
+ staticTest.computeCollisionParametrics(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeS0,
+ a, b, c, d, e, ratio, &r, &s);
+ EXPECT_DOUBLE_EQ(0.875, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+
+ d = -9.0;
+ a = 8.0;
+ staticTest.computeCollisionParametrics(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeS0,
+ a, b, c, d, e, ratio, &r, &s);
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+
+ SCOPED_TRACE("Testing when s = 1. r depends on d, b, and a.");
+ d = 6.0;
+ b = -1.0;
+ a = 8.0;
+ staticTest.computeCollisionParametrics(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeS1,
+ a, b, c, d, e, ratio, &r, &s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_DOUBLE_EQ(1.0, s);
+
+ d = -6.0;
+ b = -1.0;
+ a = 8.0;
+ staticTest.computeCollisionParametrics(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeS1,
+ a, b, c, d, e, ratio, &r, &s);
+ EXPECT_DOUBLE_EQ(0.875, r);
+ EXPECT_DOUBLE_EQ(1.0, s);
+
+ d = -6.0;
+ b = -6.0;
+ a = 8.0;
+ staticTest.computeCollisionParametrics(SegmentSegmentCcdStaticContact::SegmentCcdEdgeTypeS1,
+ a, b, c, d, e, ratio, &r, &s);
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_DOUBLE_EQ(1.0, s);
+ }
+
+};
+
+TEST(SegmentSegmentCcdStaticContactTests, CalculateParallelParametrics)
+{
+ MockSegmentSegmentCcdStaticContact staticTest;
+
+ double a;
+ double b;
+ double d;
+ double r = 4.5;
+ double s = 4.0;
+
+ {
+ SCOPED_TRACE("Segments in opposite directions. End point r = s = 0.");
+ a = 4.0;
+ b = 3.2;
+ d = 3.2;
+ staticTest.computeParallelSegmentParametrics(a, b, d, &r, &s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+ }
+
+ {
+ SCOPED_TRACE("Segments in opposite directions. End point s0 in r.");
+ a = 4.0;
+ b = 4.0;
+ d = -3.2;
+ staticTest.computeParallelSegmentParametrics(a, b, d, &r, &s);
+ EXPECT_DOUBLE_EQ(0.8, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+ }
+
+ {
+ SCOPED_TRACE("Segments in opposite directions. End point r = s = 1.");
+ a = 4.0;
+ b = 3.2;
+ d = -9.2;
+ staticTest.computeParallelSegmentParametrics(a, b, d, &r, &s);
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_DOUBLE_EQ(1.0, s);
+ }
+
+ {
+ SCOPED_TRACE("Segments in opposite directions. End point r = 1 is closest to s.");
+ a = 4.0;
+ b = 3.2;
+ d = -5.6;
+ staticTest.computeParallelSegmentParametrics(a, b, d, &r, &s);
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_DOUBLE_EQ(0.5, s);
+ }
+
+ {
+ SCOPED_TRACE("Segments in same directions, but don't overlap. End point r = 1, s = 0.");
+ a = 4.0;
+ b = -3.2;
+ d = -4.2;
+ staticTest.computeParallelSegmentParametrics(a, b, d, &r, &s);
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+ }
+
+ {
+ SCOPED_TRACE("Segments in same directions, r overlaps s0.");
+ a = 4.0;
+ b = -3.2;
+ d = -3.0;
+ staticTest.computeParallelSegmentParametrics(a, b, d, &r, &s);
+ EXPECT_DOUBLE_EQ(0.75, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+ }
+
+ {
+ SCOPED_TRACE("Segments in same directions, end points r0 and s1 closest.");
+ a = 4.0;
+ b = -3.2;
+ d = 6.0;
+ staticTest.computeParallelSegmentParametrics(a, b, d, &r, &s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_DOUBLE_EQ(1.0, s);
+ }
+
+ {
+ SCOPED_TRACE("Segments in same directions, and r0 overlaps s.");
+ a = 4.0;
+ b = -3.2;
+ d = 2.4;
+ staticTest.computeParallelSegmentParametrics(a, b, d, &r, &s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_DOUBLE_EQ(0.75, s);
+ }
+};
+
+TEST(SegmentSegmentCcdStaticContactTests, PointSegmentCollisions)
+{
+ MockSegmentSegmentCcdStaticContact staticTest;
+
+ Math::Vector3d point;
+ Math::Vector3d p0;
+ Math::Vector3d p1;
+ std::array<SurgSim::Math::Vector3d, 2> p;
+ double thicknessA = 0.25;
+ double thicknessP = 0.75;
+ double r;
+
+ {
+ SCOPED_TRACE("Segment degenerate, end 0 is close enough.");
+ point = Math::Vector3d(0.0, 0.0, 0.0);
+ p0 = Math::Vector3d(0.0, 0.0, 1.0 - 1.0e-09 / 4.0);
+ p1 = Math::Vector3d(0.0, 0.0, 1.0 + 1.0e-09 / 4.0);
+ p[0] = p0;
+ p[1] = p1;
+ EXPECT_TRUE(staticTest.collideStaticPointSegment(point, p, thicknessA, thicknessP, &r));
+ EXPECT_DOUBLE_EQ(0.0, r);
+ }
+
+ {
+ SCOPED_TRACE("Segment degenerate, end 1 is close enough.");
+ point = Math::Vector3d(0.0, 0.0, 0.0);
+ p0 = Math::Vector3d(0.0, 0.0, 1.0 - 1.0e-09 / 4.0);
+ p1 = Math::Vector3d(0.0, 0.0, 1.0 + 1.0e-09 / 4.0);
+ p[0] = p1;
+ p[1] = p0;
+ EXPECT_TRUE(staticTest.collideStaticPointSegment(point, p, thicknessA, thicknessP, &r));
+ EXPECT_DOUBLE_EQ(1.0, r);
+ }
+
+ {
+ SCOPED_TRACE("Segment degenerate, neither point close enough.");
+ point = Math::Vector3d(0.0, 0.0, 0.0);
+ p0 = Math::Vector3d(0.0, 0.0, 1.0 - 1.0e-09 / 4.0);
+ p1 = Math::Vector3d(1.0, 0.0, 1.0 + 1.0e-09 / 4.0);
+ p[0] = p1;
+ p[1] = p1;
+ EXPECT_FALSE(staticTest.collideStaticPointSegment(point, p, thicknessA, thicknessP, &r));
+ }
+
+ {
+ SCOPED_TRACE("Segment clamped to 0, close enough");
+ point = Math::Vector3d(0.0, 0.5, 0.0);
+ p0 = Math::Vector3d(0.5, 0.0, 0.25);
+ p1 = Math::Vector3d(1.5, 0.0, 0.25);
+ p[0] = p0;
+ p[1] = p1;
+ EXPECT_TRUE(staticTest.collideStaticPointSegment(point, p, thicknessA, thicknessP, &r));
+ EXPECT_DOUBLE_EQ(0.0, r);
+ }
+
+ {
+ SCOPED_TRACE("Segment clamped to 0, not close enough");
+ point = Math::Vector3d(0.0, 1.0, 0.0);
+ p0 = Math::Vector3d(0.5, 0.0, 0.25);
+ p1 = Math::Vector3d(1.5, 0.0, 0.25);
+ p[0] = p0;
+ p[1] = p1;
+ EXPECT_FALSE(staticTest.collideStaticPointSegment(point, p, thicknessA, thicknessP, &r));
+ }
+
+ {
+ SCOPED_TRACE("Segment clamped to 1, close enough");
+ point = Math::Vector3d(0.0, 0.5, 0.0);
+ p0 = Math::Vector3d(0.5, 0.0, 0.25);
+ p1 = Math::Vector3d(1.5, 0.0, 0.25);
+ p[0] = p1;
+ p[1] = p0;
+ EXPECT_TRUE(staticTest.collideStaticPointSegment(point, p, thicknessA, thicknessP, &r));
+ EXPECT_DOUBLE_EQ(1.0, r);
+ }
+
+ {
+ SCOPED_TRACE("Segment clamped to 1, not close enough");
+ point = Math::Vector3d(0.0, 1.0, 0.0);
+ p0 = Math::Vector3d(0.5, 0.0, 0.25);
+ p1 = Math::Vector3d(1.5, 0.0, 0.25);
+ p[0] = p1;
+ p[1] = p0;
+ EXPECT_FALSE(staticTest.collideStaticPointSegment(point, p, thicknessA, thicknessP, &r));
+ }
+
+ {
+ SCOPED_TRACE("Segment not clamped, close enough");
+ point = Math::Vector3d(0.0, 0.5, 0.0);
+ p0 = Math::Vector3d(-1.5, 0.0, 0.25);
+ p1 = Math::Vector3d(1.5, 0.0, 0.25);
+ p[0] = p0;
+ p[1] = p1;
+ EXPECT_TRUE(staticTest.collideStaticPointSegment(point, p, thicknessA, thicknessP, &r));
+ EXPECT_DOUBLE_EQ(0.5, r);
+ }
+
+ {
+ SCOPED_TRACE("Segment not clamped, not close enough");
+ point = Math::Vector3d(0.0, 1.0, 0.0);
+ p0 = Math::Vector3d(0.5, 0.0, 0.25);
+ p1 = Math::Vector3d(1.5, 0.0, 0.25);
+ p[0] = p0;
+ p[1] = p1;
+ EXPECT_FALSE(staticTest.collideStaticPointSegment(point, p, thicknessA, thicknessP, &r));
+ }
+};
+
+TEST(SegmentSegmentCcdStaticContactTests, SegmentSegmentCollisions)
+{
+ MockSegmentSegmentCcdStaticContact staticTest;
+
+ Math::Vector3d p0;
+ Math::Vector3d p1;
+ std::array<SurgSim::Math::Vector3d, 2> p;
+ Math::Vector3d q0;
+ Math::Vector3d q1;
+ std::array<SurgSim::Math::Vector3d, 2> q;
+ double radiusP = 0.25;
+ double radiusQ = 0.75;
+ double distanceEpsilon = radiusP + radiusQ;
+ double r;
+ double s;
+ Math::Vector3d increment(0.1, 0, 0);
+
+ {
+ SCOPED_TRACE("Test degenerate segment cases - 2 points");
+ p0 = Math::Vector3d(-1.0e-09 / 4.0, 0.0, 1.0);
+ p1 = Math::Vector3d(1.0e-09 / 4.0, 0.0, 1.0);
+ q0 = Math::Vector3d(-1.0 - 1.0e-09 / 4.0, 0.0, 0.5);
+ q1 = Math::Vector3d(-1.0 + 1.0e-09 / 4.0, 0.0, 0.5);
+
+ for (size_t ctr = 0; ctr < 2; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ q0 += increment;
+ q1 += increment;
+ }
+
+ for (size_t ctr = 0; ctr < 17; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_DOUBLE_EQ(0.0, s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ EXPECT_DOUBLE_EQ(0.0, s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ q0 += increment;
+ q1 += increment;
+ }
+
+ for (size_t ctr = 0; ctr < 2; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ q0 += increment;
+ q1 += increment;
+ }
+
+ }
+
+ {
+ SCOPED_TRACE("Test degenerate segment cases - one segment is point");
+ p0 = Math::Vector3d(0.0, 0.0, 1.0);
+ p1 = Math::Vector3d(1.0e-09 / 2.0, 0.0, 1.0);
+ q0 = Math::Vector3d(-1.0, 0.0, 0.5);
+ q1 = Math::Vector3d(-2.0, 0.0, 0.5);
+
+ for (size_t ctr = 0; ctr < 2; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ q0 += increment;
+ q1 += increment;
+ }
+
+ for (size_t ctr = 0; ctr < 8; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_DOUBLE_EQ(0.0, s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ EXPECT_DOUBLE_EQ(0.0, s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ EXPECT_DOUBLE_EQ(0.0, s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ EXPECT_DOUBLE_EQ(0.0, s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ q0 += increment;
+ q1 += increment;
+ }
+
+ for (size_t ctr = 0; ctr < 10; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_NEAR(ctr / 10.0, s, epsilon);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ EXPECT_NEAR(ctr / 10.0, s, epsilon);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ EXPECT_NEAR(ctr / 10.0, r, epsilon);
+ EXPECT_DOUBLE_EQ(0.0, s);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ EXPECT_NEAR(ctr / 10.0, r, epsilon);
+ EXPECT_DOUBLE_EQ(0.0, s);
+ q0 += increment;
+ q1 += increment;
+ }
+
+ for (size_t ctr = 0; ctr < 9; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_DOUBLE_EQ(1.0, s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ EXPECT_DOUBLE_EQ(1.0, s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_DOUBLE_EQ(0.0, s);
+ q0 += increment;
+ q1 += increment;
+ }
+
+ for (size_t ctr = 0; ctr < 2; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ q0 += increment;
+ q1 += increment;
+ }
+ }
+
+ {
+ SCOPED_TRACE("Test degenerate segment cases - Parallel segments");
+ Math::Vector3d newP;
+ Math::Vector3d newQ;
+
+ p0 = Math::Vector3d(0.0, 0.0, 1.0);
+ p1 = Math::Vector3d(1.0, 0.0, 1.0);
+ q0 = Math::Vector3d(-1.0, 0.0, 0.5);
+ q1 = Math::Vector3d(-2.0, 0.0, 0.5);
+
+ for (size_t ctr = 0; ctr < 2; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ q0 += increment;
+ q1 += increment;
+ }
+
+ for (size_t ctr = 0; ctr < 8; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_DOUBLE_EQ(0.0, s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ EXPECT_DOUBLE_EQ(0.0, s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ EXPECT_DOUBLE_EQ(0.0, s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ EXPECT_DOUBLE_EQ(0.0, s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ q0 += increment;
+ q1 += increment;
+ }
+
+ for (size_t ctr = 0; ctr < 20; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ newP = Math::interpolate(p0, p1, r);
+ newQ = Math::interpolate(q0, q1, s);
+ EXPECT_NEAR(0.5, (newP - newQ).norm(), epsilon);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ newP = Math::interpolate(p0, p1, r);
+ newQ = Math::interpolate(q0, q1, s);
+ EXPECT_NEAR(0.5, (newP - newQ).norm(), epsilon);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ newP = Math::interpolate(p0, p1, s);
+ newQ = Math::interpolate(q0, q1, r);
+ EXPECT_NEAR(0.5, (newP - newQ).norm(), epsilon);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ newP = Math::interpolate(p0, p1, s);
+ newQ = Math::interpolate(q0, q1, r);
+ EXPECT_NEAR(0.5, (newP - newQ).norm(), epsilon);
+ q0 += increment;
+ q1 += increment;
+ }
+
+ for (size_t ctr = 0; ctr < 9; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_DOUBLE_EQ(1.0, s);
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ EXPECT_DOUBLE_EQ(1.0, s);
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_DOUBLE_EQ(1.0, s);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_DOUBLE_EQ(1.0, s);
+ q0 += increment;
+ q1 += increment;
+ }
+
+ for (size_t ctr = 0; ctr < 2; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ q0 += increment;
+ q1 += increment;
+ }
+ }
+
+ {
+ SCOPED_TRACE("Test intersecting segments");
+ Math::Vector3d newP;
+ Math::Vector3d newQ;
+
+ p0 = Math::Vector3d(0.0, -1.0, 1.0);
+ p1 = Math::Vector3d(0.0, 1.0, 1.0);
+ q0 = Math::Vector3d(-1.0, 0.0, 0.5);
+ q1 = Math::Vector3d(-3.0, 0.0, 0.5);
+
+ for (size_t ctr = 0; ctr < 2; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ q0 += increment;
+ q1 += increment;
+ }
+
+ for (size_t ctr = 0; ctr < 8; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_DOUBLE_EQ(0.0, s);
+ EXPECT_DOUBLE_EQ(0.5, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ EXPECT_DOUBLE_EQ(0.0, s);
+ EXPECT_DOUBLE_EQ(0.5, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ EXPECT_DOUBLE_EQ(0.5, s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ EXPECT_DOUBLE_EQ(0.5, s);
+ EXPECT_DOUBLE_EQ(0.0, r);
+ q0 += increment;
+ q1 += increment;
+ }
+
+ for (size_t ctr = 0; ctr < 20; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ newP = Math::interpolate(p0, p1, r);
+ newQ = Math::interpolate(q0, q1, s);
+ EXPECT_NEAR(0.5, (newP - newQ).norm(), epsilon);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ newP = Math::interpolate(p0, p1, r);
+ newQ = Math::interpolate(q0, q1, s);
+ EXPECT_NEAR(0.5, (newP - newQ).norm(), epsilon);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ newP = Math::interpolate(p0, p1, s);
+ newQ = Math::interpolate(q0, q1, r);
+ EXPECT_NEAR(0.5, (newP - newQ).norm(), epsilon);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ newP = Math::interpolate(p0, p1, s);
+ newQ = Math::interpolate(q0, q1, r);
+ EXPECT_NEAR(0.5, (newP - newQ).norm(), epsilon);
+ q0 += increment;
+ q1 += increment;
+ }
+
+ for (size_t ctr = 0; ctr < 9; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_DOUBLE_EQ(1.0, s);
+ EXPECT_DOUBLE_EQ(0.5, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ EXPECT_DOUBLE_EQ(1.0, s);
+ EXPECT_DOUBLE_EQ(0.5, r);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_DOUBLE_EQ(0.5, s);
+ EXPECT_TRUE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ EXPECT_DOUBLE_EQ(1.0, r);
+ EXPECT_DOUBLE_EQ(0.5, s);
+ q0 += increment;
+ q1 += increment;
+ }
+
+ for (size_t ctr = 0; ctr < 2; ++ctr)
+ {
+ p[0] = p0;
+ p[1] = p1;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, distanceEpsilon, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(p, q, radiusP, radiusQ, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(q, p, distanceEpsilon, &r, &s));
+ EXPECT_FALSE(staticTest.collideStaticSegmentSegment(q, p, radiusP, radiusQ, &r, &s));
+ q0 += increment;
+ q1 += increment;
+ }
+ }
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/SegmentSelfContactTests.cpp b/SurgSim/Collision/UnitTests/SegmentSelfContactTests.cpp
new file mode 100644
index 0000000..bf234e4
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/SegmentSelfContactTests.cpp
@@ -0,0 +1,589 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 SegmentSegmentCheck functions.
+
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/SegmentSelfContact.h"
+#include "SurgSim/DataStructures/AabbTree.h"
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Math/SegmentMeshShape.h"
+
+using SurgSim::DataStructures::Location;
+using SurgSim::DataStructures::SegmentMeshPlain;
+using SurgSim::Math::SegmentMeshShape;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Collision
+{
+
+class MockSegmentCcdSelfContact : public SurgSim::Collision::SegmentSelfContact
+{
+public:
+ double maxTimePrecision(
+ const std::array<SurgSim::Math::Vector3d, 2>& pt0Positions,
+ const std::array<SurgSim::Math::Vector3d, 2>& pt1Positions,
+ const std::array<SurgSim::Math::Vector3d, 2>& qt0Positions,
+ const std::array<SurgSim::Math::Vector3d, 2>& qt1Positions,
+ double effectiveThickness) const
+ {
+ return SegmentSelfContact::maxTimePrecision(pt0Positions, pt1Positions,
+ qt0Positions, qt1Positions,
+ effectiveThickness);
+ }
+
+ bool isSameSegContactPoint(const Math::SegmentMeshShape& segmentShape,
+ size_t segId1, double s1, size_t segId2, double s2) const
+ {
+ return SegmentSelfContact::isSameSegContactPoint(segmentShape, segId1, s1, segId2, s2);
+ }
+
+ bool findSegSegContact(const Math::SegmentMeshShape& segmentShape,
+ const std::list<std::shared_ptr<Contact>>& contacts,
+ double toi, Collision::CollisionDetectionType collisionType,
+ size_t segId1, double s1, size_t segId2, double s2, double timeEpsilon) const
+ {
+ return SegmentSelfContact::findSegSegContact(
+ segmentShape, contacts, toi, collisionType, segId1, s1, segId2, s2, timeEpsilon);
+ }
+
+ bool detectExcessMovement(const SurgSim::Math::Vector3d& pt0,
+ const SurgSim::Math::Vector3d& pt1,
+ double threshold) const
+ {
+ return SegmentSelfContact::detectExcessMovement(pt0, pt1, threshold);
+ }
+
+ bool removeInvalidCollisions(
+ const Math::SegmentMeshShape& segmentA,
+ const Math::SegmentMeshShape& segmentB,
+ size_t id1, size_t id2) const
+ {
+ return SegmentSelfContact::removeInvalidCollisions(segmentA, segmentB, id1, id2);
+ }
+
+ void getUniqueCandidates(
+ const std::list<SurgSim::DataStructures::AabbTree::TreeNodePairType>& intersectionList,
+ std::set<std::pair<size_t, size_t>>* segmentIdList) const
+ {
+ SegmentSelfContact::getUniqueCandidates(intersectionList, segmentIdList);
+ }
+
+ bool detectCollision(
+ const std::array<SurgSim::Math::Vector3d, 2>& pt0Positions,
+ const std::array<SurgSim::Math::Vector3d, 2>& pt1Positions,
+ const std::array<SurgSim::Math::Vector3d, 2>& qt0Positions,
+ const std::array<SurgSim::Math::Vector3d, 2>& qt1Positions,
+ double segmentRadius1, double segmentRadius2, double timePrecision,
+ double* r, double* s, double* t,
+ SurgSim::Math::Vector3d* pToQDir,
+ SurgSim::Math::Vector3d* contactPtP,
+ SurgSim::Math::Vector3d* contactPtQ) const
+ {
+ return SegmentSelfContact::detectCollision(pt0Positions, pt1Positions, qt0Positions, qt1Positions,
+ segmentRadius1, segmentRadius2, timePrecision,
+ r, s, t, pToQDir, contactPtP, contactPtQ);
+ }
+};
+
+class SegmentCcdSelfContactTests : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ MockSegmentCcdSelfContact m_selfContact;
+ }
+
+ std::shared_ptr<Contact> createContact(CollisionDetectionType type, double r, double s, size_t seg1, size_t seg2,
+ double toi)
+ {
+ std::pair<Location, Location> penetrationPoints;
+ Math::Vector2d parametricCoordinateP(1.0 - r, r);
+ penetrationPoints.first.elementMeshLocalCoordinate.setValue(
+ SurgSim::DataStructures::IndexedLocalCoordinate(seg1, parametricCoordinateP));
+ Math::Vector2d parametricCoordinateQ(1.0 - s, s);
+ penetrationPoints.second.elementMeshLocalCoordinate.setValue(
+ SurgSim::DataStructures::IndexedLocalCoordinate(seg2, parametricCoordinateQ));
+
+ // Dummy up some values unless we need them
+ Vector3d normal(1.0, 0.0, 0.0);
+ Vector3d contactPoint(0.0, 0.0, 0.0);
+ return std::make_shared<Contact>(type, 0.0, toi, contactPoint, normal, penetrationPoints);
+ }
+
+ std::shared_ptr<SegmentMeshShape> build(const Vector3d& start, const Vector3d& direction, double radius,
+ size_t numVertices = 10) const
+ {
+ auto mesh = std::make_shared<SegmentMeshPlain>();
+
+ // Add the vertices
+ for (size_t i = 0; i < numVertices; ++i)
+ {
+ mesh->addVertex(SegmentMeshPlain::VertexType(start + direction * static_cast<double>(i)));
+ }
+
+ // Define the edges
+ for (size_t i = 0; i < numVertices - 1; ++i)
+ {
+ std::array<size_t, 2> indices = {{i, i + 1}};
+ mesh->addEdge(SegmentMeshPlain::EdgeType(indices));
+ }
+
+ return std::make_shared<SegmentMeshShape>(*mesh, radius);
+ }
+
+ std::shared_ptr<SegmentMeshShape> buildLoop(double zInit, double radius) const
+ {
+ auto mesh = std::make_shared<SegmentMeshPlain>();
+ double zDelta = 2 * zInit / 10;
+ double z = zInit;
+
+ mesh->addVertex(SegmentMeshPlain::VertexType(Vector3d(1.0, 1.0, z)));
+ z -= zDelta;
+ mesh->addVertex(SegmentMeshPlain::VertexType(Vector3d(0.0, 0.0, z)));
+ z -= zDelta;
+ mesh->addVertex(SegmentMeshPlain::VertexType(Vector3d(-0.25, -0.25, z)));
+ z -= zDelta;
+ mesh->addVertex(SegmentMeshPlain::VertexType(Vector3d(-0.5, -0.5, z)));
+ z -= zDelta;
+ mesh->addVertex(SegmentMeshPlain::VertexType(Vector3d(-0.25, -0.75, z)));
+ z -= zDelta;
+ mesh->addVertex(SegmentMeshPlain::VertexType(Vector3d(0.0, -1.0, z)));
+ z -= zDelta;
+ mesh->addVertex(SegmentMeshPlain::VertexType(Vector3d(0.25, -0.75, z)));
+ z -= zDelta;
+ mesh->addVertex(SegmentMeshPlain::VertexType(Vector3d(0.5, -0.5, z)));
+ z -= zDelta;
+ mesh->addVertex(SegmentMeshPlain::VertexType(Vector3d(0.25, -0.25, z)));
+ z -= zDelta;
+ mesh->addVertex(SegmentMeshPlain::VertexType(Vector3d(0.0, 0.0, z)));
+ z -= zDelta;
+ mesh->addVertex(SegmentMeshPlain::VertexType(Vector3d(-1.0, 1.0, z)));
+
+ // Define the edges
+ for (size_t i = 0; i < 10; ++i)
+ {
+ std::array<size_t, 2> indices = {{i, i + 1}};
+ mesh->addEdge(SegmentMeshPlain::EdgeType(indices));
+ }
+
+ return std::make_shared<SegmentMeshShape>(*mesh, radius);
+ }
+
+ MockSegmentCcdSelfContact m_selfContact;
+};
+
+TEST_F(SegmentCcdSelfContactTests, Initialization)
+{
+ ASSERT_NO_THROW(SegmentSelfContact selfContact);
+ SegmentSelfContact selfContact;
+
+ auto shapeTypes = selfContact.getShapeTypes();
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_SEGMENTMESH, shapeTypes.first);
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_SEGMENTMESH, shapeTypes.second);
+
+ EXPECT_DOUBLE_EQ(1e-06, selfContact.getTimeMinPrecisionEpsilon());
+ EXPECT_DOUBLE_EQ(1.0, selfContact.getTimeMaxPrecisionEpsilon());
+ EXPECT_DOUBLE_EQ(1e-04, selfContact.distanceEpsilon());
+
+ EXPECT_ANY_THROW(selfContact.setTimeMinPrecisionEpsilon(0.0));
+ EXPECT_ANY_THROW(selfContact.setTimeMinPrecisionEpsilon(-2.0e-06));
+ EXPECT_ANY_THROW(selfContact.setTimeMaxPrecisionEpsilon(0.0));
+ EXPECT_ANY_THROW(selfContact.setTimeMaxPrecisionEpsilon(-1.0e-05));
+
+ selfContact.setTimeMinPrecisionEpsilon(2.0e-06);
+ selfContact.setTimeMaxPrecisionEpsilon(1.0e-05);
+ selfContact.setDistanceEpsilon(1.0e-06);
+
+ EXPECT_DOUBLE_EQ(2e-06, selfContact.getTimeMinPrecisionEpsilon());
+ EXPECT_DOUBLE_EQ(1.0e-05, selfContact.getTimeMaxPrecisionEpsilon());
+ EXPECT_DOUBLE_EQ(1.0e-06, selfContact.distanceEpsilon());
+};
+
+TEST_F(SegmentCcdSelfContactTests, MaxTimePrecision)
+{
+ Math::Vector3d zeroPosition(0.0, 0.0, 0.0);
+ Math::Vector3d changePos(1.0, 2.0, 3.0);
+ Math::Vector3d changeNeg(-1.0, -2.0, -3.0);
+
+ std::array<Math::Vector3d, 2> noChange = {zeroPosition, zeroPosition};
+ std::array<Math::Vector3d, 2> change = {changePos, changeNeg};
+ m_selfContact.setTimeMaxPrecisionEpsilon(1.0e-05);
+
+ ASSERT_ANY_THROW(m_selfContact.maxTimePrecision(change, noChange, noChange, noChange, -0.5));
+
+ EXPECT_DOUBLE_EQ(1.0e-05,
+ m_selfContact.maxTimePrecision(change, noChange, noChange, noChange, 0.5));
+ EXPECT_DOUBLE_EQ(1.0e-05,
+ m_selfContact.maxTimePrecision(noChange, change, noChange, noChange, 0.5));
+ EXPECT_DOUBLE_EQ(1.0e-05,
+ m_selfContact.maxTimePrecision(noChange, noChange, change, noChange, 0.5));
+ EXPECT_DOUBLE_EQ(1.0e-05,
+ m_selfContact.maxTimePrecision(noChange, noChange, noChange, change, 0.5));
+
+ EXPECT_DOUBLE_EQ(3.0e-05 / std::sqrt(14),
+ m_selfContact.maxTimePrecision(change, noChange, noChange, noChange, 3.0e-05));
+ EXPECT_DOUBLE_EQ(3.0e-05 / std::sqrt(14),
+ m_selfContact.maxTimePrecision(noChange, change, noChange, noChange, 3.0e-05));
+ EXPECT_DOUBLE_EQ(3.0e-05 / std::sqrt(14),
+ m_selfContact.maxTimePrecision(noChange, noChange, change, noChange, 3.0e-05));
+ EXPECT_DOUBLE_EQ(3.0e-05 / std::sqrt(14),
+ m_selfContact.maxTimePrecision(noChange, noChange, noChange, change, 3.0e-05));
+
+ EXPECT_DOUBLE_EQ(1.0e-06,
+ m_selfContact.maxTimePrecision(change, noChange, noChange, noChange, 3.0e-07));
+ EXPECT_DOUBLE_EQ(1.0e-06,
+ m_selfContact.maxTimePrecision(noChange, change, noChange, noChange, 3.0e-07));
+ EXPECT_DOUBLE_EQ(1.0e-06,
+ m_selfContact.maxTimePrecision(noChange, noChange, change, noChange, 3.0e-07));
+ EXPECT_DOUBLE_EQ(1.0e-06,
+ m_selfContact.maxTimePrecision(noChange, noChange, noChange, change, 3.0e-07));
+};
+
+TEST_F(SegmentCcdSelfContactTests, IsSameSegContactPoint)
+{
+ std::shared_ptr<SegmentMeshShape> shape =
+ build(Vector3d(-10.0, -10.0, -10.0), Vector3d(10.0, 10.0, 10.0), 0.5, 10);
+ m_selfContact.setTimeMaxPrecisionEpsilon(1.0e-06);
+ m_selfContact.setTimeMinPrecisionEpsilon(1.0e-06);
+ m_selfContact.setDistanceEpsilon(1.0e-09);
+
+ // Different segments, no shared endpoints
+ EXPECT_FALSE(m_selfContact.isSameSegContactPoint(*shape, 1, 0.9, 7, 0.8));
+
+ // Same segment, discrete contact points
+ EXPECT_FALSE(m_selfContact.isSameSegContactPoint(*shape, 1, 0.9 - 1.0e-09, 1, 0.9 + 1.0e-09));
+
+ // Same segment, same contact point
+ EXPECT_TRUE(m_selfContact.isSameSegContactPoint(*shape, 1, 0.9 - 5.0e-10, 1, 0.9 + 4.9e-10));
+
+ // Different segment, failure on distance from endpoint
+ EXPECT_FALSE(m_selfContact.isSameSegContactPoint(*shape, 1, 2.0e-09, 0, 1.0));
+ EXPECT_FALSE(m_selfContact.isSameSegContactPoint(*shape, 1, 1.0 - 2.0e-09, 2, 0.0));
+ EXPECT_FALSE(m_selfContact.isSameSegContactPoint(*shape, 1, 0.0, 0, 1.0 - 2.0e-09));
+ EXPECT_FALSE(m_selfContact.isSameSegContactPoint(*shape, 1, 1.0, 2, 2.0e-09));
+
+ // Different segment, failure on adjacency
+ EXPECT_FALSE(m_selfContact.isSameSegContactPoint(*shape, 1, 4.9e-10, 2, 1.0 - 5.0e-10));
+ EXPECT_FALSE(m_selfContact.isSameSegContactPoint(*shape, 1, 1.0 - 5.0e-10, 0, 4.9e-10));
+ EXPECT_FALSE(m_selfContact.isSameSegContactPoint(*shape, 1, 4.9e-10, 2, 1.0 - 5.0e-10));
+ EXPECT_FALSE(m_selfContact.isSameSegContactPoint(*shape, 1, 1.0 - 5.0e-10, 0, 4.9e-10));
+
+ // Different segment, same contact point
+ EXPECT_TRUE(m_selfContact.isSameSegContactPoint(*shape, 1, 4.9e-10, 0, 1.0 - 5.0e-10));
+ EXPECT_TRUE(m_selfContact.isSameSegContactPoint(*shape, 1, 1.0 - 5.0e-10, 2, 4.9e-10));
+ EXPECT_TRUE(m_selfContact.isSameSegContactPoint(*shape, 1, 4.9e-10, 0, 1.0 - 5.0e-10));
+ EXPECT_TRUE(m_selfContact.isSameSegContactPoint(*shape, 1, 1.0 - 5.0e-10, 2, 4.9e-10));
+};
+
+TEST_F(SegmentCcdSelfContactTests, FindSegSegContact)
+{
+ std::shared_ptr<SegmentMeshShape> shape =
+ build(Vector3d(-10.0, -10.0, -10.0), Vector3d(10.0, 10.0, 10.0), 0.5, 10);
+
+ {
+ SCOPED_TRACE("Wrong collision type");
+ std::list<std::shared_ptr<Contact>> contacts;
+ contacts.emplace_back(createContact(
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_DISCRETE, 0.8, 0.4, 1, 3, 0.25));
+
+ EXPECT_FALSE(m_selfContact.findSegSegContact(*shape, contacts, 0.25,
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, 1, 0.8, 3, 0.4, 1.0e-06));
+ }
+ {
+ SCOPED_TRACE("Wrong time");
+ std::list<std::shared_ptr<Contact>> contacts;
+ contacts.emplace_back(createContact(
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, 0.8, 0.4, 1, 3, 0.25));
+
+ EXPECT_FALSE(m_selfContact.findSegSegContact(*shape, contacts, 0.25 - 2.0e-06,
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, 1, 0.8, 3, 0.4, 1.0e-06));
+ EXPECT_FALSE(m_selfContact.findSegSegContact(*shape, contacts, 0.25 + 2.0e-06,
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, 1, 0.8, 3, 0.4, 1.0e-06));
+ }
+ {
+ SCOPED_TRACE("Wrong segment ID");
+ std::list<std::shared_ptr<Contact>> contacts;
+ contacts.emplace_back(createContact(
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, 0.8, 0.4, 1, 3, 0.25));
+
+ EXPECT_FALSE(m_selfContact.findSegSegContact(*shape, contacts, 0.25,
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, 2, 0.8, 3, 0.4, 1.0e-06));
+ EXPECT_FALSE(m_selfContact.findSegSegContact(*shape, contacts, 0.25,
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, 1, 0.4, 4, 0.4, 1.0e-06));
+ }
+ {
+ SCOPED_TRACE("Wrong parametric value");
+ std::list<std::shared_ptr<Contact>> contacts;
+ contacts.emplace_back(createContact(
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, 0.8, 0.4, 1, 3, 0.25));
+
+ EXPECT_FALSE(m_selfContact.findSegSegContact(*shape, contacts, 0.25,
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, 1, 0.8 - 1.0e-03, 3, 0.4, 1.0e-06));
+ }
+ {
+ SCOPED_TRACE("Same point, middle of segment");
+ std::list<std::shared_ptr<Contact>> contacts;
+ contacts.emplace_back(createContact(
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, 0.8, 0.4, 1, 3, 0.25));
+
+ EXPECT_TRUE(m_selfContact.findSegSegContact(*shape, contacts, 0.25,
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, 1, 0.8, 3, 0.4, 1.0e-06));
+ }
+ {
+ SCOPED_TRACE("Same point, segment endpoints");
+ std::list<std::shared_ptr<Contact>> contacts;
+ contacts.emplace_back(createContact(
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, 1.0, 0.0, 1, 4, 0.25));
+
+ EXPECT_TRUE(m_selfContact.findSegSegContact(*shape, contacts, 0.25,
+ CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, 2, 0.0, 3, 1.0, 1.0e-06));
+ }
+};
+
+TEST_F(SegmentCcdSelfContactTests, DetectExcessMovement)
+{
+ Vector3d point1(1.0e-03 / 2.0, 1.0e-03 / 2.0, 1.0e-03 / 2.0);
+ Vector3d point2(0.0, 0.0, 0.0);
+
+ EXPECT_FALSE(m_selfContact.detectExcessMovement(point1, point2, 1.0e-03));
+ EXPECT_FALSE(m_selfContact.detectExcessMovement(point2, point1, 1.0e-03));
+
+ point2 = Vector3d(-1.0e-03, 0.0, 0.0);
+ EXPECT_TRUE(m_selfContact.detectExcessMovement(point1, point2, 1.0e-03));
+ EXPECT_TRUE(m_selfContact.detectExcessMovement(point2, point1, 1.0e-03));
+
+ point2 = Vector3d(0.0, -1.0e-03, 0.0);
+ EXPECT_TRUE(m_selfContact.detectExcessMovement(point1, point2, 1.0e-03));
+ EXPECT_TRUE(m_selfContact.detectExcessMovement(point2, point1, 1.0e-03));
+
+ point2 = Vector3d(0.0, 0.0, -1.0e-03);
+ EXPECT_TRUE(m_selfContact.detectExcessMovement(point1, point2, 1.0e-03));
+ EXPECT_TRUE(m_selfContact.detectExcessMovement(point2, point1, 1.0e-03));
+};
+
+TEST_F(SegmentCcdSelfContactTests, RemoveInvalidCollisions)
+{
+ std::shared_ptr<SegmentMeshShape> shapeT0 =
+ buildLoop(1.0e-03, 1.0e-04);
+ std::shared_ptr<SegmentMeshShape> shapeT1 =
+ buildLoop(-1.0e-03, 1.0e-04);
+
+ EXPECT_FALSE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 0, 9));
+ EXPECT_FALSE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 1, 9));
+ EXPECT_FALSE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 0, 8));
+ EXPECT_FALSE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 1, 8));
+
+ EXPECT_TRUE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 1, 1));
+ EXPECT_TRUE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 0, 1));
+ EXPECT_TRUE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 1, 2));
+ EXPECT_TRUE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 1, 0));
+
+ shapeT0->getVertex(0).position += Vector3d(10, 0, 0);
+ EXPECT_TRUE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 0, 9));
+ shapeT0->getVertex(0).position -= Vector3d(10, 0, 0);
+
+ shapeT0->getVertex(1).position += Vector3d(10, 0, 0);
+ EXPECT_TRUE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 0, 9));
+ shapeT0->getVertex(1).position -= Vector3d(10, 0, 0);
+
+ shapeT1->getVertex(0).position += Vector3d(10, 0, 0);
+ EXPECT_TRUE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 0, 9));
+ shapeT1->getVertex(0).position -= Vector3d(10, 0, 0);
+
+ shapeT1->getVertex(1).position += Vector3d(10, 0, 0);
+ EXPECT_TRUE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 0, 9));
+ shapeT1->getVertex(1).position -= Vector3d(10, 0, 0);
+
+ shapeT0->getVertex(9).position += Vector3d(10, 0, 0);
+ EXPECT_TRUE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 0, 9));
+ shapeT0->getVertex(9).position -= Vector3d(10, 0, 0);
+
+ shapeT0->getVertex(10).position += Vector3d(10, 0, 0);
+ EXPECT_TRUE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 0, 9));
+ shapeT0->getVertex(10).position -= Vector3d(10, 0, 0);
+
+ shapeT1->getVertex(9).position += Vector3d(10, 0, 0);
+ EXPECT_TRUE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 0, 9));
+ shapeT1->getVertex(9).position -= Vector3d(10, 0, 0);
+
+ shapeT1->getVertex(10).position += Vector3d(10, 0, 0);
+ EXPECT_TRUE(m_selfContact.removeInvalidCollisions(*shapeT0, *shapeT1, 0, 9));
+ shapeT1->getVertex(10).position -= Vector3d(10, 0, 0);
+};
+
+TEST_F(SegmentCcdSelfContactTests, GetUniqueCandidates)
+{
+ std::shared_ptr<SegmentMeshShape> shapeT0 =
+ buildLoop(1.0e-03, 1.0e-04);
+ std::shared_ptr<SegmentMeshShape> shapeT1 =
+ buildLoop(-1.0e-03, 1.0e-04);
+
+ std::set<std::pair<size_t, size_t>> segmentIdList;
+ std::list<SurgSim::DataStructures::AabbTree::TreeNodePairType> intersectionList
+ = shapeT0->getAabbTree()->spatialJoin(*(shapeT1->getAabbTree()));
+ m_selfContact.getUniqueCandidates(intersectionList, &segmentIdList);
+ EXPECT_EQ(6, segmentIdList.size());
+ EXPECT_TRUE(std::find(segmentIdList.begin(), segmentIdList.end(),
+ std::pair<size_t, size_t>(0, 9)) != segmentIdList.end());
+ EXPECT_TRUE(std::find(segmentIdList.begin(), segmentIdList.end(),
+ std::pair<size_t, size_t>(0, 8)) != segmentIdList.end());
+ EXPECT_TRUE(std::find(segmentIdList.begin(), segmentIdList.end(),
+ std::pair<size_t, size_t>(1, 9)) != segmentIdList.end());
+ EXPECT_TRUE(std::find(segmentIdList.begin(), segmentIdList.end(),
+ std::pair<size_t, size_t>(1, 8)) != segmentIdList.end());
+ EXPECT_TRUE(std::find(segmentIdList.begin(), segmentIdList.end(),
+ std::pair<size_t, size_t>(4, 5)) != segmentIdList.end());
+ EXPECT_TRUE(std::find(segmentIdList.begin(), segmentIdList.end(),
+ std::pair<size_t, size_t>(5, 6)) != segmentIdList.end());
+
+ EXPECT_FALSE(std::find(segmentIdList.begin(), segmentIdList.end(),
+ std::pair<size_t, size_t>(1, 7)) != segmentIdList.end());
+};
+
+TEST_F(SegmentCcdSelfContactTests, DetectCollision)
+{
+ std::shared_ptr<SegmentMeshShape> shapeT0 =
+ buildLoop(1.0e-03, 1.0e-04);
+ std::shared_ptr<SegmentMeshShape> shapeT1 =
+ buildLoop(-1.0e-03, 1.0e-04);
+
+ double r;
+ double s;
+ double t;
+ Vector3d pToQDir;
+ Vector3d contactPtP;
+ Vector3d contactPtQ;
+
+ {
+ SCOPED_TRACE("Successful detection");
+ std::array<Math::Vector3d, 2> pt0Positions = shapeT0->getEdgePositions(0);
+ std::array<Math::Vector3d, 2> pt1Positions = shapeT1->getEdgePositions(0);
+ std::array<Math::Vector3d, 2> qt0Positions = shapeT0->getEdgePositions(9);
+ std::array<Math::Vector3d, 2> qt1Positions = shapeT1->getEdgePositions(9);
+ m_selfContact.setTimeMaxPrecisionEpsilon(1.0e-06);
+ m_selfContact.setTimeMinPrecisionEpsilon(1.0e-06);
+ m_selfContact.setDistanceEpsilon(1.0e-09);
+
+ EXPECT_TRUE(m_selfContact.detectCollision(pt0Positions, pt1Positions, qt0Positions, qt1Positions,
+ 1.0e-04, 1.0e-04, 1.0e-09,
+ &r, &s, &t, &pToQDir, &contactPtP, &contactPtQ));
+ EXPECT_GT(1.0e-09, std::abs(r - 1.0));
+ EXPECT_GT(1.0e-09, std::abs(s));
+ EXPECT_GT(2.0e-09, std::abs(t - 0.4375));
+ EXPECT_TRUE(pToQDir.normalized().isApprox(Vector3d(0.0, 0.0, -1.0), 1.0e-05));
+ EXPECT_TRUE(contactPtP.isApprox(Vector3d(0.0, 0.0, 1.0e-04), 1.0e-05));
+ EXPECT_TRUE(contactPtQ.isApprox(Vector3d(0.0, 0.0, -1.0e-04), 1.0e-05));
+
+ EXPECT_TRUE(m_selfContact.detectCollision(pt0Positions, pt1Positions, qt0Positions, qt1Positions,
+ 0.0, 0.0, 1.0e-09,
+ &r, &s, &t, &pToQDir, &contactPtP, &contactPtQ));
+ EXPECT_GT(1.0e-09, std::abs(r - 1.0));
+ EXPECT_GT(1.0e-09, std::abs(s));
+ EXPECT_GT(2.0e-03, std::abs(t - 0.5));
+ EXPECT_GT(1.0e-09, pToQDir.norm());
+ EXPECT_GT(1.0e-09, (contactPtP - Vector3d(0.0, 0.0, 5.0e-10)).norm());
+ EXPECT_GT(1.0e-09, (contactPtQ - Vector3d(0.0, 0.0, 5.0e-10)).norm());
+ }
+ {
+ SCOPED_TRACE("Detection at T=0 (before movement)");
+ std::array<Math::Vector3d, 2> pt0Positions = shapeT0->getEdgePositions(4);
+ std::array<Math::Vector3d, 2> pt1Positions = shapeT1->getEdgePositions(4);
+ std::array<Math::Vector3d, 2> qt0Positions = shapeT0->getEdgePositions(5);
+ std::array<Math::Vector3d, 2> qt1Positions = shapeT1->getEdgePositions(5);
+
+ EXPECT_TRUE(m_selfContact.detectCollision(pt0Positions, pt1Positions, qt0Positions, qt1Positions,
+ 1.0e-04, 1.0e-04, 1.0e-03,
+ &r, &s, &t, &pToQDir, &contactPtP, &contactPtQ));
+ EXPECT_GT(1.0e-09, std::abs(r - 1.0));
+ EXPECT_GT(1.0e-09, std::abs(s));
+ EXPECT_GT(1.0e-09, std::abs(t));
+ EXPECT_GT(1.0e-09, pToQDir.norm());
+ EXPECT_TRUE(contactPtP.isApprox(Vector3d(0.0, -1.0, 0.0), 1.0e-05));
+ EXPECT_TRUE(contactPtQ.isApprox(Vector3d(0.0, -1.0, 0.0), 1.0e-05));
+
+ EXPECT_TRUE(m_selfContact.detectCollision(pt0Positions, pt1Positions, qt0Positions, qt1Positions,
+ 0.0, 0.0, 1.0e-03,
+ &r, &s, &t, &pToQDir, &contactPtP, &contactPtQ));
+ EXPECT_GT(1.0e-09, std::abs(r - 1.0));
+ EXPECT_GT(1.0e-09, std::abs(s));
+ EXPECT_GT(1.0e-09, std::abs(t));
+ EXPECT_GT(1.0e-09, pToQDir.norm());
+ EXPECT_TRUE(contactPtP.isApprox(Vector3d(0.0, -1.0, 0.0), 1.0e-05));
+ EXPECT_TRUE(contactPtQ.isApprox(Vector3d(0.0, -1.0, 0.0), 1.0e-05));
+ }
+ {
+ SCOPED_TRACE("No detection");
+ std::array<Math::Vector3d, 2> pt0Positions = shapeT0->getEdgePositions(1);
+ std::array<Math::Vector3d, 2> pt1Positions = shapeT1->getEdgePositions(1);
+ std::array<Math::Vector3d, 2> qt0Positions = shapeT0->getEdgePositions(7);
+ std::array<Math::Vector3d, 2> qt1Positions = shapeT1->getEdgePositions(7);
+
+ EXPECT_FALSE(m_selfContact.detectCollision(pt0Positions, pt1Positions, qt0Positions, qt1Positions,
+ 1.0e-04, 1.0e-04, 1.0e-03,
+ &r, &s, &t, &pToQDir, &contactPtP, &contactPtQ));
+ EXPECT_FALSE(m_selfContact.detectCollision(pt0Positions, pt1Positions, qt0Positions, qt1Positions,
+ 0.0, 0.0, 1.0e-03,
+ &r, &s, &t, &pToQDir, &contactPtP, &contactPtQ));
+ }
+};
+TEST_F(SegmentCcdSelfContactTests, CalculateContact)
+{
+ {
+ SCOPED_TRACE("Successful detection");
+ std::shared_ptr<SegmentMeshShape> shapeT0 =
+ buildLoop(1.0e-03, 1.0e-04);
+ std::shared_ptr<SegmentMeshShape> shapeT1 =
+ buildLoop(-1.0e-03, 1.0e-04);
+
+ m_selfContact.setDistanceEpsilon(1.0e-06);
+ auto transformP = Math::RigidTransform3d::Identity();
+ transformP.translate(Vector3d(1.0, 2.0, 3.0));
+
+ auto transformQ = Math::RigidTransform3d::Identity();
+ transformQ.translate(Vector3d(3.0, 4.0, 5.0));
+
+ auto collisionList = m_selfContact.calculateCcdContact(
+ *shapeT0, transformP,
+ *shapeT1, transformQ,
+ *shapeT0, transformP, // unused for self-collision
+ *shapeT1, transformQ); // unused for self-collision
+
+ EXPECT_EQ(1, collisionList.size());
+ std::shared_ptr<Collision::Contact> contacted = *(collisionList.begin());
+ EXPECT_EQ(CollisionDetectionType::COLLISION_DETECTION_TYPE_CONTINUOUS, contacted->type);
+ EXPECT_GT(2.0 * m_selfContact.getTimeMinPrecisionEpsilon(), std::abs(contacted->time - 0.4375));
+ EXPECT_GT(1.0e-08, contacted->contact.norm());
+ EXPECT_GT(1.0e-04, (contacted->normal.normalized() - Vector3d(0.0, 0.0, 1.0)).norm());
+ auto contactP = contacted->penetrationPoints.first.rigidLocalPosition.getValue();
+ EXPECT_TRUE((contactP.isApprox(transformP.inverse() *
+ (contacted->contact + Vector3d(0.0, 0.0, 0.0001)), 1.0e-06)));
+ auto contactQ = contacted->penetrationPoints.second.rigidLocalPosition.getValue();
+ EXPECT_TRUE((contactQ.isApprox(transformQ.inverse() *
+ (contacted->contact + Vector3d(0.0, 0.0, -0.0001)), 1.0e-06)));
+ EXPECT_GT(1.0e-10, contacted->depth);
+ }
+};
+
+}; // namespace Collision
+}; // namespace SurgSim
diff --git a/SurgSim/Collision/UnitTests/ShapeCollisionRepresentationTest.cpp b/SurgSim/Collision/UnitTests/ShapeCollisionRepresentationTest.cpp
index 320b811..a7448fe 100644
--- a/SurgSim/Collision/UnitTests/ShapeCollisionRepresentationTest.cpp
+++ b/SurgSim/Collision/UnitTests/ShapeCollisionRepresentationTest.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -27,7 +27,8 @@
using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Vector3d;
-namespace {
+namespace
+{
static const double dt = 0.001;
}
@@ -38,33 +39,34 @@ namespace Collision
TEST(ShapeCollisionRepresentationTest, MeshUpdateTest)
{
- SurgSim::Framework::Runtime runtime("config.txt");
+ auto runtime = std::make_shared<SurgSim::Framework::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));
+ const std::string fileName = "Geometry/staple_collision.ply";
+ auto mesh = std::make_shared<SurgSim::DataStructures::TriangleMeshPlain>();
+ ASSERT_NO_THROW(mesh->load(fileName));
+
+ auto originalMesh = std::make_shared<SurgSim::Math::MeshShape>(*mesh);
+ auto expectedMesh = std::make_shared<SurgSim::Math::MeshShape>(*mesh);
auto collisionRepresentation = std::make_shared<ShapeCollisionRepresentation>("Collision");
- collisionRepresentation->setShape(meshShape);
+ collisionRepresentation->setShape(originalMesh);
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(collisionRepresentation->getShapeType(), originalMesh->getType());
+ auto actualMesh = std::static_pointer_cast<SurgSim::Math::MeshShape>(collisionRepresentation->getPosedShape());
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));
-
+ 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);
-
+ actualMesh = std::static_pointer_cast<SurgSim::Math::MeshShape>(collisionRepresentation->getPosedShape());
+ expectedMesh->transform(transform);
+ EXPECT_TRUE(expectedMesh->update());
EXPECT_EQ(expectedMesh->getVertices(), actualMesh->getVertices());
EXPECT_EQ(expectedMesh->getTriangles(), actualMesh->getTriangles());
}
@@ -73,7 +75,7 @@ TEST(ShapeCollisionRepresentationTest, SerializationTest)
{
SurgSim::Framework::Runtime runtime("config.txt");
- const std::string fileName = "MeshShapeData/staple_collision.ply";
+ const std::string fileName = "Geometry/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));
@@ -81,28 +83,25 @@ TEST(ShapeCollisionRepresentationTest, SerializationTest)
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));
+ 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>>()));
+ (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"));
+ 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());
+ EXPECT_EQ(meshShape->getNumEdges(), mesh->getNumEdges());
+ EXPECT_EQ(meshShape->getNumTriangles(), mesh->getNumTriangles());
+ EXPECT_EQ(meshShape->getNumVertices(), mesh->getNumVertices());
}
} // namespace Collision
diff --git a/SurgSim/Collision/UnitTests/SphereDoubleSidedPlaneContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/SphereDoubleSidedPlaneContactCalculationTests.cpp
index fa6cb18..25e9761 100644
--- a/SurgSim/Collision/UnitTests/SphereDoubleSidedPlaneContactCalculationTests.cpp
+++ b/SurgSim/Collision/UnitTests/SphereDoubleSidedPlaneContactCalculationTests.cpp
@@ -14,7 +14,7 @@
// limitations under the License.
#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
-#include "SurgSim/Collision/SphereDoubleSidedPlaneDcdContact.h"
+#include "SurgSim/Collision/SphereDoubleSidedPlaneContact.h"
#include "SurgSim/Math/PlaneShape.h"
#include "SurgSim/Math/SphereShape.h"
@@ -50,7 +50,7 @@ void doSphereDoubleSidedPlaneTest(std::shared_ptr<SphereShape> sphere,
sphereRep->setShape(sphere);
sphereRep->setLocalPose(SurgSim::Math::makeRigidTransform(sphereQuat, sphereTrans));
- SphereDoubleSidedPlaneDcdContact calcNormal;
+ SphereDoubleSidedPlaneContact 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
@@ -158,7 +158,7 @@ TEST(SphereDoubleSidedPlaneContactCalculationTests, ShouldFail)
std::shared_ptr<CollisionPair> pairpp = std::make_shared<CollisionPair>(repp0, repp1);
std::shared_ptr<CollisionPair> pairss = std::make_shared<CollisionPair>(reps0, reps1);
- SphereDoubleSidedPlaneDcdContact contact;
+ SphereDoubleSidedPlaneContact contact;
EXPECT_ANY_THROW(contact.calculateContact(pairpp));
EXPECT_ANY_THROW(contact.calculateContact(pairss));
diff --git a/SurgSim/Collision/UnitTests/SpherePlaneContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/SpherePlaneContactCalculationTests.cpp
index 2c3a7c7..b8cc8d8 100644
--- a/SurgSim/Collision/UnitTests/SpherePlaneContactCalculationTests.cpp
+++ b/SurgSim/Collision/UnitTests/SpherePlaneContactCalculationTests.cpp
@@ -14,7 +14,7 @@
// limitations under the License.
#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
-#include "SurgSim/Collision/SpherePlaneDcdContact.h"
+#include "SurgSim/Collision/SpherePlaneContact.h"
#include "SurgSim/Math/PlaneShape.h"
#include "SurgSim/Math/SphereShape.h"
@@ -46,7 +46,7 @@ void doSpherePlaneTest(std::shared_ptr<SphereShape> sphere,
sphereRep->setShape(sphere);
sphereRep->setLocalPose(SurgSim::Math::makeRigidTransform(sphereQuat, sphereTrans));
- SpherePlaneDcdContact calcNormal;
+ SpherePlaneContact 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
diff --git a/SurgSim/Collision/UnitTests/SphereSphereContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/SphereSphereContactCalculationTests.cpp
index c0f323e..dc23a97 100644
--- a/SurgSim/Collision/UnitTests/SphereSphereContactCalculationTests.cpp
+++ b/SurgSim/Collision/UnitTests/SphereSphereContactCalculationTests.cpp
@@ -14,7 +14,7 @@
// limitations under the License.
#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
-#include "SurgSim/Collision/SphereSphereDcdContact.h"
+#include "SurgSim/Collision/SphereSphereContact.h"
#include "SurgSim/Math/Geometry.h"
#include "SurgSim/Math/SphereShape.h"
@@ -30,7 +30,7 @@ void doSphereSphereTest(double r0, Vector3d p0, double r1, Vector3d p1, bool has
Vector3d expectedPenetrationPoint0 = Vector3d::Zero(),
Vector3d expectedPenetrationPoint1 = Vector3d::Zero())
{
- SphereSphereDcdContact calc;
+ SphereSphereContact calc;
auto sphere1 = std::make_shared<SurgSim::Math::SphereShape>(r0);
auto sphereRep1 = std::make_shared<ShapeCollisionRepresentation>("TestSphereShapeCollisionRep1");
diff --git a/SurgSim/Collision/UnitTests/TriangleMeshParticlesContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/TriangleMeshParticlesContactCalculationTests.cpp
new file mode 100644
index 0000000..9f20305
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/TriangleMeshParticlesContactCalculationTests.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 <gtest/gtest.h>
+#include <unordered_set>
+
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/Collision/TriangleMeshParticlesContact.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/ParticlesShape.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+using SurgSim::Math::Geometry::DistanceEpsilon;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::MeshShape;
+using SurgSim::Math::ParticlesShape;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+
+namespace SurgSim
+{
+namespace Collision
+{
+
+void doCollisionTest(std::shared_ptr<ParticlesShape> particles, std::shared_ptr<MeshShape> mesh,
+ const RigidTransform3d& meshPose, const size_t numContactingParticles)
+{
+ auto meshRep = std::make_shared<ShapeCollisionRepresentation>("Mesh");
+ meshRep->setShape(mesh);
+ meshRep->setLocalPose(meshPose);
+
+ auto particlesRep = std::make_shared<ShapeCollisionRepresentation>("Particles");
+ particlesRep->setShape(particles);
+
+ TriangleMeshParticlesContact calcContact;
+ auto pair = std::make_shared<CollisionPair>(meshRep, particlesRep);
+ calcContact.calculateContact(pair);
+
+ std::unordered_set<size_t> contactingParticles;
+ for (auto& contact : pair->getContacts())
+ {
+ size_t particleIndex = contact->penetrationPoints.second.index.getValue();
+ ASSERT_LE(0u, particleIndex);
+ ASSERT_GT(particles->getVertices().size(), particleIndex);
+ contactingParticles.insert(particleIndex);
+
+ Vector3d particlesToMesh = meshPose * mesh->getCenter() - particles->getVertex(particleIndex).position;
+ if (!particlesToMesh.isZero())
+ {
+ // Check that each normal is pointing into the mesh
+ EXPECT_LT(0.0, contact->normal.dot(particlesToMesh));
+ }
+
+ // Check that the depth is sane
+ EXPECT_LT(0.0, contact->depth);
+ EXPECT_GE(particles->getRadius() + DistanceEpsilon, contact->depth);
+ }
+ EXPECT_EQ(numContactingParticles, contactingParticles.size());
+}
+
+TEST(TriangleMeshParticlesContactCalculationTests, CanConstruct)
+{
+ EXPECT_NO_THROW({ TriangleMeshParticlesContact calculation;});
+}
+
+TEST(TriangleMeshParticlesContactCalculationTests, CollisionTests)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ auto cube = std::make_shared<DataStructures::TriangleMeshPlain>();
+ cube->load("Geometry/Cube.ply");
+
+ {
+ SCOPED_TRACE("No intersection, no particles");
+ auto mesh = std::make_shared<MeshShape>(*cube);
+ auto meshPose = RigidTransform3d::Identity();
+ auto particles = std::make_shared<ParticlesShape>(0.5);
+ doCollisionTest(particles, mesh, meshPose, 0);
+ }
+ {
+ SCOPED_TRACE("No Intersection with particles");
+ auto mesh = std::make_shared<MeshShape>(*cube);
+ auto meshPose = RigidTransform3d::Identity();
+ auto particles = std::make_shared<ParticlesShape>(0.5);
+ particles->addVertex(ParticlesShape::VertexType(Vector3d::Constant(100.0)));
+ particles->addVertex(ParticlesShape::VertexType(Vector3d::UnitX() * 2.0));
+ particles->addVertex(ParticlesShape::VertexType(Vector3d::UnitY() * -3.0));
+ particles->addVertex(ParticlesShape::VertexType(Vector3d::UnitZ() * 4.0));
+ particles->update();
+ doCollisionTest(particles, mesh, meshPose, 0);
+ }
+ {
+ SCOPED_TRACE("Intersection with one particle");
+ auto mesh = std::make_shared<MeshShape>(*cube);
+ auto meshPose = RigidTransform3d::Identity();
+ auto particles = std::make_shared<ParticlesShape>(0.5);
+ particles->addVertex(ParticlesShape::VertexType(Vector3d::UnitX()));
+ particles->addVertex(ParticlesShape::VertexType(Vector3d::Constant(100.0)));
+ particles->addVertex(ParticlesShape::VertexType(Vector3d::UnitX() * 2.0));
+ particles->addVertex(ParticlesShape::VertexType(Vector3d::UnitY() * -3.0));
+ particles->addVertex(ParticlesShape::VertexType(Vector3d::UnitZ() * 4.0));
+ particles->update();
+ doCollisionTest(particles, mesh, meshPose, 1);
+ }
+ {
+ SCOPED_TRACE("No intersection with posed mesh");
+ auto mesh = std::make_shared<MeshShape>(*cube);
+ auto meshPose = makeRigidTransform(Eigen::AngleAxisd(M_PI_4, Vector3d::UnitZ()).matrix(), Vector3d::Zero());
+ auto particles = std::make_shared<ParticlesShape>(0.5);
+ particles->addVertex(ParticlesShape::VertexType(Vector3d::Ones() * 2.0));
+ particles->addVertex(ParticlesShape::VertexType(Vector3d::Ones() * -2.0));
+ particles->addVertex(ParticlesShape::VertexType(Vector3d(1.15, 1.15, 0.0)));
+ particles->update();
+ doCollisionTest(particles, mesh, meshPose, 0);
+ }
+ {
+ SCOPED_TRACE("Intersection with posed mesh");
+ auto mesh = std::make_shared<MeshShape>(*cube);
+ auto meshPose = makeRigidTransform(Eigen::AngleAxisd(M_PI_4, Vector3d::UnitZ()).matrix(),
+ Vector3d::UnitX() * 10.0);
+ auto particles = std::make_shared<ParticlesShape>(0.5);
+ particles->addVertex(ParticlesShape::VertexType(Vector3d::UnitX() * 11.6));
+ particles->addVertex(ParticlesShape::VertexType(Vector3d(10.0, 1.9, 0.0)));
+ particles->addVertex(ParticlesShape::VertexType(Vector3d::Zero()));
+ particles->update();
+ doCollisionTest(particles, mesh, meshPose, 2);
+ }
+}
+
+};
+};
diff --git a/SurgSim/Collision/UnitTests/TriangleMeshPlaneContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/TriangleMeshPlaneContactCalculationTests.cpp
index 79a77aa..0600ee7 100644
--- a/SurgSim/Collision/UnitTests/TriangleMeshPlaneContactCalculationTests.cpp
+++ b/SurgSim/Collision/UnitTests/TriangleMeshPlaneContactCalculationTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -13,8 +13,9 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "SurgSim/Collision/TriangleMeshPlaneDcdContact.h"
+#include "SurgSim/Collision/TriangleMeshPlaneContact.h"
#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/DataStructures/AabbTree.h"
#include "SurgSim/DataStructures/TriangleMesh.h"
#include "SurgSim/Math/Geometry.h"
#include "SurgSim/Math/Matrix.h"
@@ -103,15 +104,16 @@ void generateTriangleMeshPlaneContact(std::list<std::shared_ptr<Contact>>* expec
depth = -planeNormalGlobal.dot(vertex - pointOnPlane);
boxLocalVertex = calculateTriangleMeshVertex(expectedMeshIndicesInContacts[i],
- Quaterniond::Identity(), Vector3d::Zero());
+ 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));
+ expectedContacts->push_back(std::make_shared<Contact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, depth, 1.0,
+ Vector3d::Zero(), collisionNormal, penetrationPoint));
}
}
@@ -139,21 +141,23 @@ void doTriangleMeshPlaneTest(std::shared_ptr<SurgSim::Math::MeshShape> mesh,
if (expectedNumberOfContacts > 0)
{
generateTriangleMeshPlaneContact(&expectedContacts, expectedNumberOfContacts, expectedMeshIndicesInContacts,
- meshTrans, meshQuat, plane->getNormal(), plane->getD(), planeTrans, planeQuat);
+ meshTrans, meshQuat, plane->getNormal(), plane->getD(),
+ planeTrans, planeQuat);
}
// Perform collision detection.
- TriangleMeshPlaneDcdContact calcContact;
+ TriangleMeshPlaneContact 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();
+ // update the AABB tree
+ mesh->update();
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);
+ plane->getD(), &nearestPointOnPlane);
const double minDepth = -distanceMeshPlane - maxRadius;
const double maxDepth = -distanceMeshPlane + maxRadius;
diff --git a/SurgSim/Collision/UnitTests/TriangleMeshSurfaceMeshContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/TriangleMeshSurfaceMeshContactCalculationTests.cpp
new file mode 100644
index 0000000..3e37e50
--- /dev/null
+++ b/SurgSim/Collision/UnitTests/TriangleMeshSurfaceMeshContactCalculationTests.cpp
@@ -0,0 +1,380 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/TriangleMeshSurfaceMeshContact.h"
+#include "SurgSim/Collision/UnitTests/ContactCalculationTestsCommon.h"
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.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 doTriangleMeshSurfaceMeshTest(std::shared_ptr<MeshShape> meshA,
+ const RigidTransform3d& meshATransform,
+ std::shared_ptr<Math::SurfaceMeshShape> 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.
+ TriangleMeshSurfaceMeshContact calcContact;
+ std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(meshARep, meshBRep);
+ calcContact.calculateContact(pair);
+
+ contactsInfoEqualityTest(expectedContacts, pair->getContacts(), true);
+}
+
+TEST(TriangleMeshSurfaceMeshContactCalculationTests, NonintersectionTest)
+{
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::makeRotationQuaternion;
+
+ // Create a Mesh Cube
+ std::shared_ptr<TriangleMeshPlain> meshA = std::make_shared<TriangleMeshPlain>();
+ std::shared_ptr<TriangleMeshPlain> meshB = 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);
+ meshA->addVertex(v);
+ meshB->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);
+ meshA->addEdge(e);
+ meshB->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);
+ meshA->addTriangle(t);
+ if (i < cubeNumTriangles - 2)
+ {
+ meshB->addTriangle(t);
+ }
+ }
+
+ std::shared_ptr<SurgSim::Math::MeshShape> cubeMeshA = std::make_shared<SurgSim::Math::MeshShape>(*meshA);
+ std::shared_ptr<Math::SurfaceMeshShape> cubeMeshB = std::make_shared<Math::SurfaceMeshShape>(*meshB);
+
+ 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;
+
+ doTriangleMeshSurfaceMeshTest(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;
+
+ doTriangleMeshSurfaceMeshTest(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;
+
+ doTriangleMeshSurfaceMeshTest(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;
+
+ doTriangleMeshSurfaceMeshTest(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;
+
+ doTriangleMeshSurfaceMeshTest(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;
+
+ doTriangleMeshSurfaceMeshTest(cubeMeshA, cubeMeshATransform, cubeMeshB, cubeMeshBTransform, emptyContacts);
+ }
+}
+
+namespace
+{
+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(TriangleMeshSurfaceMeshContactCalculationTests, 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 first 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 zIncrement = 1.0 / (numTriangles + 1);
+ double zValue;
+ std::pair<Location, Location> expectedPenetrationPoints;
+ Vector3d expectedPoint0, expectedPoint1;
+ Vector3d expectedNormal, expectedContact(0, 0, 0);
+ for (int i = 0; i < numTriangles; i++)
+ {
+ zValue = static_cast<double>(i + 1) * zIncrement;
+ addNewTriangle(baseTriangles,
+ Vector3d(50, 50, zValue),
+ Vector3d(-50, 50, zValue),
+ Vector3d(0, -50, zValue));
+ expectedDepth = zValue;
+ if (expectedDepth >= 0.5)
+ {
+ expectedDepth = 1.0 - expectedDepth;
+ expectedNormal = pose.linear() * Vector3d(0, 0, -1);
+ expectedPoint0 = Vector3d(0, 0, 1.0);
+ expectedPoint1 = Vector3d(0, 0, zValue);
+ }
+ else
+ {
+ expectedNormal = pose.linear() * Vector3d(0, 0, 1);
+ expectedPoint0 = Vector3d(0, 0, 0.0);
+ expectedPoint1 = Vector3d(0, 0, zValue);
+ }
+ if (expectedDepth > 0.0)
+ {
+ expectedPenetrationPoints.first.rigidLocalPosition.setValue(expectedPoint0);
+ expectedPenetrationPoints.second.rigidLocalPosition.setValue(expectedPoint1);
+ SurgSim::DataStructures::IndexedLocalCoordinate triangleLocalPosition;
+ triangleLocalPosition.index = 0;
+ expectedPenetrationPoints.first.triangleMeshLocalCoordinate.setValue(triangleLocalPosition);
+ triangleLocalPosition.index = i;
+ expectedPenetrationPoints.second.triangleMeshLocalCoordinate.setValue(triangleLocalPosition);
+ auto contact = std::make_shared<TriangleContact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, expectedDepth,
+ 1.0, expectedContact, expectedNormal, expectedPenetrationPoints);
+ contact->firstVertices = intersectingTriangle->getTrianglePositions(0);
+ contact->secondVertices = baseTriangles->getTrianglePositions(baseTriangles->getNumTriangles() - 1);
+ expectedContacts.push_back(contact);
+ }
+ }
+ auto baseMesh = std::make_shared<Math::SurfaceMeshShape>(*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
+ //
+
+ doTriangleMeshSurfaceMeshTest(triangleMesh, pose, baseMesh, pose, expectedContacts);
+ }
+}
+
+
+}; // namespace Collision
+}; // namespace Surgsim
diff --git a/SurgSim/Collision/UnitTests/TriangleMeshTriangleMeshContactCalculationTests.cpp b/SurgSim/Collision/UnitTests/TriangleMeshTriangleMeshContactCalculationTests.cpp
index 45ba02c..334a235 100644
--- a/SurgSim/Collision/UnitTests/TriangleMeshTriangleMeshContactCalculationTests.cpp
+++ b/SurgSim/Collision/UnitTests/TriangleMeshTriangleMeshContactCalculationTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -13,11 +13,10 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "SurgSim/Collision/TriangleMeshTriangleMeshDcdContact.h"
+#include "SurgSim/Collision/TriangleMeshTriangleMeshContact.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"
@@ -87,7 +86,7 @@ void doTriangleMeshTriangleMeshTest(std::shared_ptr<MeshShape> meshA,
meshBRep->setLocalPose(meshBTransform);
// Perform collision detection.
- TriangleMeshTriangleMeshDcdContact calcContact;
+ TriangleMeshTriangleMeshContact calcContact;
std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(meshARep, meshBRep);
calcContact.calculateContact(pair);
@@ -215,7 +214,8 @@ TEST(TriangleMeshTriangleMeshContactCalculationTests, NonintersectionTest)
}
}
-
+namespace
+{
void addNewTriangle(std::shared_ptr<TriangleMeshPlain> mesh,
SurgSim::Math::Vector3d point0, SurgSim::Math::Vector3d point1, SurgSim::Math::Vector3d point2)
{
@@ -256,6 +256,7 @@ void addNewTriangle(std::shared_ptr<TriangleMeshPlain> mesh,
TriangleMeshPlain::TriangleType meshTriangle(triangle);
mesh->addTriangle(meshTriangle);
}
+}
TEST(TriangleMeshTriangleMeshContactCalculationTests, IntersectionTest)
{
@@ -315,11 +316,12 @@ TEST(TriangleMeshTriangleMeshContactCalculationTests, IntersectionTest)
expectedPenetrationPoints.second.rigidLocalPosition.setValue(expectedPoint1);
SurgSim::DataStructures::IndexedLocalCoordinate triangleLocalPosition;
triangleLocalPosition.index = i;
- expectedPenetrationPoints.first.meshLocalCoordinate.setValue(triangleLocalPosition);
+ expectedPenetrationPoints.first.triangleMeshLocalCoordinate.setValue(triangleLocalPosition);
triangleLocalPosition.index = 0;
- expectedPenetrationPoints.second.meshLocalCoordinate.setValue(triangleLocalPosition);
- auto contact = std::make_shared<TriangleContact>(expectedDepth, expectedContact, expectedNormal,
- expectedPenetrationPoints);
+ expectedPenetrationPoints.second.triangleMeshLocalCoordinate.setValue(triangleLocalPosition);
+ auto contact = std::make_shared<TriangleContact>(
+ COLLISION_DETECTION_TYPE_DISCRETE, expectedDepth,
+ 1.0, expectedContact, expectedNormal, expectedPenetrationPoints);
contact->firstVertices = baseTriangles->getTrianglePositions(baseTriangles->getNumTriangles() - 1);
contact->secondVertices = intersectingTriangle->getTrianglePositions(0);
for (size_t i = 0; i < 3; ++i)
@@ -404,6 +406,7 @@ TEST(TriangleMeshTriangleMeshContactCalculationTests, IntersectionTestAtIdentica
std::list<std::shared_ptr<Contact>> expectedContacts;
double expectedDepth;
+ double expectedTime;
double interval = 1.0 / static_cast<double>(numTriangles + 1);
double coordinate;
std::pair<Location, Location> expectedPenetrationPoints;
@@ -418,16 +421,19 @@ TEST(TriangleMeshTriangleMeshContactCalculationTests, IntersectionTestAtIdentica
Vector3d(0.5, 1.0 - coordinate, coordinate),
Vector3d(-e, 1.0 - coordinate, coordinate));
expectedDepth = coordinate;
+ expectedTime = 1.0;
{
expectedPenetrationPoints.first.rigidLocalPosition.setValue(Vector3d(0, 0, coordinate));
expectedPenetrationPoints.second.rigidLocalPosition.setValue(Vector3d(0, 0, 0));
triangleLocalPosition.index = i;
- expectedPenetrationPoints.first.meshLocalCoordinate.setValue(triangleLocalPosition);
+ expectedPenetrationPoints.first.triangleMeshLocalCoordinate.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);
+ expectedPenetrationPoints.second.triangleMeshLocalCoordinate.setValue(triangleLocalPosition);
+ auto contact = std::make_shared<TriangleContact>(
+ COLLISION_DETECTION_TYPE_DISCRETE,
+ expectedDepth, expectedTime, 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)
@@ -441,12 +447,14 @@ TEST(TriangleMeshTriangleMeshContactCalculationTests, IntersectionTestAtIdentica
expectedPenetrationPoints.first.rigidLocalPosition.setValue(Vector3d(0, -coordinate, coordinate));
expectedPenetrationPoints.second.rigidLocalPosition.setValue(Vector3d(0, 0, coordinate));
triangleLocalPosition.index = i;
- expectedPenetrationPoints.first.meshLocalCoordinate.setValue(triangleLocalPosition);
+ expectedPenetrationPoints.first.triangleMeshLocalCoordinate.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);
+ expectedPenetrationPoints.second.triangleMeshLocalCoordinate.setValue(triangleLocalPosition);
+ auto contact = std::make_shared<TriangleContact>(
+ COLLISION_DETECTION_TYPE_DISCRETE,
+ expectedDepth, expectedTime, 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)
@@ -470,7 +478,7 @@ TEST(TriangleMeshTriangleMeshContactCalculationTests, IntersectionTestAtIdentica
meshBRep->setLocalPose(pose);
// Perform collision detection.
- TriangleMeshTriangleMeshDcdContact calcContact;
+ TriangleMeshTriangleMeshContact calcContact;
std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(meshARep, meshBRep);
calcContact.calculateContact(pair);
diff --git a/SurgSim/Collision/UnitTests/config.txt.in b/SurgSim/Collision/UnitTests/config.txt.in
index 9c1cf05..8dc3922 100644
--- a/SurgSim/Collision/UnitTests/config.txt.in
+++ b/SurgSim/Collision/UnitTests/config.txt.in
@@ -1 +1 @@
-${CMAKE_CURRENT_BINARY_DIR}/Data/
\ No newline at end of file
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data/
diff --git a/SurgSim/DataStructures/AabbTree.cpp b/SurgSim/DataStructures/AabbTree.cpp
index 52e4477..766d506 100644
--- a/SurgSim/DataStructures/AabbTree.cpp
+++ b/SurgSim/DataStructures/AabbTree.cpp
@@ -47,6 +47,20 @@ void AabbTree::add(const SurgSim::Math::Aabbd& aabb, size_t objectId)
m_typedRoot->addData(aabb, objectId, m_maxObjectsPerNode);
}
+void AabbTree::set(const std::list<AabbTreeData::Item>& items)
+{
+ m_typedRoot = std::make_shared<AabbTreeNode>();
+ setRoot(m_typedRoot);
+ m_typedRoot->setData(items, m_maxObjectsPerNode);
+}
+
+void AabbTree::set(std::list<AabbTreeData::Item>&& items)
+{
+ m_typedRoot = std::make_shared<AabbTreeNode>();
+ setRoot(m_typedRoot);
+ m_typedRoot->setData(std::move(items), m_maxObjectsPerNode);
+}
+
size_t AabbTree::getMaxObjectsPerNode() const
{
return m_maxObjectsPerNode;
diff --git a/SurgSim/DataStructures/AabbTree.h b/SurgSim/DataStructures/AabbTree.h
index 4eb85a8..3accb2c 100644
--- a/SurgSim/DataStructures/AabbTree.h
+++ b/SurgSim/DataStructures/AabbTree.h
@@ -19,7 +19,7 @@
#include <list>
#include "SurgSim/DataStructures/Tree.h"
-
+#include "SurgSim/DataStructures/AabbTreeData.h"
#include "SurgSim/Math/Aabb.h"
namespace SurgSim
@@ -50,13 +50,21 @@ public:
/// \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);
+ /// Create the tree from a list of tree items, all the tree information will be deleted
+ /// \param items list of items to insert into the tree
+ void set(const std::list<AabbTreeData::Item>& items);
+
+ /// Create the tree from a list of tree items, all the tree information will be deleted
+ /// \param items rvalue reference to list of items to insert into the tree
+ void set(std::list<AabbTreeData::Item>&& items);
+
+ /// \return the AABB for the tree
const SurgSim::Math::Aabbd& getAabb() const;
/// Type indicating a relationship between two AabbTreeNodes
@@ -64,7 +72,7 @@ public:
/// 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
+ /// \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.
diff --git a/SurgSim/DataStructures/AabbTreeData.cpp b/SurgSim/DataStructures/AabbTreeData.cpp
index 5a0fe99..f94d7d5 100644
--- a/SurgSim/DataStructures/AabbTreeData.cpp
+++ b/SurgSim/DataStructures/AabbTreeData.cpp
@@ -14,6 +14,7 @@
// limitations under the License.
#include "SurgSim/DataStructures/AabbTreeData.h"
+#include "SurgSim/Framework/Assert.h"
#include <algorithm>
@@ -27,6 +28,16 @@ AabbTreeData::AabbTreeData()
}
+AabbTreeData::AabbTreeData(const std::list<Item>& data) : m_data(data)
+{
+ recalculateAabb();
+}
+
+AabbTreeData::AabbTreeData(std::list<Item>&& data) : m_data(std::move(data))
+{
+ recalculateAabb();
+}
+
AabbTreeData::~AabbTreeData()
{
@@ -89,16 +100,25 @@ std::shared_ptr<AabbTreeData> AabbTreeData::takeLargerElements()
int axis;
m_aabb.sizes().maxCoeff(&axis);
double centerValue = m_aabb.center()(axis);
+ // HS-2015-03-20
+ // The new left and right aabb extents can probably be calculated here
+ // Only the separating axis extents would change the other two axis are unaffected
+ // #performance
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);
+ // In some cases all pieces may end up on one or the other side of the split, make this a noop
+ if (split != m_data.begin() && split != m_data.end())
+ {
+ result->m_data.splice(result->m_data.end(), m_data, split, m_data.end());
+ recalculateAabb();
+ result->recalculateAabb();
+ }
+
+ return result;
}
void AabbTreeData::recalculateAabb()
diff --git a/SurgSim/DataStructures/AabbTreeData.h b/SurgSim/DataStructures/AabbTreeData.h
index 8baf14e..3083f38 100644
--- a/SurgSim/DataStructures/AabbTreeData.h
+++ b/SurgSim/DataStructures/AabbTreeData.h
@@ -35,16 +35,23 @@ class AabbTreeData : public TreeData
{
public:
+ typedef std::pair<SurgSim::Math::Aabbd, size_t> Item;
+
/// Constructor
AabbTreeData();
/// Copy Constructor
AabbTreeData(const AabbTreeData& data);
+ /// Constructor with list of items
+ explicit AabbTreeData(const std::list<Item>& data);
+
+ /// Constructor with moveable list of items
+ explicit AabbTreeData(std::list<Item>&& 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
@@ -82,7 +89,7 @@ private:
/// Recalculate the aabb of this class, in case items where updated
void recalculateAabb();
- virtual bool isEqual(const TreeData* data) const override;
+ bool isEqual(const TreeData* data) const override;
/// AABB containg all items
SurgSim::Math::Aabbd m_aabb;
diff --git a/SurgSim/DataStructures/AabbTreeIntersectionVisitor.h b/SurgSim/DataStructures/AabbTreeIntersectionVisitor.h
index eae01c5..acad561 100644
--- a/SurgSim/DataStructures/AabbTreeIntersectionVisitor.h
+++ b/SurgSim/DataStructures/AabbTreeIntersectionVisitor.h
@@ -41,9 +41,9 @@ public:
/// Destructor
virtual ~AabbTreeIntersectionVisitor();
- virtual bool handle(TreeNode* node) override;
+ bool handle(TreeNode* node) override;
- virtual bool handle(AabbTreeNode* node) override;
+ bool handle(AabbTreeNode* node) override;
/// \return true if the visitor has found intersections
bool hasIntersections() const;
diff --git a/SurgSim/DataStructures/AabbTreeNode.cpp b/SurgSim/DataStructures/AabbTreeNode.cpp
index c9ff71f..3e04bd5 100644
--- a/SurgSim/DataStructures/AabbTreeNode.cpp
+++ b/SurgSim/DataStructures/AabbTreeNode.cpp
@@ -16,7 +16,7 @@
#include "SurgSim/DataStructures/AabbTreeNode.h"
#include "SurgSim/DataStructures/AabbTreeData.h"
-#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Log.h"
namespace SurgSim
{
@@ -32,19 +32,42 @@ AabbTreeNode::~AabbTreeNode()
}
-void AabbTreeNode::splitNode()
+void AabbTreeNode::splitNode(size_t maxNodeData)
{
auto leftData = std::static_pointer_cast<AabbTreeData>(getData());
- if (leftData->getSize() > 0)
+ if (leftData->getSize() > maxNodeData)
{
std::shared_ptr<AabbTreeData> rightData = leftData->takeLargerElements();
+ std::shared_ptr<AabbTreeNode> leftChild;
+ std::shared_ptr<AabbTreeNode> rightChild;
+
+ // Early exit, takeLargerElements() may not be able to split the list, in this
+ // case we abort the splitNode.
+ if (leftData->getSize() == 0 || rightData->getSize() == 0)
+ {
+ auto count = std::max(leftData->getSize(), rightData->getSize());
+
+ if (maxNodeData > 0 && count > 3 * maxNodeData)
+ {
+ SURGSIM_LOG_ONCE(Framework::Logger::getLogger("DataStructures/AabbTreeNode"), WARNING)
+ << "The aabb tree build process encountered some items that could not be split anymore "
+ << "this may cause suboptimal behavior when querying the aabb tree.";
+ }
+ return;
+ }
+
if (getNumChildren() != 2)
{
- std::shared_ptr<TreeNode> child = std::make_shared<AabbTreeNode>();
- addChild(std::move(child));
+ leftChild = std::make_shared<AabbTreeNode>();
+ addChild(leftChild);
- child = std::make_shared<AabbTreeNode>();
- addChild(std::move(child));
+ rightChild = std::make_shared<AabbTreeNode>();
+ addChild(rightChild);
+ }
+ else
+ {
+ leftChild = std::static_pointer_cast<AabbTreeNode>(getChild(0));
+ rightChild = std::static_pointer_cast<AabbTreeNode>(getChild(1));
}
// Update the local aabb
@@ -53,9 +76,22 @@ void AabbTreeNode::splitNode()
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);
+
+ size_t leftCount = leftData->getSize();
+ size_t rightCount = rightData->getSize();
+
+ leftChild->setData(std::move(leftData));
+ if (maxNodeData > 0 && leftCount > maxNodeData)
+ {
+ leftChild->splitNode(maxNodeData);
+ }
+
+ rightChild->setData(std::move(rightData));
+ if (maxNodeData > 0 && rightCount > maxNodeData)
+ {
+ rightChild->splitNode(maxNodeData);
+ }
}
}
@@ -98,6 +134,26 @@ void AabbTreeNode::addData(const SurgSim::Math::Aabbd& aabb, size_t id, size_t m
}
}
+void AabbTreeNode::setData(const std::list<AabbTreeData::Item>& items, size_t maxNodeData)
+{
+ SURGSIM_ASSERT(getNumChildren() == 0) << "Can't call setData on a node that already has nodes";
+ SURGSIM_ASSERT(getData() == nullptr) << "Can't call setData on a node that already has data.";
+
+ auto data = std::make_shared<AabbTreeData>(items);
+ setData(data);
+ splitNode(maxNodeData);
+}
+
+void AabbTreeNode::setData(std::list<AabbTreeData::Item>&& items, size_t maxNodeData)
+{
+ SURGSIM_ASSERT(getNumChildren() == 0) << "Can't call setData on a node that already has nodes";
+ SURGSIM_ASSERT(getData() == nullptr) << "Can't call setData on a node that already has data.";
+
+ auto data = std::make_shared<AabbTreeData>(std::move(items));
+ setData(data);
+ splitNode(maxNodeData);
+}
+
bool AabbTreeNode::doAccept(TreeVisitor* visitor)
{
return visitor->handle(this);
diff --git a/SurgSim/DataStructures/AabbTreeNode.h b/SurgSim/DataStructures/AabbTreeNode.h
index eb92679..339c756 100644
--- a/SurgSim/DataStructures/AabbTreeNode.h
+++ b/SurgSim/DataStructures/AabbTreeNode.h
@@ -31,6 +31,8 @@ class AabbTreeNode : public TreeNode
{
public:
+ using TreeNode::setData;
+
/// Constructor
AabbTreeNode();
@@ -39,7 +41,12 @@ public:
/// 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();
+ /// \param maxNodeData number of maximum items of data in this node, if more, the node will split,
+ /// if 0 the node will not be split until it is no longer possible, the structure will
+ /// approach a binary tree.
+ /// \note Sometimes the current mechanism to split the list of AABBs along the longest axis will fail to actually
+ /// split the list, if the size of the list is greater than 3 * maxNodeData a warning will be generated
+ void splitNode(size_t maxNodeData = 0);
/// 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.
@@ -50,8 +57,26 @@ public:
/// \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);
+ /// if 0 the node will not be split until it is no longer possible, the structure will
+ /// approach a binary tree.
+ void addData(const SurgSim::Math::Aabbd& aabb, size_t id, size_t maxNodeData = 0);
+
+ /// Set the data on this node, the node needs to be empty and not have any children for this to work.
+ /// Nodes will be split until there are only maxNodeData elements in one node
+ /// \param items list of AabbTreeData::Item elements that should populate the tree
+ /// \param maxNodeData number of maximum items of data in this node, if more, the node will split,
+ /// if 0 the node will not be split until it is no longer possible, the structure will
+ /// approach a binary tree.
+ void setData(const std::list<AabbTreeData::Item>& items, size_t maxNodeData = 0);
+
+ /// Set the data on this node, rvalue reference version,
+ /// the node needs to be empty and not have any children for this to work.
+ /// Nodes will be split until there are only maxNodeData elements in one node
+ /// \param items list of AabbTreeData::Item elements that should populate the tree
+ /// \param maxNodeData number of maximum items of data in this node, if more, the node will split,
+ /// if 0 the node will not be split until it is no longer possible, the structure will
+ /// approach a binary tree.
+ void setData(std::list<AabbTreeData::Item>&& items, size_t maxNodeData);
/// Fetch a list of items that have AABBs intersecting with the given AABB.
/// \param aabb The bounding box for the query.
@@ -60,7 +85,7 @@ public:
protected:
- virtual bool doAccept(TreeVisitor* visitor) override;
+ bool doAccept(TreeVisitor* visitor) override;
private:
diff --git a/SurgSim/DataStructures/BufferedValue-inl.h b/SurgSim/DataStructures/BufferedValue-inl.h
index bde013d..ec14769 100644
--- a/SurgSim/DataStructures/BufferedValue-inl.h
+++ b/SurgSim/DataStructures/BufferedValue-inl.h
@@ -42,8 +42,11 @@ BufferedValue<T>::~BufferedValue()
template <class T>
void BufferedValue<T>::publish()
{
- UniqueLock lock(m_mutex);
- m_safeValue = std::make_shared<const T>(m_value);
+ auto newSafeValue = std::make_shared<const T>(m_value);
+ {
+ UniqueLock lock(m_mutex);
+ std::swap(newSafeValue, m_safeValue);
+ }
}
template <class T>
diff --git a/SurgSim/DataStructures/CMakeLists.txt b/SurgSim/DataStructures/CMakeLists.txt
index 7165ff2..8a58251 100644
--- a/SurgSim/DataStructures/CMakeLists.txt
+++ b/SurgSim/DataStructures/CMakeLists.txt
@@ -1,5 +1,5 @@
# This file is a part of the OpenSurgSim project.
-# Copyright 2012-2013, SimQuest Solutions Inc.
+# Copyright 2012-2016, SimQuest Solutions Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -25,13 +25,14 @@ set(SURGSIM_DATA_STRUCTURES_SOURCES
IndexDirectory.cpp
IndexedLocalCoordinate.cpp
OctreeNode.cpp
+ OctreeNodePlyReaderDelegate.cpp
ply.c
PlyReader.cpp
+ SegmentMesh.cpp
Tree.cpp
TreeData.cpp
TreeNode.cpp
TriangleMesh.cpp
- TriangleMeshUtilities.cpp
)
set(SURGSIM_DATA_STRUCTURES_HEADERS
@@ -47,8 +48,16 @@ set(SURGSIM_DATA_STRUCTURES_HEADERS
DataStructuresConvert.h
DataStructuresConvert-inl.h
EmptyData.h
+ Grid.h
+ Grid-inl.h
+ Groups.h
+ Groups-inl.h
Image.h
Image-inl.h
+ ImageBase.h
+ ImageBase-inl.h
+ ImageMap.h
+ ImageMap-inl.h
IndexDirectory.h
IndexedLocalCoordinate.h
Location.h
@@ -58,12 +67,18 @@ set(SURGSIM_DATA_STRUCTURES_HEADERS
NamedDataBuilder.h
NamedVariantData.h
NamedVariantData-inl.h
+ NormalData.h
OctreeNode.h
OctreeNode-inl.h
+ OctreeNodePlyReaderDelegate.h
+ OctreeNodePlyReaderDelegate-inl.h
OptionalValue.h
ply.h
PlyReader.h
PlyReaderDelegate.h
+ SegmentEmptyData.h
+ SegmentMesh.h
+ SegmentMesh-inl.h
TetrahedronMesh.h
TetrahedronMesh-inl.h
Tree.h
@@ -72,21 +87,18 @@ set(SURGSIM_DATA_STRUCTURES_HEADERS
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
+ Vertices-inl.h
)
+surgsim_create_library_header(DataStructures.h "${SURGSIM_DATA_STRUCTURES_HEADERS}")
surgsim_add_library(
SurgSimDataStructures
"${SURGSIM_DATA_STRUCTURES_SOURCES}"
"${SURGSIM_DATA_STRUCTURES_HEADERS}"
- "SurgSim/DataStructures"
)
set(LIBS
diff --git a/SurgSim/DataStructures/DataGroup.cpp b/SurgSim/DataStructures/DataGroup.cpp
index 15f6ca8..ec07516 100644
--- a/SurgSim/DataStructures/DataGroup.cpp
+++ b/SurgSim/DataStructures/DataGroup.cpp
@@ -32,6 +32,7 @@ DataGroup::DataGroup(const DataGroup& dataGroup) :
m_integers(dataGroup.m_integers),
m_booleans(dataGroup.m_booleans),
m_strings(dataGroup.m_strings),
+ m_images(dataGroup.m_images),
m_customData(dataGroup.m_customData)
{
}
@@ -45,6 +46,7 @@ DataGroup& DataGroup::operator=(const DataGroup& dataGroup)
m_integers = dataGroup.m_integers;
m_booleans = dataGroup.m_booleans;
m_strings = dataGroup.m_strings;
+ m_images = dataGroup.m_images;
m_customData = dataGroup.m_customData;
return *this;
@@ -59,6 +61,7 @@ DataGroup& DataGroup::operator=(DataGroup&& dataGroup)
m_integers = std::move(dataGroup.m_integers);
m_booleans = std::move(dataGroup.m_booleans);
m_strings = std::move(dataGroup.m_strings);
+ m_images = std::move(dataGroup.m_images);
m_customData = std::move(dataGroup.m_customData);
return *this;
@@ -135,6 +138,16 @@ const NamedData<DataGroup::StringType>& DataGroup::strings() const
return m_strings;
}
+NamedData<DataGroup::ImageType>& DataGroup::images()
+{
+ return m_images;
+}
+
+const NamedData<DataGroup::ImageType>& DataGroup::images() const
+{
+ return m_images;
+}
+
NamedVariantData& DataGroup::customData()
{
return m_customData;
@@ -154,6 +167,7 @@ void DataGroup::resetAll()
m_integers.resetAll();
m_booleans.resetAll();
m_strings.resetAll();
+ m_images.resetAll();
m_customData.resetAll();
}
@@ -166,6 +180,7 @@ bool DataGroup::isEmpty() const
m_integers.isValid() ||
m_booleans.isValid() ||
m_strings.isValid() ||
+ m_images.isValid() ||
m_customData.isValid());
}
diff --git a/SurgSim/DataStructures/DataGroup.h b/SurgSim/DataStructures/DataGroup.h
index a8be01c..861200f 100644
--- a/SurgSim/DataStructures/DataGroup.h
+++ b/SurgSim/DataStructures/DataGroup.h
@@ -18,6 +18,7 @@
#include <Eigen/Core>
+#include "SurgSim/DataStructures/Image.h"
#include "SurgSim/DataStructures/NamedData.h"
#include "SurgSim/DataStructures/NamedVariantData.h"
#include "SurgSim/Math/RigidTransform.h"
@@ -31,15 +32,16 @@ namespace DataStructures
/// A collection of \ref NamedData objects.
///
/// A DataGroup object contains a NamedData for each of several predefined types:
+/// \li \em Booleans contain a Boolean logic value (true or false).
+/// \li \em Images contain float images.
+/// \li \em Integers contain an integer value.
/// \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 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 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
@@ -80,6 +82,8 @@ public:
typedef bool BooleanType;
/// The type used for strings.
typedef std::string StringType;
+ /// The type used for images.
+ typedef Image<float> ImageType;
/// Construct an empty object, with no associated names and indices yet.
DataGroup();
@@ -182,6 +186,14 @@ public:
/// \return the read-only string data.
const NamedData<StringType>& strings() const;
+ /// Return the image data structure.
+ /// \return the mutable iamge data.
+ NamedData<ImageType>& images();
+
+ /// Return the image data structure.
+ /// \return the read-only images data.
+ const NamedData<ImageType>& images() const;
+
/// Return the custom data structure.
/// \return the mutable data.
NamedVariantData& customData();
@@ -219,6 +231,9 @@ private:
/// The string values.
NamedData<StringType> m_strings;
+ /// The string values.
+ NamedData<ImageType> m_images;
+
/// The custom data values.
NamedVariantData m_customData;
};
diff --git a/SurgSim/DataStructures/DataGroupBuilder.cpp b/SurgSim/DataStructures/DataGroupBuilder.cpp
index 2a86267..1abc932 100644
--- a/SurgSim/DataStructures/DataGroupBuilder.cpp
+++ b/SurgSim/DataStructures/DataGroupBuilder.cpp
@@ -35,6 +35,7 @@ DataGroup DataGroupBuilder::createData() const
data.integers() = integers().createData();
data.booleans() = booleans().createData();
data.strings() = strings().createData();
+ data.images() = images().createData();
data.customData() = customData().createData();
return data;
}
@@ -114,6 +115,16 @@ const NamedDataBuilder<DataGroupBuilder::StringType>& DataGroupBuilder::strings(
return m_strings;
}
+NamedDataBuilder<DataGroupBuilder::ImageType>& DataGroupBuilder::images()
+{
+ return m_images;
+}
+
+const NamedDataBuilder<DataGroupBuilder::ImageType>& DataGroupBuilder::images() const
+{
+ return m_images;
+}
+
NamedVariantDataBuilder& DataGroupBuilder::customData()
{
return m_customData;
@@ -159,12 +170,16 @@ void DataGroupBuilder::addString(const std::string& name)
strings().addEntry(name);
}
+void DataGroupBuilder::addImage(const std::string& name)
+{
+ images().addEntry(name);
+}
+
void DataGroupBuilder::addCustom(const std::string& name)
{
customData().addEntry(name);
}
-
void DataGroupBuilder::addEntriesFrom(const DataGroupBuilder& builder)
{
poses().addEntriesFrom(builder.poses());
@@ -174,6 +189,7 @@ void DataGroupBuilder::addEntriesFrom(const DataGroupBuilder& builder)
integers().addEntriesFrom(builder.integers());
booleans().addEntriesFrom(builder.booleans());
strings().addEntriesFrom(builder.strings());
+ images().addEntriesFrom(builder.images());
customData().addEntriesFrom(builder.customData());
}
@@ -186,6 +202,7 @@ void DataGroupBuilder::addEntriesFrom(const DataGroup& data)
integers().addEntriesFrom(data.integers());
booleans().addEntriesFrom(data.booleans());
strings().addEntriesFrom(data.strings());
+ images().addEntriesFrom(data.images());
customData().addEntriesFrom(data.customData());
}
diff --git a/SurgSim/DataStructures/DataGroupBuilder.h b/SurgSim/DataStructures/DataGroupBuilder.h
index 26bf489..d0e25c4 100644
--- a/SurgSim/DataStructures/DataGroupBuilder.h
+++ b/SurgSim/DataStructures/DataGroupBuilder.h
@@ -52,6 +52,8 @@ public:
typedef DataGroup::BooleanType BooleanType;
/// The type used for strings.
typedef DataGroup::StringType StringType;
+ /// The type used for images.
+ typedef Image<float> ImageType;
/// Constructs an empty builder object.
DataGroupBuilder();
@@ -122,6 +124,14 @@ public:
/// \return a read-only reference to the sub-object that contains string value entries.
const NamedDataBuilder<StringType>& strings() const;
+ /// Provides access to the image value entries.
+ /// \return a writable reference to the sub-object that contains image value entries.
+ NamedDataBuilder<ImageType>& images();
+
+ /// Provides access to the image value entries.
+ /// \return a read-only reference to the sub-object that contains image value entries.
+ const NamedDataBuilder<ImageType>& images() const;
+
/// Provides access to the custom data entries.
/// \return a writable reference to the sub-object that contains custom data entries.
NamedVariantDataBuilder& customData();
@@ -158,6 +168,10 @@ public:
/// Identical to <code>%strings().addEntry(name)</code>.
void addString(const std::string& name);
+ /// A shortcut for adding a named image entry.
+ /// Identical to <code>%images().addEntry(name)</code>.
+ void addImage(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);
@@ -196,6 +210,9 @@ private:
/// The subsidiary builder used for string values.
NamedDataBuilder<StringType> m_strings;
+ /// The subsidiary builder used for image values.
+ NamedDataBuilder<ImageType> m_images;
+
/// The subsidiary builder used for custom data.
NamedVariantDataBuilder m_customData;
};
diff --git a/SurgSim/DataStructures/DataGroupCopier.cpp b/SurgSim/DataStructures/DataGroupCopier.cpp
index d9a4ca5..bd115ff 100644
--- a/SurgSim/DataStructures/DataGroupCopier.cpp
+++ b/SurgSim/DataStructures/DataGroupCopier.cpp
@@ -25,35 +25,32 @@ namespace SurgSim
namespace DataStructures
{
-DataGroupCopier::DataGroupCopier(const DataGroup& source, DataGroup& target) :
- m_source(source),
- m_target(target)
+DataGroupCopier::DataGroupCopier(const DataGroup& source, DataGroup* target)
{
- findMap();
+ SURGSIM_ASSERT(target != nullptr) << "Target is a nullptr";
+ m_map[0] = findMap(source.poses().getDirectory(), target->poses().getDirectory());
+ m_map[1] = findMap(source.vectors().getDirectory(), target->vectors().getDirectory());
+ m_map[2] = findMap(source.matrices().getDirectory(), target->matrices().getDirectory());
+ m_map[3] = findMap(source.scalars().getDirectory(), target->scalars().getDirectory());
+ m_map[4] = findMap(source.integers().getDirectory(), target->integers().getDirectory());
+ m_map[5] = findMap(source.booleans().getDirectory(), target->booleans().getDirectory());
+ m_map[6] = findMap(source.strings().getDirectory(), target->strings().getDirectory());
+ m_map[7] = findMap(source.images().getDirectory(), target->images().getDirectory());
+ m_map[8] = findMap(source.customData().getDirectory(), target->customData().getDirectory());
}
-void DataGroupCopier::copy()
+void DataGroupCopier::copy(const DataGroup& source, DataGroup* target)
{
- 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());
+ SURGSIM_ASSERT(target != nullptr) << "Target is a nullptr";
+ target->poses().copy(source.poses(), m_map[0]);
+ target->vectors().copy(source.vectors(), m_map[1]);
+ target->matrices().copy(source.matrices(), m_map[2]);
+ target->scalars().copy(source.scalars(), m_map[3]);
+ target->integers().copy(source.integers(), m_map[4]);
+ target->booleans().copy(source.booleans(), m_map[5]);
+ target->strings().copy(source.strings(), m_map[6]);
+ target->images().copy(source.images(), m_map[7]);
+ target->customData().copy(source.customData(), m_map[8]);
}
NamedDataCopyMap DataGroupCopier::findMap(std::shared_ptr<const IndexDirectory> source,
diff --git a/SurgSim/DataStructures/DataGroupCopier.h b/SurgSim/DataStructures/DataGroupCopier.h
index f1199b4..5a03c6a 100644
--- a/SurgSim/DataStructures/DataGroupCopier.h
+++ b/SurgSim/DataStructures/DataGroupCopier.h
@@ -30,7 +30,7 @@ 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;
+typedef std::array<NamedDataCopyMap, 9> DataGroupCopyMap;
/// A class that assists in copying from one DataGroup to another, when assignment is not possible.
/// \sa SurgSim::DataStructures::DataGroup
@@ -40,15 +40,16 @@ public:
/// Construct a copier.
/// \param source The source DataGroup.
/// \param target The target DataGroup.
- DataGroupCopier(const DataGroup& source, DataGroup& target);
+ 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();
+ /// The source and target IndexDirectories are assumed to be the same as the source and target
+ /// used in the constructor.
+ /// \param source The source DataGroup.
+ /// \param target The target DataGroup.
+ void copy(const DataGroup& source, DataGroup* target);
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.
@@ -56,12 +57,6 @@ private:
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;
};
diff --git a/SurgSim/DataStructures/DataStructuresConvert-inl.h b/SurgSim/DataStructures/DataStructuresConvert-inl.h
index 1d59228..dee9cc2 100644
--- a/SurgSim/DataStructures/DataStructuresConvert-inl.h
+++ b/SurgSim/DataStructures/DataStructuresConvert-inl.h
@@ -35,7 +35,7 @@ const std::string valueName = "Value";
template <class T>
YAML::Node YAML::convert<SurgSim::DataStructures::OptionalValue<T>>::encode(
- const SurgSim::DataStructures::OptionalValue<T>& rhs)
+ const SurgSim::DataStructures::OptionalValue<T>& rhs)
{
Node node;
node[SurgSim::DataStructures::Convert::hasValueName] = rhs.hasValue();
@@ -52,14 +52,34 @@ YAML::Node YAML::convert<SurgSim::DataStructures::OptionalValue<T>>::encode(
template <class T>
bool YAML::convert<SurgSim::DataStructures::OptionalValue<T>>::decode(
- const Node& node, SurgSim::DataStructures::OptionalValue<T>& rhs)
+ const Node& node, SurgSim::DataStructures::OptionalValue<T>& rhs) //NOLINT
{
bool result = true;
- if (node[SurgSim::DataStructures::Convert::hasValueName].as<bool>())
+ if (node.IsMap())
+ {
+ 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();
+ }
+ }
+ else if (node.IsScalar())
{
try
{
- rhs.setValue(node[SurgSim::DataStructures::Convert::valueName].as<T>());
+ rhs.setValue(node.as<T>());
}
catch (YAML::RepresentationException)
{
@@ -68,10 +88,6 @@ bool YAML::convert<SurgSim::DataStructures::OptionalValue<T>>::decode(
SURGSIM_LOG(logger, WARNING) << "Bad conversion";
}
}
- else
- {
- rhs.invalidate();
- }
return result;
}
@@ -87,7 +103,7 @@ YAML::Node YAML::convert<std::array<T, N>>::encode(const std::array<T, N>& rhs)
}
template <class T, size_t N>
-bool YAML::convert<std::array<T, N>>::decode(const Node& node, std::array<T, N>& rhs)
+bool YAML::convert<std::array<T, N>>::decode(const Node& node, std::array<T, N>& rhs) //NOLINT
{
if (!node.IsSequence() || node.size() != N)
{
@@ -124,7 +140,7 @@ YAML::Node YAML::convert<std::unordered_map<Key, T>>::encode(const std::unordere
}
template <class Key, class T>
-bool YAML::convert<std::unordered_map<Key, T>>::decode(const Node& node, std::unordered_map<Key, T>& rhs)
+bool YAML::convert<std::unordered_map<Key, T>>::decode(const Node& node, std::unordered_map<Key, T>& rhs) //NOLINT
{
if (!node.IsMap())
{
@@ -160,7 +176,7 @@ YAML::Node YAML::convert<std::unordered_set<Value>>::encode(const std::unordered
}
template <class Value>
-bool YAML::convert<std::unordered_set<Value>>::decode(const Node& node, std::unordered_set<Value>& rhs)
+bool YAML::convert<std::unordered_set<Value>>::decode(const Node& node, std::unordered_set<Value>& rhs) //NOLINT
{
if (!node.IsSequence())
{
@@ -184,4 +200,4 @@ bool YAML::convert<std::unordered_set<Value>>::decode(const Node& node, std::uno
return result;
}
-#endif // SURGSIM_DATASTRUCTURES_DATASTRUCTURESCONVERT_INL_H
\ No newline at end of file
+#endif // SURGSIM_DATASTRUCTURES_DATASTRUCTURESCONVERT_INL_H
diff --git a/SurgSim/DataStructures/DataStructuresConvert.h b/SurgSim/DataStructures/DataStructuresConvert.h
index 335f66b..c9b9ed7 100644
--- a/SurgSim/DataStructures/DataStructuresConvert.h
+++ b/SurgSim/DataStructures/DataStructuresConvert.h
@@ -34,7 +34,7 @@ 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);
+ static bool decode(const Node& node, SurgSim::DataStructures::OptionalValue<T>& rhs); //NOLINT
};
/// YAML::convert specialization for std::array.
@@ -43,7 +43,7 @@ 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);
+ static bool decode(const Node& node, std::array<T, N>& rhs); //NOLINT
};
/// YAML::convert specialization for std::unordered_map.
@@ -52,7 +52,7 @@ 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);
+ static bool decode(const Node& node, std::unordered_map<Key, T>& rhs); //NOLINT
};
/// YAML::convert specialization for std::unordered_set.
@@ -61,11 +61,11 @@ 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);
+ static bool decode(const Node& node, std::unordered_set<Value>& rhs); //NOLINT
};
} // namespace YAML
#include "SurgSim/DataStructures/DataStructuresConvert-inl.h"
-#endif // SURGSIM_DATASTRUCTURES_DATASTRUCTURESCONVERT_H
\ No newline at end of file
+#endif // SURGSIM_DATASTRUCTURES_DATASTRUCTURESCONVERT_H
diff --git a/SurgSim/DataStructures/Grid-inl.h b/SurgSim/DataStructures/Grid-inl.h
new file mode 100644
index 0000000..549a1b4
--- /dev/null
+++ b/SurgSim/DataStructures/Grid-inl.h
@@ -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.
+
+#ifndef SURGSIM_DATASTRUCTURES_GRID_INL_H
+#define SURGSIM_DATASTRUCTURES_GRID_INL_H
+
+#include <boost/functional/hash.hpp>
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+namespace
+{
+
+/// Class handling number in a given base with a given number of digits
+/// Example numbers in base 3 with 3 digits are defined in order:
+/// 000, 001, 002, 010, 011, 012, 020, 021, 022,
+/// 100, 101, 102, 110, 111, 112, 120, 121, 122
+/// 200, 201, 202, 210, 211, 212, 220, 221, 222
+/// Note that the storage is done in Eigen column-major vector (ith entry corresponds to the ith degree of the base)
+/// Expected usage looking for all neighbors in a 3d array:
+/// Number<size_t, 3, 3> neighborOffset;
+/// Eigen::Matrix<size_t, 3, 1> currentElement = ...; // Location of an element in a 3d grid;
+/// do{
+/// Eigen::Matrix<size_t, 3, 1> neighborElement = currentElement + neighborOffset;
+/// // do something with the neighbor element
+/// ...} while (neighborOffset.next());
+/// \tparam T The type storing the number (must be an integral type)
+/// \tparam B The base in which the number is expressed (must be within [2..9])
+/// \tparam N The number of digits for this number
+template <typename T, size_t B, size_t N>
+class Number : public Eigen::Matrix<T, N, 1>
+{
+public:
+ static_assert(B > 1 && B < 10, "B (the base) needs to be within [2..9]");
+
+ /// Constructor
+ Number()
+ {
+ this->setZero();
+ }
+
+ /// \return the number expressed in base 10
+ size_t toDecimal() const
+ {
+ size_t value = 0;
+ size_t BexponentDigit = 1;
+ for (size_t digit = 0; digit < N; ++digit)
+ {
+ value += (*this)[digit] * BexponentDigit;
+ BexponentDigit *= B;
+ }
+ return value;
+ }
+
+ /// Increment the number
+ /// \return False if there is no next number on N digits, True otherwise.
+ bool next()
+ {
+ size_t digit = 0;
+ do
+ {
+ (*this)[digit]++;
+ if ((*this)[digit] == B)
+ {
+ (*this)[digit] = 0;
+ }
+ else
+ {
+ return true;
+ }
+ digit++;
+ }
+ while (digit < N);
+
+ return false;
+ }
+};
+}; // namespace
+
+
+template <typename T, size_t N>
+size_t Grid<T, N>::NDIdHash::operator()(const NDId& nd) const
+{
+ return boost::hash_range(nd.data(), nd.data() + N);
+}
+
+template <typename T, size_t N>
+Grid<T, N>::Grid(const Eigen::Matrix<double, N, 1>& cellSize, const Eigen::AlignedBox<double, N>& bounds)
+ : m_size(cellSize),
+ m_aabb(bounds),
+ m_neighborsDirtyFlag(false)
+{
+ static_assert(N >= 1, "A grid must have a positive non null dimension");
+}
+
+template <typename T, size_t N>
+Grid<T, N>::~Grid()
+{
+}
+
+template <typename T, size_t N>
+void Grid<T, N>::reset()
+{
+ // Clear the mapping element -> cellId
+ m_cellIds.clear();
+
+ // Clear the active cells
+ m_activeCells.clear();
+
+ // Nothing in the grid (no elements, no neighbors)...so it is up to date
+ m_neighborsDirtyFlag = false;
+}
+
+template <typename T, size_t N>
+template <class Derived>
+void Grid<T, N>::addElement(const T element, const Eigen::MatrixBase<Derived>& position)
+{
+ // Only add element that are located in the grid
+ if (!m_aabb.contains(position))
+ {
+ return;
+ }
+
+ // Find the dimension-N cell id from the location
+ // Example in 3D: cell (i, j, k) has 3D min/max coordinates
+ // min[axis] = (size[axis] * (-numCellPerDim[axis] / 2 + i)
+ // max[axis] = (size[axis] * (-numCellPerDim[axis] / 2 + i + 1)
+ NDId cellId = ((position - m_aabb.min()).cwiseQuotient(m_size)).template cast<int>();
+
+ // Register the element into its corresponding cell if it exists, or creates it.
+ m_activeCells[cellId].elements.push_back(element);
+
+ // Add this element in the map [element -> cellID]
+ m_cellIds[element] = cellId;
+
+ /// Flag that the neighbors list will need to be recomputed on the next access
+ m_neighborsDirtyFlag = true;
+}
+
+template <typename T, size_t N>
+void Grid<T, N>::update()
+{
+ std::array<NDId, powerOf3<N>::value> cellsIds;
+
+ // Start by clearing up all the neighbor's list
+ for (typename std::unordered_map<NDId, typename Grid<T, N>::CellContent, NDIdHash>::reference cell : m_activeCells)
+ {
+ cell.second.neighbors.clear();
+ }
+
+ // Compute each cell's neighbors list
+ for (typename std::unordered_map<NDId, typename Grid<T, N>::CellContent, NDIdHash>::reference cell : m_activeCells)
+ {
+ getNeighborsCellIds(cell.first, &cellsIds);
+
+ for (size_t index = 0; index < powerOf3<N>::value; ++index)
+ {
+ // Use symmetry between pair of cells to only treat neighbors with a larger or equal id.
+ if (isNdGreaterOrEqual(cellsIds[index], cell.first))
+ {
+ auto neighborCell = m_activeCells.find(cellsIds[index]);
+ if (neighborCell != m_activeCells.end())
+ {
+ cell.second.neighbors.insert(cell.second.neighbors.end(),
+ neighborCell->second.elements.cbegin(),
+ neighborCell->second.elements.cend());
+
+ // Treat symmetry if the cells are different
+ if (cellsIds[index] != cell.first)
+ {
+ neighborCell->second.neighbors.insert(neighborCell->second.neighbors.end(),
+ cell.second.elements.cbegin(),
+ cell.second.elements.cend());
+ }
+ }
+ }
+ }
+ }
+ m_neighborsDirtyFlag = false;
+}
+
+template <typename T, size_t N>
+const std::vector<T>& Grid<T, N>::getNeighbors(const T& element)
+{
+ static std::vector<T> empty;
+
+ if (m_neighborsDirtyFlag)
+ {
+ update();
+ }
+
+ auto const foundCell = m_cellIds.find(element);
+ if (foundCell != m_cellIds.cend())
+ {
+ return m_activeCells[foundCell->second].neighbors;
+ }
+
+ return empty;
+}
+
+template <typename T, size_t N>
+template <class Derived>
+const std::vector<T>& Grid<T, N>::getNeighbors(const Eigen::MatrixBase<Derived>& position)
+{
+ static const std::vector<T> empty;
+
+ // If outside the bounding box, can't find any neighbors
+ if (m_aabb.contains(position))
+ {
+ if (m_neighborsDirtyFlag)
+ {
+ update();
+ }
+
+ NDId cellId = ((position - m_aabb.min()).cwiseQuotient(m_size)).template cast<int>();
+
+ auto foundCell = m_activeCells.find(cellId);
+ if (foundCell == m_activeCells.end())
+ {
+ // If the cell doesn't exist, collect all the neighbors
+ std::array<NDId, powerOf3<N>::value> cellsIds;
+ getNeighborsCellIds(cellId, &cellsIds);
+ std::vector<T> neighbors;
+ for (const auto& neighborId : cellsIds)
+ {
+ auto neighborCell = m_activeCells.find(neighborId);
+ if (neighborCell != m_activeCells.end())
+ {
+ neighbors.insert(neighbors.end(),
+ neighborCell->second.elements.cbegin(),
+ neighborCell->second.elements.cend());
+ }
+ }
+ m_activeCells[cellId].neighbors = std::move(neighbors);
+ }
+ return m_activeCells[cellId].neighbors;
+ }
+ return empty;
+}
+
+template <typename T, size_t N>
+void Grid<T, N>::getNeighborsCellIds(NDId cellId,
+ std::array<NDId, powerOf3<N>::value>* cellsIds)
+{
+ // Now build up all the 3^N neighbors cell around this n-d cell
+ // It corresponds to all possible permutation in dimension-N of the indices
+ // {(cellIdnD[0] - 1, cellIdnD[0], cellIdnD[0] + 1) x ... x
+ // (cellIdnD[N - 1] - 1, cellIdnD[N - 1], cellIdnD[N - 1] + 1)}
+ // It is (cellIdnD[0] - 1, ... , cellIdnD[N - 1] - 1) + all possible number in base 3 with N digit
+ // For example in 2D, the NumberInBase3<2> are in order: 00 01 02 10 11 12 20 21 22
+ // For example in 3D, the NumberInBase3<3> are in order:
+ // 000 001 002 010 011 012 020 021 022
+ // 100 101 102 110 111 112 120 121 122
+ // 200 201 202 210 211 212 220 221 222
+ cellId -= NDId::Ones();
+
+ Number<int, 3, N> currentNumberNDigitBase3;
+ for (size_t i = 0; i < powerOf3<N>::value; ++i)
+ {
+ (*cellsIds)[i] = cellId + currentNumberNDigitBase3;
+ currentNumberNDigitBase3.next();
+ }
+}
+
+template <typename T, size_t N>
+bool Grid<T, N>::isNdGreaterOrEqual(const NDId& a, const NDId& b)
+{
+ for (size_t i = 0; i < N; ++i)
+ {
+ if (a[i] > b[i])
+ {
+ return true;
+ }
+
+ if (a[i] < b[i])
+ {
+ return false;
+ }
+ }
+ return true;
+}
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_GRID_INL_H
diff --git a/SurgSim/DataStructures/Grid.h b/SurgSim/DataStructures/Grid.h
new file mode 100644
index 0000000..8b14c30
--- /dev/null
+++ b/SurgSim/DataStructures/Grid.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_DATASTRUCTURES_GRID_H
+#define SURGSIM_DATASTRUCTURES_GRID_H
+
+#include <array>
+#include <unordered_map>
+
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+/// Templated function to compute a power of 3 at compile time (useful for template parameter)
+template <size_t N>
+struct powerOf3
+{
+ enum { value = 3 * powerOf3<N-1>::value };
+};
+template <>
+struct powerOf3<0>
+{
+ enum { value = 1 };
+};
+
+/// n-dimensional grid structure with uniform non-cubic cells
+/// This data structure is useful to search for neighbors in a given range (the size of each cell)
+/// \tparam T Element type to be stored
+/// \tparam N The dimension of the grid (i.e. 2 => 2D, 3 => 3D)
+template <typename T, size_t N>
+class Grid
+{
+public:
+ /// Constructor
+ /// \param cellSize The size of each cell in dimension N (i.e. cells are not necessarily cubic).
+ /// \param bounds The dimension-N boundaries of the space covered by the grid.
+ Grid(const Eigen::Matrix<double, N, 1>& cellSize, const Eigen::AlignedBox<double, N>& bounds);
+
+ /// Destructor
+ virtual ~Grid();
+
+ /// Reset the grid content and the neighbors' mapping
+ void reset();
+
+ /// Add an element in the grid
+ /// \param element to be added at this position
+ /// \param position of the element in the n-D space
+ /// \note If the position is outside of the grid, the element is simply not added to the grid
+ template <class Derived>
+ void addElement(const T element, const Eigen::MatrixBase<Derived>& position);
+
+ /// Retrieve an elements' neighbors
+ /// \param element The element for which the neighbors are requested
+ /// \return The element's neighbors list (including the element itself)
+ const std::vector<T>& getNeighbors(const T& element);
+
+ /// Retrieve the neighbors of a location
+ /// \param position The position for which the neighbors are requested
+ /// \return The neighbors for this position, i.e. all the elements in the positions cell and all surrounding cells
+ template <class Derived>
+ const std::vector<T>& getNeighbors(const Eigen::MatrixBase<Derived>& position);
+
+protected:
+ /// Data structure for a cell's content (the list of elements and the list of all the neighbors)
+ typedef struct
+ {
+ std::vector<T> elements;
+ std::vector<T> neighbors;
+ } CellContent;
+
+ /// The type of the n-dimensional cell Id.
+ typedef Eigen::Matrix<int, N, 1> NDId;
+
+ /// Enable the NDId to be used as a key in an unordered map.
+ class NDIdHash
+ {
+ public:
+ size_t operator()(const NDId& nd) const;
+ };
+
+ /// Active cells (referenced by their ids (spatial hashing)) with their content
+ std::unordered_map<NDId, CellContent, NDIdHash> m_activeCells;
+
+ /// Mapping from element to cell id containing the element
+ std::unordered_map<T, NDId> m_cellIds;
+
+ /// Size of each cell (same on all dimension)
+ Eigen::Matrix<double, N, 1> m_size;
+
+ /// Grid min and max
+ Eigen::AlignedBox<double, N> m_aabb;
+
+ /// Does the neighbors needs to be recomputed ?
+ bool m_neighborsDirtyFlag;
+
+private:
+ /// Update the neighbors lists
+ void update();
+
+ /// Retrieve the neighboring cells id (including this cell)
+ /// \param cellId for which the neighbors cells are requested
+ /// \param cellsIds [out] Neighbors cells ids
+ void getNeighborsCellIds(NDId cellId,
+ std::array<NDId, powerOf3<N>::value>* cellsIds);
+
+ /// \param a The first cell ID.
+ /// \param b The second cell ID.
+ /// \return true if a is > b.
+ bool isNdGreaterOrEqual(const NDId& a, const NDId& b);
+};
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
+#include "SurgSim/DataStructures/Grid-inl.h"
+
+#endif // SURGSIM_DATASTRUCTURES_GRID_H
diff --git a/SurgSim/DataStructures/Groups-inl.h b/SurgSim/DataStructures/Groups-inl.h
new file mode 100644
index 0000000..466fbb8
--- /dev/null
+++ b/SurgSim/DataStructures/Groups-inl.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_DATASTRUCTURES_GROUPS_INL_H
+#define SURGSIM_DATASTRUCTURES_GROUPS_INL_H
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <typename Key, typename T>
+bool Groups<Key, T>::add(const Key& group, const T& element)
+{
+ UniqueLock lock(m_mutex);
+ auto result = m_groups[group].insert(element);
+ if (result.second == true)
+ {
+ m_membership[element].insert(group);
+ }
+ return result.second;
+}
+
+
+template <typename Key, typename T>
+bool Groups<Key, T>::add(const std::vector<Key>& groups, const T& element)
+{
+ bool result = false;
+ for (auto& group : groups)
+ {
+ result = add(group, element) || result;
+ }
+ return result;
+}
+
+
+template <typename Key, typename T>
+bool Groups<Key, T>::add(const Groups<Key, T>& other)
+{
+ bool result = false;
+ for (auto& members : other.m_membership)
+ {
+ result = add(std::vector<Key>(members.second.begin(), members.second.end()), members.first) || result;
+ }
+ return result;
+}
+
+
+template <typename Key, typename T>
+bool Groups<Key, T>::remove(const Key& group, const T& element)
+{
+ bool result = false;
+ UniqueLock lock(m_mutex);
+ auto found = m_groups.find(group);
+ if (found != m_groups.end())
+ {
+ auto count = found->second.erase(element);
+ if (count > 0)
+ {
+ if (found->second.empty())
+ {
+ m_groups.erase(group);
+ }
+
+ m_membership[element].erase(group);
+
+ if (m_membership[element].empty())
+ {
+ m_membership.erase(element);
+ }
+
+ result = true;
+ }
+ }
+ return result;
+}
+
+template <typename Key, typename T>
+std::vector<T> Groups<Key, T>::getMembers(const Key& group) const
+{
+ std::vector<T> result;
+ SharedLock lock(m_mutex);
+ auto found = m_groups.find(group);
+ if (found != m_groups.end())
+ {
+ result.assign(found->second.cbegin(), found->second.cend());
+ }
+ return result;
+}
+
+template <typename Key, typename T>
+std::vector<Key> Groups<Key, T>::getGroups(const T& element) const
+{
+ std::vector<Key> result;
+ SharedLock lock(m_mutex);
+ auto found = m_membership.find(element);
+ if (found != m_membership.end())
+ {
+ result.assign(found->second.cbegin(), found->second.cend());
+ }
+ return result;
+}
+
+template <typename Key, typename T>
+std::vector<Key> Groups<Key, T>::getGroups() const
+{
+ std::vector<Key> result;
+ {
+ SharedLock lock(m_mutex);
+ std::for_each(m_groups.cbegin(), m_groups.cend(),
+ [&result](const std::pair<Key, std::unordered_set<T>>& value)
+ {
+ result.emplace(result.end(), value.first);
+ });
+ }
+ return result;
+}
+
+template <typename Key, typename T>
+bool Groups<Key, T>::remove(const T& element)
+{
+ bool result = false;
+ UniqueLock lock(m_mutex);
+ if (m_membership.find(element) != m_membership.end())
+ {
+ for (auto& group : m_membership[element])
+ {
+ m_groups[group].erase(element);
+ }
+ m_membership.erase(element);
+ result = true;
+ }
+ return result;
+}
+
+template <typename Key, typename T>
+std::vector<T> Groups<Key, T>::operator[](const Key& group) const
+{
+ // Get member does the locking, not needed here
+ return getMembers(group);
+}
+
+
+template <typename Key, typename T>
+void Groups<Key, T>::clear()
+{
+ UniqueLock lock(m_mutex);
+ m_groups.clear();
+ m_membership.clear();
+}
+
+
+}
+}
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/DataStructures/Groups.h b/SurgSim/DataStructures/Groups.h
new file mode 100644
index 0000000..ab64f2e
--- /dev/null
+++ b/SurgSim/DataStructures/Groups.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_DATASTRUCTURES_GROUPS_H
+#define SURGSIM_DATASTRUCTURES_GROUPS_H
+
+#include <unordered_map>
+#include <unordered_set>
+#include <vector>
+
+#include <boost/thread.hpp>
+
+namespace SurgSim
+{
+
+
+namespace DataStructures
+{
+
+/// Class to wrap grouping operations, gives access to the members of a group and the groups of members.
+/// Groups is threadsafe with regard to add and remove operations, and observations
+/// \tparam Key the label to be used for the groups
+/// \tparam T the type of the group members
+template <typename Key, typename T>
+class Groups
+{
+public:
+
+ typedef Key IdentifierType;
+ typedef T MemberType;
+
+ /// Add an element to the given group, if the group doesn't exist it will be created, if the element
+ /// is already a member of the group, nothing happens
+ /// \param group the group to use
+ /// \param element the element to add
+ /// \return true if the element was actually added to the group
+ bool add(const Key& group, const T& element);
+
+ /// Add a member to the given groups, if any of the groups don't exist they will be created, if the element
+ /// is already a member of a group, it won't be added to that specific group
+ /// \param groups the groups to use
+ /// \param element the element to add
+ /// \return true if the element was added to at least one group
+ bool add(const std::vector<Key>& groups, const T& element);
+
+ /// Add all the members from the other group to this group, essentially forming a union of the two
+ /// \param other object to add groups from
+ /// \return true if at least one new element was added
+ bool add(const Groups<Key, T>& other);
+
+ /// Remove an element from a given group, if the group does not exist or the element is not a member of that
+ /// group, nothing will happen.
+ /// \param group the group to use
+ /// \param element the element to remove
+ /// \return true if the element was member of that group
+ bool remove(const Key& group, const T& element);
+
+ /// Remove an element from all known groups, if the element is not a member of any group, nothing happens
+ /// \param element the element to remove
+ /// \return true if there was an actual removal that was executed
+ bool remove(const T& element);
+
+ /// Return all the members of the given group
+ /// \param group the group to query
+ /// \return members of the given group, empty if the group has no members, or doesn't exist
+ std::vector<T> getMembers(const Key& group) const;
+
+ /// Return all the groups that the given member is a member of
+ /// \param element the element to query
+ /// \return groups which contain the given element, empty if the element is not member of any group
+ std::vector<Key> getGroups(const T& element) const;
+
+ /// \return all the known groups that have members
+ std::vector<Key> getGroups() const;
+
+ /// Return all the members of the given group
+ /// \param group group to query
+ /// \return members of the given group, empty if the group has no members
+ std::vector<T> operator[](const Key& group) const;
+
+ /// Erases all entries
+ void clear();
+
+
+private:
+
+ typedef boost::shared_lock<boost::shared_mutex> SharedLock;
+ typedef boost::unique_lock<boost::shared_mutex> UniqueLock;
+
+ /// The mutex used to lock for reading and writing
+ mutable boost::shared_mutex m_mutex;
+
+ /// Map groups to members
+ std::unordered_map<Key, std::unordered_set<T>> m_groups;
+
+ /// Map members to groups
+ std::unordered_map<T, std::unordered_set<Key>> m_membership;
+};
+
+}
+}
+
+#include "SurgSim/DataStructures/Groups-inl.h"
+
+#endif
diff --git a/SurgSim/DataStructures/Image-inl.h b/SurgSim/DataStructures/Image-inl.h
index 19da1f8..e269b03 100644
--- a/SurgSim/DataStructures/Image-inl.h
+++ b/SurgSim/DataStructures/Image-inl.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -13,11 +13,9 @@
// 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
{
@@ -25,31 +23,44 @@ namespace DataStructures
{
template<class T>
-Image<T>::Image() :
- m_width(0), m_height(0), m_channels(0)
+Image<T>::Image()
{
+ ImageBase<T>::setSize(0, 0, 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])
+ m_data(new T[width * height * channels])
{
+ ImageBase<T>::setSize(width, height, 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])
+ m_data(new T[width * height * channels])
{
+ ImageBase<T>::setSize(width, height, 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])
+template<class D>
+Image<T>::Image(size_t width, size_t height, size_t channels, const D* const data) :
+ m_data(new T[width * height * channels])
{
- std::copy(other.m_data.get(), other.m_data.get() + m_width * m_height * m_channels, m_data.get());
+ ImageBase<T>::setSize(width, height, channels);
+ Eigen::Map<const Eigen::Matrix<D, Eigen::Dynamic, 1>> theirData(data, width * height * channels);
+ Eigen::Map<typename ImageBase<T>::VectorType, Eigen::Unaligned> myData(m_data.get(), width * height * channels);
+ myData = theirData.template cast<T>();
+}
+
+template<class T>
+Image<T>::Image(const Image<T>& other)
+{
+ ImageBase<T>::setSize(other.getWidth(), other.getHeight(), other.getNumChannels());
+ size_t size = other.getWidth() * other.getHeight() * other.getNumChannels();
+ m_data = std::unique_ptr<T[]>(new T[size]);
+ std::copy(other.m_data.get(), other.m_data.get() + size, m_data.get());
}
template<class T>
@@ -65,14 +76,12 @@ 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();
+ size_t oldDataSize = ImageBase<T>::getWidth() * ImageBase<T>::getHeight() * ImageBase<T>::getNumChannels();
if (newDataSize != oldDataSize)
{
m_data.reset(new T[newDataSize]);
}
- m_width = other.getWidth();
- m_height = other.getHeight();
- m_channels = other.getNumChannels();
+ ImageBase<T>::setSize(other.getWidth(), other.getHeight(), other.getNumChannels());
std::copy(other.m_data.get(), other.m_data.get() + newDataSize, m_data.get());
}
return *this;
@@ -84,13 +93,9 @@ 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();
+ ImageBase<T>::setSize(other.getWidth(), other.getHeight(), other.getNumChannels());
- other.m_width = 0;
- other.m_height = 0;
- other.m_channels = 0;
+ other.setSize(0, 0, 0);
}
return *this;
}
@@ -101,38 +106,6 @@ 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();
@@ -144,7 +117,7 @@ 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
index 09aba6b..679c290 100644
--- a/SurgSim/DataStructures/Image.h
+++ b/SurgSim/DataStructures/Image.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,22 +16,21 @@
#ifndef SURGSIM_DATASTRUCTURES_IMAGE_H
#define SURGSIM_DATASTRUCTURES_IMAGE_H
-#include <array>
#include <memory>
-#include <Eigen/Core>
+#include "SurgSim/DataStructures/ImageBase.h"
+
namespace SurgSim
{
-
namespace DataStructures
{
/// A templated Image class
///
-/// \tparam T the data type stored in the Image
+/// \tparam T the data type of the image data
template<class T>
-class Image
+class Image : public ImageBase<T>
{
public:
/// Default Constructor
@@ -50,6 +49,15 @@ public:
/// \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 from a data pointer of a different type
+ /// \tparam D type of data stored in the 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
+ template<class D>
+ Image(size_t width, size_t height, size_t channels, const D* const data);
+
/// Copy constructor
/// \param other Image to copy from
Image(const Image<T>& other);
@@ -71,47 +79,14 @@ public:
/// \return The Image that was assigned into
Image<T>& operator=(Image<T>&& other);
- /// Get the Image width
- /// \return the width
- size_t getWidth() const;
+ T* const getData() override;
- /// 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;
+ const T* const getData() const override;
private:
- size_t m_width;
- size_t m_height;
- size_t m_channels;
std::unique_ptr<T[]> m_data;
};
-typedef Image<float> Imagef;
-
}
}
diff --git a/SurgSim/DataStructures/ImageBase-inl.h b/SurgSim/DataStructures/ImageBase-inl.h
new file mode 100644
index 0000000..eabfe4c
--- /dev/null
+++ b/SurgSim/DataStructures/ImageBase-inl.h
@@ -0,0 +1,138 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_IMAGEBASE_INL_H
+#define SURGSIM_DATASTRUCTURES_IMAGEBASE_INL_H
+
+#include "SurgSim/Framework/Assert.h"
+
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template<class T>
+ImageBase<T>::~ImageBase()
+{
+}
+
+template<class T>
+Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>> ImageBase<T>::operator()(size_t x, size_t y)
+{
+ SURGSIM_ASSERT(x < m_width) << "x is larger than the image width (" << x << " >= " << m_width << ")";
+ SURGSIM_ASSERT(y < m_height) << "y is larger than the image height (" << y << " >= " << m_height << ")";
+ return Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>>
+ (getData() + m_channels * (x + y * m_width), m_channels);
+}
+
+template<class T>
+Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, 1>> ImageBase<T>::operator()(size_t x, size_t y) const
+{
+ SURGSIM_ASSERT(x < m_width) << "x is larger than the image width (" << x << " >= " << m_width << ")";
+ SURGSIM_ASSERT(y < m_height) << "y is larger than the image height (" << y << " >= " << m_height << ")";
+ return Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, 1>>
+ (getData() + m_channels * (x + y * m_width), m_channels);
+}
+
+template<class T>
+Eigen::Map<typename ImageBase<T>::ChannelType, Eigen::Unaligned, Eigen::Stride<-1, -1>>
+ImageBase<T>::getChannel(size_t index)
+{
+ SURGSIM_ASSERT(index < m_channels) << "Channel number is larger than the number of channels";
+ Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> stride(m_width*m_channels, m_channels);
+ return Eigen::Map<ChannelType, Eigen::Unaligned, Eigen::Stride<-1, -1>>
+ (getData() + index, m_width, m_height, stride);
+}
+
+template<class T>
+Eigen::Map<const typename ImageBase<T>::ChannelType, Eigen::Unaligned, Eigen::Stride<-1, -1>>
+ImageBase<T>::getChannel(size_t index) const
+{
+ SURGSIM_ASSERT(index < m_channels) << "Channel number is larger than the number of channels";
+ Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> stride(m_width*m_channels, m_channels);
+ return Eigen::Map<const ChannelType, Eigen::Unaligned, Eigen::Stride<-1, -1>>
+ (getData() + index, m_width, m_height, stride);
+}
+
+template<class T>
+void ImageBase<T>::setChannel(size_t index, const Eigen::Ref<const ChannelType>& data)
+{
+ auto myChannel = getChannel(index);
+ SURGSIM_ASSERT(myChannel.rows() == data.rows() && myChannel.cols() == data.cols())
+ << "Channel data must be of size " << myChannel.rows() << "x" << myChannel.cols() << ". "
+ << data.rows() << "x" << data.cols() << " data was provided.";
+ myChannel = data;
+}
+
+template<class T>
+Eigen::Map<typename ImageBase<T>::VectorType, Eigen::Unaligned> ImageBase<T>::getAsVector()
+{
+ return Eigen::Map<VectorType, Eigen::Unaligned>(getData(), m_width * m_height * m_channels);
+}
+
+template<class T>
+Eigen::Map<const typename ImageBase<T>::VectorType, Eigen::Unaligned> ImageBase<T>::getAsVector() const
+{
+ return Eigen::Map<const VectorType, Eigen::Unaligned>(getData(), m_width * m_height * m_channels);
+}
+
+template<class T>
+void ImageBase<T>::setAsVector(const Eigen::Ref<const VectorType>& data)
+{
+ auto myVector = getAsVector();
+ SURGSIM_ASSERT(myVector.rows() == data.rows() && myVector.cols() == data.cols())
+ << "Vector must be of size " << myVector.rows() << "x" << myVector.cols() << ". "
+ << "A " << data.rows() << "x" << data.cols() << " vector was provided.";
+ myVector = data;
+}
+
+template<class T>
+size_t ImageBase<T>::getWidth() const
+{
+ return m_width;
+}
+
+template<class T>
+size_t ImageBase<T>::getHeight() const
+{
+ return m_height;
+}
+
+template<class T>
+std::array<size_t, 3> ImageBase<T>::getSize() const
+{
+ std::array<size_t, 3> size = {m_width, m_height, m_channels};
+ return size;
+}
+
+template<class T>
+size_t ImageBase<T>::getNumChannels() const
+{
+ return m_channels;
+}
+
+template<class T>
+void ImageBase<T>::setSize(size_t width, size_t height, size_t channels)
+{
+ m_width = width;
+ m_height = height;
+ m_channels = channels;
+}
+
+};
+};
+
+#endif //SURGSIM_DATASTRUCTURES_IMAGEBASE_INL_H
diff --git a/SurgSim/DataStructures/ImageBase.h b/SurgSim/DataStructures/ImageBase.h
new file mode 100644
index 0000000..d900688
--- /dev/null
+++ b/SurgSim/DataStructures/ImageBase.h
@@ -0,0 +1,125 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_IMAGEBASE_H
+#define SURGSIM_DATASTRUCTURES_IMAGEBASE_H
+
+#include <array>
+#include <Eigen/Core>
+
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// Base class for Image-like classes
+///
+/// \tparam T the data type of the image data
+template<class T>
+class ImageBase
+{
+public:
+ /// Destructor
+ virtual ~ImageBase();
+
+ /// 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;
+
+ /// Get the pixel value at (x, y)
+ /// \param x The horizontal image position
+ /// \param y The vertical image position
+ /// \return mutable pixel value as Eigen::Map of size (channels x 1)
+ Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>> operator()(size_t x, size_t y);
+
+ /// Get the pixel value at (x, y), constant version
+ /// \param x The horizontal position in the image
+ /// \param y The vertical position in the image
+ /// \return constant pixel value as Eigen::Map of size (channels x 1)
+ Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, 1>> operator()(size_t x, size_t y) const;
+
+ /// 2D Channel Type;
+ typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> ChannelType;
+
+ /// Get the 2D image channel data
+ /// \param index the channel number
+ /// \return mutable channel data as an Eigen::Map (can be used as an Eigen::Matrix)
+ Eigen::Map<ChannelType, Eigen::Unaligned, Eigen::Stride<-1, -1>> getChannel(size_t index);
+
+ /// Get the 2D image channel data, constant version
+ /// \param index the channel number
+ /// \return constant channel data as an Eigen::Map (can be used as an Eigen::Matrix)
+ Eigen::Map<const ChannelType, Eigen::Unaligned, Eigen::Stride<-1, -1>> getChannel(size_t index) const;
+
+ /// Set the image data in the channel
+ /// \param index the channel number
+ /// \param data the channel data as a compatible Eigen type
+ void setChannel(size_t index, const Eigen::Ref<const ChannelType>& data);
+
+ /// 1D Vector Type;
+ typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorType;
+
+ /// Get the data as a 1D Vector
+ /// \return mutable 1D data as an Eigen::Map (can be used as an Eigen::Matrix)
+ Eigen::Map<VectorType, Eigen::Unaligned> getAsVector();
+
+ /// Get the data as a 1D Vector, constant version
+ /// \return constant 1D data as an Eigen::Map (can be used as an Eigen::Matrix)
+ Eigen::Map<const VectorType, Eigen::Unaligned> getAsVector() const;
+
+ /// Set the image data as a 1D Vector
+ /// \param data the data as a compatible Eigen vector type
+ void setAsVector(const Eigen::Ref<const VectorType>& data);
+
+ /// Get the pointer to the data
+ /// \return the data
+ virtual T* const getData() = 0;
+
+ /// Get the pointer to the data, constant version
+ /// \return the data
+ virtual const T* const getData() const = 0;
+
+protected:
+ /// Set the Image size
+ /// \param width the image width
+ /// \param height the image height
+ /// \param channels the number of channels in the image
+ void setSize(size_t width, size_t height, size_t channels);
+
+private:
+ size_t m_width;
+ size_t m_height;
+ size_t m_channels;
+};
+
+};
+};
+
+#include "SurgSim/DataStructures/ImageBase-inl.h"
+
+#endif //SURGSIM_DATASTRUCTURES_IMAGEBASE_H
diff --git a/SurgSim/DataStructures/ImageMap-inl.h b/SurgSim/DataStructures/ImageMap-inl.h
new file mode 100644
index 0000000..968b873
--- /dev/null
+++ b/SurgSim/DataStructures/ImageMap-inl.h
@@ -0,0 +1,47 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_IMAGEMAP_INL_H
+#define SURGSIM_DATASTRUCTURES_IMAGEMAP_INL_H
+
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template<class T>
+ImageMap<T>::ImageMap(size_t width, size_t height, size_t channels, T* data) :
+ m_data(data)
+{
+ ImageBase<T>::setSize(width, height, channels);
+}
+
+template<class T>
+T* const ImageMap<T>::getData()
+{
+ return m_data;
+}
+
+template<class T>
+const T* const ImageMap<T>::getData() const
+{
+ return m_data;
+}
+
+};
+};
+
+#endif //SURGSIM_DATASTRUCTURES_IMAGEMAP_INL_H
diff --git a/SurgSim/DataStructures/ImageMap.h b/SurgSim/DataStructures/ImageMap.h
new file mode 100644
index 0000000..99e69ef
--- /dev/null
+++ b/SurgSim/DataStructures/ImageMap.h
@@ -0,0 +1,54 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_IMAGEMAP_H
+#define SURGSIM_DATASTRUCTURES_IMAGEMAP_H
+
+#include "SurgSim/DataStructures/ImageBase.h"
+
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// A class that behaves like an Image but maps an existing array of data
+///
+/// \tparam T the data type of the image data
+template<class T>
+class ImageMap : public ImageBase<T>
+{
+public:
+ /// Constructor
+ /// \param width the image width
+ /// \param height the image height
+ /// \param channels the number of channels in the image
+ /// \param data pointer to the array to map
+ ImageMap(size_t width, size_t height, size_t channels, T* data);
+
+ T* const getData() override;
+
+ const T* const getData() const override;
+
+private:
+ T* m_data;
+};
+
+};
+};
+
+#include "SurgSim/DataStructures/ImageMap-inl.h"
+
+#endif //SURGSIM_DATASTRUCTURES_IMAGEMAP_H
diff --git a/SurgSim/DataStructures/IndexedLocalCoordinate.cpp b/SurgSim/DataStructures/IndexedLocalCoordinate.cpp
index d2539d7..b81f77e 100644
--- a/SurgSim/DataStructures/IndexedLocalCoordinate.cpp
+++ b/SurgSim/DataStructures/IndexedLocalCoordinate.cpp
@@ -30,6 +30,13 @@ IndexedLocalCoordinate::IndexedLocalCoordinate(size_t index, const SurgSim::Math
{
}
+bool IndexedLocalCoordinate::isApprox(const IndexedLocalCoordinate& other, double precision) const
+{
+ return (index == other.index) &&
+ ((coordinate.isZero(precision) && other.coordinate.isZero(precision)) ||
+ coordinate.isApprox(other.coordinate, precision));
+}
+
} // namespace DataStructures
} // namespace SurgSim
diff --git a/SurgSim/DataStructures/IndexedLocalCoordinate.h b/SurgSim/DataStructures/IndexedLocalCoordinate.h
index 80194ac..fa08168 100644
--- a/SurgSim/DataStructures/IndexedLocalCoordinate.h
+++ b/SurgSim/DataStructures/IndexedLocalCoordinate.h
@@ -42,6 +42,13 @@ struct IndexedLocalCoordinate
/// Coordinates with respect to the entity identified by the index.
SurgSim::Math::Vector coordinate;
+
+ /// Comparison method 'isApprox'
+ /// \param other The other IndexedLocalCoordinate to compare it to
+ /// \param precision The precision with which to compare
+ /// \return True if the two IndexedLocalCoordinate are equal within precision, False otherwise
+ bool isApprox(const IndexedLocalCoordinate& other, double precision = std::numeric_limits<double>::epsilon())
+ const;
};
} // namespace DataStructures
diff --git a/SurgSim/DataStructures/Location.h b/SurgSim/DataStructures/Location.h
index 5e03bdb..a747462 100644
--- a/SurgSim/DataStructures/Location.h
+++ b/SurgSim/DataStructures/Location.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -18,9 +18,9 @@
#include <vector>
+#include "SurgSim/DataStructures/OptionalValue.h"
#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
#include "SurgSim/DataStructures/OctreeNode.h"
-#include "SurgSim/DataStructures/OptionalValue.h"
#include "SurgSim/Math/Vector.h"
namespace SurgSim
@@ -28,14 +28,34 @@ namespace SurgSim
namespace DataStructures
{
+/// A Location defines a local position w.r.t. any shape. Depending on the type of shape,
+/// different data is needed to specify a location on it. This structure supports:
+/// - Any rigid shape (location identified by a local position)
+/// - Octree (location identified by an octree path)
+/// - Index (location indentified by an index)
+/// - A triangle mesh (location identified by the triangle id, and the barycentric coordinate of a point in it)
+/// - A node in a mesh (location identified by the node id)
+/// - An element in a mesh (location identified by the element id, and the barycentric coordinate of a point in it)
struct Location
{
public:
+ enum Type {TRIANGLE, ELEMENT};
+
/// Default constructor
Location()
{
}
+ /// Copy constructor
+ /// \param other The location to be copied while constructing.
+ Location(const Location& other)
+ : rigidLocalPosition(other.rigidLocalPosition),
+ octreeNodePath(other.octreeNodePath),
+ index(other.index),
+ triangleMeshLocalCoordinate(other.triangleMeshLocalCoordinate),
+ elementMeshLocalCoordinate(other.elementMeshLocalCoordinate)
+ {}
+
/// Constructor for rigid local position
/// \param localPosition The 3D local position to set this location to
explicit Location(const SurgSim::Math::Vector3d& localPosition)
@@ -50,18 +70,148 @@ public:
octreeNodePath.setValue(nodePath);
}
- /// Constructor for mesh local coordinate
- /// \param localCoordinate The mesh local coordinate
- explicit Location(const SurgSim::DataStructures::IndexedLocalCoordinate& localCoordinate)
+ /// Constructor for an index
+ /// \param val The index to set this location to
+ explicit Location(const size_t val)
+ {
+ index.setValue(val);
+ }
+
+ /// Constructor for mesh-based location
+ /// \param localCoordinate index-based local coordinate
+ /// \param meshType the type of location (a node, a triangle or an element)
+ Location(const SurgSim::DataStructures::IndexedLocalCoordinate& localCoordinate, Type meshType)
+ {
+ switch (meshType)
+ {
+ case TRIANGLE:
+ triangleMeshLocalCoordinate.setValue(localCoordinate);
+ break;
+ case ELEMENT:
+ elementMeshLocalCoordinate.setValue(localCoordinate);
+ break;
+ default:
+ SURGSIM_FAILURE() << "Unknown location";
+ break;
+ }
+ }
+
+ /// Gives access to the coordinates via enum rather than '.' public member access, this can reduce complexity
+ /// in the calling code
+ /// \param meshType (TRIANGLE or ELEMENT)
+ /// \return the appropriate coordinate for the type of mesh
+ const SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::IndexedLocalCoordinate>& get(Type meshType)
{
- meshLocalCoordinate.setValue(localCoordinate);
+ static SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::IndexedLocalCoordinate> null;
+ switch (meshType)
+ {
+ case TRIANGLE:
+ return triangleMeshLocalCoordinate;
+ break;
+ case ELEMENT:
+ return elementMeshLocalCoordinate;
+ break;
+ default:
+ SURGSIM_FAILURE() << "Unknown location";
+ break;
+ }
+ return null;
+ }
+
+ bool isApprox(const Location& other, double precision = std::numeric_limits<double>::epsilon()) const
+ {
+ bool result = false;
+
+ if (rigidLocalPosition.hasValue() && other.rigidLocalPosition.hasValue())
+ {
+ auto const& vector1 = rigidLocalPosition.getValue();
+ auto const& vector2 = other.rigidLocalPosition.getValue();
+
+ result = (vector1.isZero(precision) && vector2.isZero(precision)) || vector1.isApprox(vector2, precision);
+ }
+ else if (octreeNodePath.hasValue() && other.octreeNodePath.hasValue())
+ {
+ result = (octreeNodePath.getValue() == other.octreeNodePath.getValue());
+ }
+ else if (index.hasValue() && other.index.hasValue())
+ {
+ result = (index.getValue() == other.index.getValue());
+ }
+ else if (triangleMeshLocalCoordinate.hasValue() && other.triangleMeshLocalCoordinate.hasValue())
+ {
+ result = triangleMeshLocalCoordinate.getValue().isApprox(other.triangleMeshLocalCoordinate.getValue());
+ }
+ else if (elementMeshLocalCoordinate.hasValue() && other.elementMeshLocalCoordinate.hasValue())
+ {
+ result = elementMeshLocalCoordinate.getValue().isApprox(other.elementMeshLocalCoordinate.getValue());
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Invalid location type";
+ }
+
+ return result;
}
SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d> rigidLocalPosition;
SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::OctreePath> octreeNodePath;
- SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::IndexedLocalCoordinate> meshLocalCoordinate;
+ SurgSim::DataStructures::OptionalValue<size_t> index;
+ SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::IndexedLocalCoordinate> triangleMeshLocalCoordinate;
+ SurgSim::DataStructures::OptionalValue<SurgSim::DataStructures::IndexedLocalCoordinate> elementMeshLocalCoordinate;
};
+
+template <typename charT, typename traits, typename T>
+std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& out,
+ const SurgSim::DataStructures::OptionalValue<T>& val)
+{
+ if (val.hasValue())
+ {
+ out << val.getValue();
+ }
+ else
+ {
+ out << "<empty>";
+ }
+ return out;
+}
+
+template <typename charT, typename traits>
+std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& out,
+ const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>& val)
+{
+ if (val.hasValue())
+ {
+ out << val.getValue().transpose();
+ }
+ else
+ {
+ out << "<empty>";
+ }
+ return out;
+}
+
+
+template <typename charT, typename traits>
+std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& out,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& val)
+{
+ out << "[ " << val.index << " : " << val.coordinate.transpose() << " ]";
+ return out;
+}
+
+
+template <typename charT, typename traits>
+std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& out,
+ const Location& loc)
+{
+ out << "ElementMesh: " << loc.elementMeshLocalCoordinate << std::endl;
+ out << "Index: " << loc.index << std::endl;
+ out << "RigidLocal: " << loc.rigidLocalPosition << std::endl;
+ out << "TriangleMeshLocal: " << loc.triangleMeshLocalCoordinate << std::endl;
+ return out;
+}
+
}; // namespace DataStructures
}; // namespace SurgSim
diff --git a/SurgSim/DataStructures/MeshElement.h b/SurgSim/DataStructures/MeshElement.h
index bc15026..6fb5a24 100644
--- a/SurgSim/DataStructures/MeshElement.h
+++ b/SurgSim/DataStructures/MeshElement.h
@@ -61,6 +61,17 @@ struct MeshElement
{
}
+ /// 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 MeshElement
+ /// \param other the MeshElement to copy from
+ template <class T>
+ explicit MeshElement(const MeshElement<N, T>& other) :
+ verticesId(other.verticesId),
+ isValid(other.isValid)
+ {
+ }
+
typedef std::array<size_t, N> IdType;
/// Element vertices.
diff --git a/SurgSim/DataStructures/NamedData-inl.h b/SurgSim/DataStructures/NamedData-inl.h
index 953dc27..e42397a 100644
--- a/SurgSim/DataStructures/NamedData-inl.h
+++ b/SurgSim/DataStructures/NamedData-inl.h
@@ -174,11 +174,7 @@ inline bool NamedData<T>::hasData(int index) const
template <typename T>
inline bool NamedData<T>::hasData(const std::string& name) const
{
- if (! isValid())
- {
- return false;
- }
- int index = m_directory->getIndex(name);
+ int index = getIndex(name);
if (index < 0)
{
return false;
@@ -207,11 +203,7 @@ inline bool NamedData<T>::get(int index, T* value) const
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);
+ int index = getIndex(name);
if ((index < 0) || ! m_isDataValid[index])
{
return false;
@@ -240,22 +232,48 @@ inline bool NamedData<T>::set(int index, const T& value)
}
template <typename T>
+inline bool NamedData<T>::set(int index, T&& value)
+{
+ if (! hasEntry(index))
+ {
+ return false;
+ }
+ else
+ {
+ m_data[index] = std::move(value);
+ m_isDataValid[index] = true;
+ return true;
+ }
+}
+
+template <typename T>
inline bool NamedData<T>::set(const std::string& name, const T& value)
{
- if (! isValid())
+ int index = getIndex(name);
+ if (index < 0)
{
return false;
}
- int index = m_directory->getIndex(name);
+ else
+ {
+ SURGSIM_ASSERT(set(index, value) == true)
+ << "The directory returned an index larger than the number of entries in the stored data.";
+ return true;
+ }
+}
+
+template <typename T>
+inline bool NamedData<T>::set(const std::string& name, T&& value)
+{
+ int index = getIndex(name);
if (index < 0)
{
return false;
}
else
{
- SURGSIM_ASSERT(hasEntry(index));
- m_data[index] = value;
- m_isDataValid[index] = true;
+ SURGSIM_ASSERT(set(index, std::move(value)) == true)
+ << "The directory returned an index larger than the number of entries in the stored data.";
return true;
}
}
@@ -277,19 +295,15 @@ inline bool NamedData<T>::reset(int index)
template <typename T>
inline bool NamedData<T>::reset(const std::string& name)
{
- if (! isValid())
- {
- return false;
- }
- int index = m_directory->getIndex(name);
+ int index = getIndex(name);
if (index < 0)
{
return false;
}
else
{
- SURGSIM_ASSERT(hasEntry(index));
- m_isDataValid[index] = false;
+ SURGSIM_ASSERT(reset(index) == true)
+ << "The directory returned an index larger than the number of entries in the stored data.";
return true;
}
}
diff --git a/SurgSim/DataStructures/NamedData.h b/SurgSim/DataStructures/NamedData.h
index 0fd86fc..f0446df 100644
--- a/SurgSim/DataStructures/NamedData.h
+++ b/SurgSim/DataStructures/NamedData.h
@@ -40,6 +40,7 @@ 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 TOOLDOF = "toolDof";
static const char* const POSE = "pose";
static const char* const INPUT_POSE = "inputPose";
@@ -65,6 +66,12 @@ 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";
+
+static const char* const PROJECTION_MATRIX = "projectionMatix";
+static const char* const LEFT_PROJECTION_MATRIX = "leftProjectionMatix";
+static const char* const RIGHT_PROJECTION_MATRIX = "rightProjectionMatix";
+
+static const char* const KEY = "key";
};
/// A templated dictionary in which data can be accessed by name or index, with immutable names & indices.
@@ -239,6 +246,15 @@ public:
/// \return true if successful.
inline bool set(int index, const T& value);
+ /// Record the data for an entry specified by an index.
+ /// This version accepts rvalues, and the data will be moved
+ /// 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, T&& value);
+
/// Record the data for an entry specified by a name.
/// The entry will also be marked as containing valid data.
///
@@ -247,6 +263,15 @@ public:
/// \return true if successful.
inline bool set(const std::string& name, const T& value);
+ /// Record the data for an entry specified by a name.
+ /// This version accepts rvalues, and the data will be moved
+ /// 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, T&& value);
+
/// Invalidate an entry— mark it as not containing any valid data.
///
/// \param index The index of the entry.
diff --git a/SurgSim/DataStructures/NormalData.h b/SurgSim/DataStructures/NormalData.h
new file mode 100644
index 0000000..5e5ca93
--- /dev/null
+++ b/SurgSim/DataStructures/NormalData.h
@@ -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.
+
+#ifndef SURGSIM_DATASTRUCTURES_NORMALDATA_H
+#define SURGSIM_DATASTRUCTURES_NORMALDATA_H
+
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+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 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 NormalData& rhs) const
+ {
+ return !((*this) == rhs);
+ }
+};
+
+};
+};
+
+#endif // SURGSIM_DATASTRUCTURES_NORMALDATA_H
diff --git a/SurgSim/DataStructures/OctreeNode-inl.h b/SurgSim/DataStructures/OctreeNode-inl.h
index d711511..062b9cb 100644
--- a/SurgSim/DataStructures/OctreeNode-inl.h
+++ b/SurgSim/DataStructures/OctreeNode-inl.h
@@ -20,7 +20,11 @@
#include <cmath>
#include <fstream>
+#include "SurgSim/DataStructures/OctreeNodePlyReaderDelegate.h"
+#include "SurgSim/DataStructures/PlyReader.h"
#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Timer.h"
namespace SurgSim
{
@@ -95,6 +99,12 @@ OctreeNode<Data>::~OctreeNode()
}
template<class Data>
+std::string OctreeNode<Data>::getClassName() const
+{
+ return m_className;
+}
+
+template<class Data>
const SurgSim::Math::Aabbd& OctreeNode<Data>::getBoundingBox() const
{
return m_boundingBox;
@@ -142,7 +152,7 @@ void OctreeNode<Data>::subdivide()
}
template<class Data>
-bool OctreeNode<Data>::addData(const SurgSim::Math::Vector3d& position, const Data& nodeData, const int level)
+bool OctreeNode<Data>::addData(const SurgSim::Math::Vector3d& position, const int level, const Data& nodeData)
{
return doAddData(position, nodeData, level, 1);
}
@@ -179,25 +189,25 @@ bool OctreeNode<Data>::doAddData(const SurgSim::Math::Vector3d& position, const
}
template<class Data>
-std::array<std::shared_ptr<OctreeNode<Data> >, 8>& OctreeNode<Data>::getChildren()
+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
+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)
+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
+const std::shared_ptr<OctreeNode<Data>> OctreeNode<Data>::getChild(size_t index) const
{
return m_children[index];
}
@@ -228,34 +238,18 @@ std::shared_ptr<OctreeNode<Data>> OctreeNode<Data>::getNode(const OctreePath& pa
}
template<class Data>
-bool SurgSim::DataStructures::OctreeNode<Data>::doLoad(const std::string& filePath)
+bool SurgSim::DataStructures::OctreeNode<Data>::doLoad(const std::string& fileName)
{
- 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);
- }
-
+ SurgSim::Framework::Timer timer;
+ auto delegate = std::make_shared<OctreeNodePlyReaderDelegate<Data>>(this->shared_from_this());
+
+ PlyReader reader(fileName);
+ SURGSIM_ASSERT(reader.isValid()) << "'" << fileName << "' is an invalid .ply file.";
+ SURGSIM_ASSERT(reader.parseWithDelegate(delegate)) <<
+ "The input file " << fileName << " does not have the property required by the octree.";
+ timer.endFrame();
+ SURGSIM_LOG_INFO(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Loading " << fileName << " took " << timer.getCumulativeTime() << "s. ";
return true;
}
diff --git a/SurgSim/DataStructures/OctreeNode.cpp b/SurgSim/DataStructures/OctreeNode.cpp
index 40c4ce9..a02bf82 100644
--- a/SurgSim/DataStructures/OctreeNode.cpp
+++ b/SurgSim/DataStructures/OctreeNode.cpp
@@ -24,6 +24,7 @@
#include <boost/container/static_vector.hpp>
+
namespace
{
@@ -135,46 +136,21 @@ static const std::array<std::array<Symbol, 3>, 8> VertexNeighbors =
}
+
+
namespace SurgSim
{
namespace DataStructures
{
+SURGSIM_REGISTER(SurgSim::Framework::Asset,
+ SurgSim::DataStructures::OctreeNode<EmptyData>,
+ OctreeNodeEmptyData);
-
-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;
-}
+// Predefine classname of OctreeNode of EmptyData
+template<>
+std::string OctreeNode<EmptyData>::m_className = "SurgSim::DataStructures::OctreeNode<EmptyData>";
SurgSim::DataStructures::OctreePath getNeighbor(const OctreePath& origin, const std::array<Symbol, 3>& direction)
{
@@ -234,7 +210,7 @@ SurgSim::DataStructures::OctreePath getNeighbor(const OctreePath& origin, const
result.clear();
}
- return std::move(result);
+ return result;
}
std::vector<OctreePath> getNeighbors(const OctreePath& origin, int type)
@@ -267,9 +243,10 @@ std::vector<OctreePath> getNeighbors(const OctreePath& origin, int type)
{
std::for_each(VertexNeighbors.begin(), VertexNeighbors.end(), f);
}
- return std::move(result);
+ return result;
}
+
}; // namespace DataStructures
}; // namespace SurgSim
diff --git a/SurgSim/DataStructures/OctreeNode.h b/SurgSim/DataStructures/OctreeNode.h
index 4dd76f9..e2744d7 100644
--- a/SurgSim/DataStructures/OctreeNode.h
+++ b/SurgSim/DataStructures/OctreeNode.h
@@ -109,6 +109,9 @@ OctreePath getNeighbor(const OctreePath& origin, const std::array<Symbol, 3>& di
/// \return list of paths with neighbors of the node at origin
std::vector<OctreePath> getNeighbors(const OctreePath& origin, int type);
+template <typename Data>
+class OctreeNodePlyReaderDelegate;
+
/// Octree data structure
///
/// The octree node consists of an axis aligned bounding box, that can be
@@ -129,6 +132,7 @@ class OctreeNode : public SurgSim::Framework::Asset,
public std::enable_shared_from_this<OctreeNode<Data>>
{
friend class SurgSim::Math::OctreeShape;
+ friend class SurgSim::DataStructures::OctreeNodePlyReaderDelegate<Data>;
public:
@@ -149,6 +153,8 @@ public:
template <class T>
OctreeNode(const OctreeNode<T>& other);
+ std::string getClassName() const override;
+
/// Constructor
/// \param boundingBox The region contained by this octree node
explicit OctreeNode(const SurgSim::Math::Aabbd& boundingBox);
@@ -184,27 +190,27 @@ public:
/// \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);
+ bool addData(const SurgSim::Math::Vector3d& position, const int level, const Data& nodeData = Data());
/// Get the children of this node (non const version)
/// \return vector of all eight children
- std::array<std::shared_ptr<OctreeNode<Data> >, 8>& getChildren();
+ 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;
+ 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);
+ 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;
+ 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.
@@ -212,7 +218,7 @@ public:
/// \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);
+ virtual std::shared_ptr<OctreeNode<Data>> getNode(const OctreePath& path, bool returnLastValid = false);
/// Extra node data
Data data;
@@ -227,7 +233,7 @@ protected:
bool doAddData(const SurgSim::Math::Vector3d& position, const Data& nodeData, const int level,
const int currentLevel);
- virtual bool doLoad(const std::string& filePath) override;
+ bool doLoad(const std::string& filePath) override;
/// The bounding box of the current OctreeNode
SurgSim::Math::Aabbd m_boundingBox;
@@ -239,14 +245,11 @@ protected:
bool m_hasChildren;
/// The children of this node
- std::array<std::shared_ptr<OctreeNode<Data> >, 8> m_children;
-};
+ 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);
+private:
+ static std::string m_className;
+};
}; // namespace DataStructures
}; // namespace SurgSim
diff --git a/SurgSim/DataStructures/OctreeNodePlyReaderDelegate-inl.h b/SurgSim/DataStructures/OctreeNodePlyReaderDelegate-inl.h
new file mode 100644
index 0000000..28229ed
--- /dev/null
+++ b/SurgSim/DataStructures/OctreeNodePlyReaderDelegate-inl.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_DATASTRUCTURES_OCTREENODEPLYREADERDELEGATE_INL_H
+#define SURGSIM_DATASTRUCTURES_OCTREENODEPLYREADERDELEGATE_INL_H
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <typename Data>
+OctreeNodePlyReaderDelegate<Data>::OctreeNodePlyReaderDelegate() :
+ m_octree(std::make_shared<OctreeNode<Data>>())
+{
+ static_assert(std::is_default_constructible<Data>::value, "OctreeNode Data needs default constructor.");
+}
+
+template <typename Data>
+OctreeNodePlyReaderDelegate<Data>::OctreeNodePlyReaderDelegate(std::shared_ptr<OctreeNode<Data>> octree) :
+ m_octree(octree)
+{
+ static_assert(std::is_default_constructible<Data>::value, "OctreeNode Data needs default constructor");
+ SURGSIM_ASSERT(!m_octree->hasChildren()) << "Can't process an octree that already has children in it.";
+}
+
+template <typename Data>
+OctreeNodePlyReaderDelegate<Data>::~OctreeNodePlyReaderDelegate()
+{
+
+}
+
+template <typename Data>
+std::shared_ptr<OctreeNode<Data>> OctreeNodePlyReaderDelegate<Data>::getOctree()
+{
+ return m_octree;
+}
+
+template <typename Data>
+void OctreeNodePlyReaderDelegate<Data>::processVoxel(const std::string& elementName)
+{
+ SurgSim::Math::Vector3d position;
+ position[0] = m_voxel.x;
+ position[1] = m_voxel.y;
+ position[2] = m_voxel.z;
+ m_octree->addData(position, m_numLevels);
+}
+
+template <typename Data>
+void OctreeNodePlyReaderDelegate<Data>::initializeOctree()
+{
+ m_octree->m_boundingBox = m_boundingBox;
+}
+
+}
+}
+
+#endif
diff --git a/SurgSim/DataStructures/OctreeNodePlyReaderDelegate.cpp b/SurgSim/DataStructures/OctreeNodePlyReaderDelegate.cpp
new file mode 100644
index 0000000..8371e69
--- /dev/null
+++ b/SurgSim/DataStructures/OctreeNodePlyReaderDelegate.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 "SurgSim/DataStructures/OctreeNodePlyReaderDelegate.h"
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Framework/Log.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+OctreeNodePlyReaderDelegateBase::OctreeNodePlyReaderDelegateBase() :
+ m_haveSpacing(false),
+ m_haveDimensions(false),
+ m_haveBounds(false)
+{
+
+}
+
+OctreeNodePlyReaderDelegateBase::~OctreeNodePlyReaderDelegateBase()
+{
+
+}
+
+bool OctreeNodePlyReaderDelegateBase::registerDelegate(PlyReader* reader)
+{
+ // Vertex processing
+ reader->requestElement("bounds",
+ std::bind(&OctreeNodePlyReaderDelegateBase::beginBounds, this,
+ std::placeholders::_1, std::placeholders::_2),
+ nullptr,
+ nullptr);
+ reader->requestScalarProperty("bounds", "xMin", PlyReader::TYPE_DOUBLE, offsetof(BoundsData, xMin));
+ reader->requestScalarProperty("bounds", "yMin", PlyReader::TYPE_DOUBLE, offsetof(BoundsData, yMin));
+ reader->requestScalarProperty("bounds", "zMin", PlyReader::TYPE_DOUBLE, offsetof(BoundsData, zMin));
+ reader->requestScalarProperty("bounds", "xMax", PlyReader::TYPE_DOUBLE, offsetof(BoundsData, xMax));
+ reader->requestScalarProperty("bounds", "yMax", PlyReader::TYPE_DOUBLE, offsetof(BoundsData, yMax));
+ reader->requestScalarProperty("bounds", "zMax", PlyReader::TYPE_DOUBLE, offsetof(BoundsData, zMax));
+
+ reader->requestElement("dimension",
+ std::bind(&OctreeNodePlyReaderDelegateBase::beginDimension, this,
+ std::placeholders::_1, std::placeholders::_2),
+ nullptr,
+ nullptr);
+ reader->requestScalarProperty("dimension", "x", PlyReader::TYPE_UNSIGNED_INT, offsetof(DimensionData, x));
+ reader->requestScalarProperty("dimension", "y", PlyReader::TYPE_UNSIGNED_INT, offsetof(DimensionData, y));
+ reader->requestScalarProperty("dimension", "z", PlyReader::TYPE_UNSIGNED_INT, offsetof(DimensionData, z));
+
+ reader->requestElement("spacing",
+ std::bind(&OctreeNodePlyReaderDelegateBase::beginSpacing, this,
+ std::placeholders::_1, std::placeholders::_2),
+ nullptr,
+ nullptr);
+ reader->requestScalarProperty("spacing", "x", PlyReader::TYPE_DOUBLE, offsetof(SpacingData, x));
+ reader->requestScalarProperty("spacing", "y", PlyReader::TYPE_DOUBLE, offsetof(SpacingData, y));
+ reader->requestScalarProperty("spacing", "z", PlyReader::TYPE_DOUBLE, offsetof(SpacingData, z));
+
+ reader->requestElement("voxel",
+ std::bind(&OctreeNodePlyReaderDelegateBase::beginVoxel, this,
+ std::placeholders::_1, std::placeholders::_2),
+ std::bind(&OctreeNodePlyReaderDelegateBase::processVoxel, this, std::placeholders::_1),
+ nullptr);
+ reader->requestScalarProperty("voxel", "x", PlyReader::TYPE_DOUBLE, offsetof(VoxelData, x));
+ reader->requestScalarProperty("voxel", "y", PlyReader::TYPE_DOUBLE, offsetof(VoxelData, y));
+ reader->requestScalarProperty("voxel", "z", PlyReader::TYPE_DOUBLE, offsetof(VoxelData, z));
+
+ return true;
+}
+
+bool OctreeNodePlyReaderDelegateBase::fileIsAcceptable(const PlyReader& reader)
+{
+ bool result = true;
+
+ // Shortcut test if one fails ...
+ result = result && reader.hasProperty("bounds", "xMin") && reader.hasProperty("bounds", "yMin") &&
+ reader.hasProperty("bounds", "zMin") && reader.hasProperty("bounds", "xMax") &&
+ reader.hasProperty("bounds", "yMax") && reader.hasProperty("bounds", "zMax");
+ result = result && reader.hasProperty("dimension", "x") && reader.hasProperty("dimension", "y") &&
+ reader.hasProperty("dimension", "z");
+ result = result && reader.hasProperty("spacing", "x") && reader.hasProperty("spacing", "y") &&
+ reader.hasProperty("spacing", "z");
+ result = result && reader.hasProperty("voxel", "x") && reader.hasProperty("voxel", "y") &&
+ reader.hasProperty("voxel", "z");
+
+ return result;
+}
+
+void* OctreeNodePlyReaderDelegateBase::beginBounds(const std::string& elementName, size_t count)
+{
+ SURGSIM_ASSERT(count == 1) << "Bounds should only have exactly one entry.";
+ m_haveBounds = true;
+ return &m_bounds;
+}
+
+void* OctreeNodePlyReaderDelegateBase::beginDimension(const std::string& elementName, size_t count)
+{
+ SURGSIM_ASSERT(count == 1) << "Dimension should only have exactly one entry.";
+ m_haveDimensions = true;
+ return &m_dimension;
+}
+
+void* OctreeNodePlyReaderDelegateBase::beginSpacing(const std::string& elementName, size_t count)
+{
+ SURGSIM_ASSERT(count == 1) << "Spacing should only have exactly one entry.";
+ m_haveSpacing = true;
+ return &m_spacing;
+}
+
+void* OctreeNodePlyReaderDelegateBase::beginVoxel(const std::string& elementName, size_t count)
+{
+ SURGSIM_ASSERT(m_haveSpacing) << "Need spacing data for complete octree.";
+ SURGSIM_ASSERT(m_haveBounds) << "Need bounds data for complete octree.";
+ SURGSIM_ASSERT(m_haveDimensions) << "Need dimension data for complete octree.";
+
+ auto maxDimension = std::max(m_dimension.x, std::max(m_dimension.y, m_dimension.z));
+
+ m_numLevels = 1 + static_cast<int>(std::ceil(std::log(maxDimension) / std::log(2.0)));
+ SurgSim::Math::Vector3d octreeDimensions = SurgSim::Math::Vector3d::Ones() * std::pow(2.0, m_numLevels - 1);
+
+ Math::Vector3d spacing(m_spacing.x, m_spacing.y, m_spacing.z);
+
+ m_boundingBox.min() = Math::Vector3d(m_bounds.xMin, m_bounds.yMin, m_bounds.zMin);
+ m_boundingBox.max() = Math::Vector3d(m_bounds.xMin, m_bounds.yMin, m_bounds.zMin).array() +
+ octreeDimensions.array() * spacing.array();
+
+ initializeOctree();
+
+ return &m_voxel;
+}
+
+}
+}
+
diff --git a/SurgSim/DataStructures/OctreeNodePlyReaderDelegate.h b/SurgSim/DataStructures/OctreeNodePlyReaderDelegate.h
new file mode 100644
index 0000000..9be6510
--- /dev/null
+++ b/SurgSim/DataStructures/OctreeNodePlyReaderDelegate.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_DATASTRUCTURES_OCTREENODEPLYREADERDELEGATE_H
+#define SURGSIM_DATASTRUCTURES_OCTREENODEPLYREADERDELEGATE_H
+
+#include "SurgSim/DataStructures/PlyReaderDelegate.h"
+#include "SurgSim/DataStructures/OctreeNode.h"
+#include "SurgSim/DataStructures/EmptyData.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// Delegate to read Octrees from ply files, this is the abstract base class, to let us modify
+/// loading by datatype
+class OctreeNodePlyReaderDelegateBase : public PlyReaderDelegate
+{
+public:
+
+ /// Constructor
+ OctreeNodePlyReaderDelegateBase();
+
+ /// Destructor
+ virtual ~OctreeNodePlyReaderDelegateBase();
+
+ bool registerDelegate(PlyReader* reader) override;
+
+ bool fileIsAcceptable(const PlyReader& reader) override;
+
+ /// Callback function, begin the processing of the bounds.
+ /// \param elementName Name of the element.
+ /// \param count Number of entries.
+ /// \throws SurgSim::Framework::AssertionFailure if count != 1, only one bounds entry is expected
+ /// \return memory for bounds data to the reader.
+ void* beginBounds(const std::string& elementName, size_t count);
+
+ /// Callback function, begin the processing of the dimension.
+ /// \param elementName Name of the element.
+ /// \param count Number of entries.
+ /// \throws SurgSim::Framework::AssertionFailure if count != 1, only one dimesion entry is expected
+ /// \return memory for dimension data to the reader.
+ void* beginDimension(const std::string& elementName, size_t count);
+
+ /// Callback function, begin the processing of the bounds.
+ /// \param elementName Name of the element.
+ /// \param count Number of entries.
+ /// \throws SurgSim::Framework::AssertionFailure if count != 1, only one spacing entry is expected
+ /// \return memory for spacing data to the reader.
+ void* beginSpacing(const std::string& elementName, size_t count);
+
+ /// Callback function, begin the processing of the voxels.
+ /// \param elementName Name of the element.
+ /// \param count Number of bounds entry.
+ /// \return memory for voxel data to the reader.
+ virtual void* beginVoxel(const std::string& elementName, size_t count);
+
+ /// Callback function to process one voxel. This is left up to the subclasses, they might have to deal with
+ /// data specific processing
+ /// \param elementName Name of the element.
+ virtual void processVoxel(const std::string& elementName) = 0;
+
+protected:
+ /// Set up the octree, this is left up to the derived classes
+ virtual void initializeOctree() = 0;
+
+ /// Data Structure to receive the bounds information
+ struct BoundsData
+ {
+ double xMin;
+ double yMin;
+ double zMin;
+ double xMax;
+ double yMax;
+ double zMax;
+ } m_bounds;
+
+ /// Bounding box, will be initialized from the file
+ Eigen::AlignedBox<double, 3> m_boundingBox;
+
+ /// Data structure to receive the spacing information from the file
+ struct SpacingData
+ {
+ double x;
+ double y;
+ double z;
+ } m_spacing;
+
+ /// Data structure to receive the dimension information from the file
+ struct DimensionData
+ {
+ unsigned int x;
+ unsigned int y;
+ unsigned int z;
+ } m_dimension;
+
+ /// Data structure to receive the specific voxel information from the file
+ struct VoxelData
+ {
+ double x;
+ double y;
+ double z;
+ } m_voxel;
+
+ /// Calculated number of levels for the octree
+ int m_numLevels;
+
+ ///@{
+ /// Check wether we actually received the appropriate information, if not we can't initialize the octree
+ bool m_haveSpacing;
+ bool m_haveDimensions;
+ bool m_haveBounds;
+ ///@}
+
+};
+
+/// Subclass the OctreeNodePLyReaderDelegateBase class to enable processing of the templated data, this should be
+/// specialized to enable specific processing of various extended node types
+/// \tparam Data The data the should be inside the Octree, needs to be default constructable
+template <typename Data>
+class OctreeNodePlyReaderDelegate : public OctreeNodePlyReaderDelegateBase
+{
+public:
+
+ /// Constructor
+ OctreeNodePlyReaderDelegate();
+
+ /// Constructor
+ /// \param octree read the data into this octree
+ explicit OctreeNodePlyReaderDelegate(std::shared_ptr<OctreeNode<Data>> octree);
+
+ /// Destructor
+ virtual ~OctreeNodePlyReaderDelegate();
+
+ /// \return the octree
+ std::shared_ptr<OctreeNode<Data>> getOctree();
+
+ void processVoxel(const std::string& elementName) override;
+
+ void initializeOctree() override;
+
+private:
+
+ /// The octree that will be filled with the data from the file
+ std::shared_ptr<OctreeNode<Data>> m_octree;
+};
+
+}
+}
+
+#include "SurgSim/DataStructures/OctreeNodePlyReaderDelegate-inl.h"
+
+#endif
diff --git a/SurgSim/DataStructures/PerformanceTests/CMakeLists.txt b/SurgSim/DataStructures/PerformanceTests/CMakeLists.txt
index 9f85eef..803223b 100644
--- a/SurgSim/DataStructures/PerformanceTests/CMakeLists.txt
+++ b/SurgSim/DataStructures/PerformanceTests/CMakeLists.txt
@@ -19,6 +19,7 @@ include_directories(
)
set(UNIT_TEST_SOURCES
+ GridPerformanceTest.cpp
NamedDataPerformanceTest.cpp
)
diff --git a/SurgSim/DataStructures/PerformanceTests/GridPerformanceTest.cpp b/SurgSim/DataStructures/PerformanceTests/GridPerformanceTest.cpp
new file mode 100644
index 0000000..9132a41
--- /dev/null
+++ b/SurgSim/DataStructures/PerformanceTests/GridPerformanceTest.cpp
@@ -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.
+
+#include <gtest/gtest.h>
+
+#include <boost/exception/to_string.hpp>
+
+#include <array>
+
+#include "SurgSim/Framework/Timer.h"
+#include "SurgSim/DataStructures/Grid.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// This class test the grid timings for a given concentration of elements per cell and a given number of element per
+/// dimension. These two information are embedded in GTest WithParamInterface which takes a
+/// tuple<double = concentrationPerCell, size_t = numElementsPerDimension>
+class Grid3DPerformanceTests : public ::testing::Test,
+ public ::testing::WithParamInterface<std::tuple<double, size_t>>
+{
+public:
+ typedef Eigen::Matrix<size_t, 3, 1> Vector3ui;
+
+ virtual void SetUp()
+ {
+ m_h = 0.1;
+ m_bounds.min().setConstant(-pow(2, 10) / 2.0);
+ m_bounds.max().setConstant(pow(2, 10) / 2.0);
+ m_grid = std::make_shared<Grid<size_t, 3>>(Eigen::Matrix<double, 3, 1>::Constant(m_h), m_bounds);
+ }
+
+ void addElementsUniformDistribution(const Vector3ui& numElementsPerAxis, double concentrationPerAxis)
+ {
+ double coef = m_h / static_cast<double>(concentrationPerAxis);
+ size_t elementId = 0;
+
+ for (size_t x = 0; x < numElementsPerAxis[0]; x++)
+ {
+ for (size_t y = 0; y < numElementsPerAxis[1]; y++)
+ {
+ for (size_t z = 0; z < numElementsPerAxis[2]; z++)
+ {
+ SurgSim::Math::Vector3d point(x * coef, y * coef, z * coef);
+ m_grid->addElement(elementId, point);
+ elementId++;
+ }
+ }
+ }
+ }
+
+ double performTimingTest(double concentrationPerCell, size_t numElementPerDimension)
+ {
+ SurgSim::Framework::Timer timer;
+ Vector3ui numElementsPerAxis = Vector3ui::Constant(numElementPerDimension);
+ double concentrationPerAxis = pow(concentrationPerCell, 1.0 / 3.0);
+
+ timer.start();
+
+ // Clear the grid from all previously added elements and clear the neighbor's list
+ m_grid->reset();
+ // Add all elements in the grid, triggering a dirty flag for the neighbor's list
+ addElementsUniformDistribution(numElementsPerAxis, concentrationPerAxis);
+ // Request any neighbor's list to force all neighbor's lists recalculation
+ m_grid->getNeighbors(0);
+
+ timer.endFrame();
+
+ return timer.getCumulativeTime();
+ }
+
+protected:
+ /// Grid size (cells are cubic in this test)
+ double m_h;
+
+ /// Grid boundary
+ Eigen::AlignedBox<double, 3> m_bounds;
+
+ /// Grid
+ std::shared_ptr<Grid<size_t, 3>> m_grid;
+};
+
+TEST_P(Grid3DPerformanceTests, Grid3DTest)
+{
+ double concentrationPerCell;
+ size_t numElementsPerDimension;
+ std::tie(concentrationPerCell, numElementsPerDimension) = GetParam();
+ size_t numElements = numElementsPerDimension * numElementsPerDimension * numElementsPerDimension;
+ RecordProperty("ElementsPerCell", boost::to_string(concentrationPerCell));
+ RecordProperty("NumberOfElements", boost::to_string(numElements));
+ RecordProperty("Duration", boost::to_string(performTimingTest(concentrationPerCell, numElementsPerDimension)));
+}
+
+INSTANTIATE_TEST_CASE_P(
+ Grid3D,
+ Grid3DPerformanceTests,
+ ::testing::Combine(
+ // Concentration per cell is fine between 1 and 3^3, then coarser
+ ::testing::Values(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, 19.0, 20.0, 21.0, 22.0, 23.0, 24.0, 25.0, 26.0, 27.0,
+ 4 * 4 * 4, 5 * 5 * 5, 6 * 6 * 6, 7 * 7 * 7, 8 * 8 * 8, 9 * 9 * 9, 10 * 10 * 10),
+ // Number of elements per dimension
+ ::testing::Values(50, 60, 60, 70, 80, 90, 100, 110, 120)));
+
+} // namespace DataStructures
+} // namespace SurgSim
diff --git a/SurgSim/DataStructures/PlyReader.cpp b/SurgSim/DataStructures/PlyReader.cpp
index e19c901..a69f896 100644
--- a/SurgSim/DataStructures/PlyReader.cpp
+++ b/SurgSim/DataStructures/PlyReader.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -57,7 +57,7 @@ struct PlyReader::Data
};
-PlyReader::PlyReader(std::string filename) :
+PlyReader::PlyReader(const std::string& filename) :
m_filename(filename), m_data(new Data())
{
m_data->plyFile = ply_open_for_reading(
@@ -91,7 +91,7 @@ bool PlyReader::isValid() const
return m_data->plyFile != nullptr;
}
-bool PlyReader::requestElement(std::string elementName,
+bool PlyReader::requestElement(const 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)
@@ -115,13 +115,14 @@ bool PlyReader::requestElement(std::string elementName,
return result;
}
-bool PlyReader::requestScalarProperty(std::string elementName, std::string propertyName, int dataType, int dataOffset)
+bool PlyReader::requestScalarProperty(const std::string& elementName, const std::string& propertyName,
+ int dataType, int dataOffset)
{
return requestProperty(elementName, propertyName, dataType, dataOffset, 0, 0);
}
-bool PlyReader::requestListProperty(std::string elementName,
- std::string propertyName,
+bool PlyReader::requestListProperty(const std::string& elementName,
+ const std::string& propertyName,
int dataType, int dataOffset,
int countType, int countOffset)
{
@@ -138,8 +139,8 @@ void PlyReader::setEndParseFileCallback(std::function<void (void)> endParseFileC
m_endParseFileCallback = endParseFileCallback;
}
-bool PlyReader::requestProperty(std::string elementName,
- std::string propertyName,
+bool PlyReader::requestProperty(const std::string& elementName,
+ const std::string& propertyName,
int dataType, int dataOffset,
int countType, int countOffset)
{
@@ -266,7 +267,25 @@ void PlyReader::parseFile()
ply_get_element(m_data->plyFile, readBuffer);
if (elementInfo.processElementCallback != nullptr)
{
- elementInfo.processElementCallback(currentElementName);
+ try
+ {
+ elementInfo.processElementCallback(currentElementName);
+ }
+ catch (const std::exception&)
+ {
+ for (size_t i = 0; i<listOffsets.size(); ++i)
+ {
+ void** item = (void **)((char *)readBuffer + listOffsets[i]); // NOLINT
+ free(item[0]);
+ }
+ for (int i = 0; i < propertyCount; ++i)
+ {
+ free(properties[i]->name);
+ free(properties[i]);
+ }
+ free(properties);
+ throw;
+ }
}
// Free the lists that where allocated by plyreader
@@ -321,14 +340,14 @@ bool PlyReader::parseWithDelegate(std::shared_ptr<PlyReaderDelegate> delegate)
return result;
}
-bool PlyReader::hasElement(std::string elementName) const
+bool PlyReader::hasElement(const 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
+bool PlyReader::hasProperty(const std::string& elementName, const std::string& propertyName) const
{
SURGSIM_ASSERT(isValid()) << "'" << m_filename << "' is an invalid .ply file";
@@ -342,7 +361,7 @@ bool PlyReader::hasProperty(std::string elementName, std::string propertyName) c
return result;
}
-bool PlyReader::isScalar(std::string elementName, std::string propertyName) const
+bool PlyReader::isScalar(const std::string& elementName, const std::string& propertyName) const
{
SURGSIM_ASSERT(isValid()) << "'" << m_filename << "' is an invalid .ply file";
diff --git a/SurgSim/DataStructures/PlyReader.h b/SurgSim/DataStructures/PlyReader.h
index c159d2f..60821dc 100644
--- a/SurgSim/DataStructures/PlyReader.h
+++ b/SurgSim/DataStructures/PlyReader.h
@@ -112,7 +112,7 @@ public:
/// Constructor.
/// \param filename Filename of the .ply file.
- explicit PlyReader(std::string filename);
+ explicit PlyReader(const std::string& filename);
/// Destructor.
virtual ~PlyReader();
@@ -129,7 +129,7 @@ public:
/// \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,
+ bool requestElement(const 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);
@@ -147,7 +147,8 @@ public:
/// \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);
+ bool requestScalarProperty(const std::string& elementName, const 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
@@ -163,27 +164,27 @@ public:
/// \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,
+ bool requestListProperty(const std::string& elementName,
+ const 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;
+ bool hasElement(const 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;
+ bool hasProperty(const std::string& elementName, const 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;
+ bool isScalar(const std::string& elementName, const std::string& propertyName) const;
/// Sets a delegate for parsing and then parse the file.
/// \param delegate The delegate.
@@ -217,8 +218,8 @@ private:
/// \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,
+ bool requestProperty(const std::string& elementName,
+ const std::string& propertyName,
int dataType, int dataOffset,
int countType, int countOffset);
diff --git a/SurgSim/DataStructures/SegmentEmptyData.h b/SurgSim/DataStructures/SegmentEmptyData.h
new file mode 100644
index 0000000..e4c027c
--- /dev/null
+++ b/SurgSim/DataStructures/SegmentEmptyData.h
@@ -0,0 +1,41 @@
+// 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_SEGMENTEMPTYDATA_H
+#define SURGSIM_DATASTRUCTURES_SEGMENTEMPTYDATA_H
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+/// SegmentEmptyData class
+/// This class is to ensure that the static TriangleMesh::m_className in SegmentMesh does not clash with
+/// static TriangleMesh<VertexData, EdgeData, EmptyData>.
+class SegmentEmptyData
+{
+public:
+
+ /// Comparison operator
+ /// \param data The data to compare it to.
+ /// \return true for all cases.
+ bool operator==(const SegmentEmptyData& data) const
+ {
+ return true;
+ }
+};
+
+}
+}
+#endif //SURGSIM_DATASTRUCTURES_SEGMENTEMPTYDATA_H
diff --git a/SurgSim/DataStructures/SegmentMesh-inl.h b/SurgSim/DataStructures/SegmentMesh-inl.h
new file mode 100644
index 0000000..afcc792
--- /dev/null
+++ b/SurgSim/DataStructures/SegmentMesh-inl.h
@@ -0,0 +1,243 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_SEGMENTMESH_INL_H
+#define SURGSIM_DATASTRUCTURES_SEGMENTMESH_INL_H
+
+#include "SurgSim/Framework/Log.h"
+
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <class VertexData, class EdgeData>
+SegmentMesh<VertexData, EdgeData>::SegmentMesh()
+{
+}
+
+template <class VertexData, class EdgeData>
+SegmentMesh<VertexData, EdgeData>::SegmentMesh(
+ const SegmentMesh<VertexData, EdgeData>& other) :
+ TriangleMeshType(other)
+{
+}
+
+template <class VertexData, class EdgeData>
+template <class V, class E>
+SegmentMesh<VertexData, EdgeData>::SegmentMesh(const SegmentMesh<V, E>& other) :
+ TriangleMeshType(other)
+{
+}
+
+template <class VertexData, class EdgeData>
+SegmentMesh<VertexData, EdgeData>::~SegmentMesh()
+{
+}
+
+template <class VertexData, class EdgeData>
+SegmentMesh<VertexData, EdgeData>::SegmentMesh(SegmentMesh<VertexData, EdgeData>&& other) :
+ TriangleMeshType(std::move(other))
+{
+}
+
+template <class VertexData, class EdgeData>
+SegmentMesh<VertexData, EdgeData>& SegmentMesh<VertexData, EdgeData>::operator=(
+ const SegmentMesh<VertexData, EdgeData>& other)
+{
+ return TriangleMeshType::operator=(other);
+}
+
+template <class VertexData, class EdgeData>
+SegmentMesh<VertexData, EdgeData>& SegmentMesh<VertexData, EdgeData>::operator=(
+ SegmentMesh<VertexData, EdgeData>&& other)
+{
+ return TriangleMeshType::operator=(std::move(other));
+}
+
+template <class VertexData, class EdgeData>
+size_t SegmentMesh<VertexData, EdgeData>::addTriangle(const TriangleType& triangle)
+{
+ SURGSIM_FAILURE() << "Cannot insert triangle into segment mesh.";
+ return static_cast<size_t>(0);
+}
+
+template <class VertexData, class EdgeData>
+size_t SegmentMesh<VertexData, EdgeData>::getNumTriangles() const
+{
+ SURGSIM_FAILURE() << "No triangles present in segment mesh.";
+ return static_cast<size_t>(0);
+}
+
+template <class VertexData, class EdgeData>
+const std::vector<typename SegmentMesh<VertexData, EdgeData>::TriangleType>&
+SegmentMesh<VertexData, EdgeData>::getTriangles() const
+{
+ SURGSIM_FAILURE() << "No triangles present in segment mesh.";
+ return TriangleMeshType::getTriangles();
+}
+
+template <class VertexData, class EdgeData>
+std::vector<typename SegmentMesh<VertexData, EdgeData>::TriangleType>&
+SegmentMesh<VertexData, EdgeData>::getTriangles()
+{
+ SURGSIM_FAILURE() << "No triangles present in segment mesh.";
+ return TriangleMeshType::getTriangles();
+}
+
+template <class VertexData, class EdgeData>
+const typename SegmentMesh<VertexData, EdgeData>::TriangleType&
+SegmentMesh<VertexData, EdgeData>::getTriangle(size_t id) const
+{
+ SURGSIM_FAILURE() << "No triangles present in segment mesh.";
+ return TriangleMeshType::getTriangle(id);
+}
+
+template <class VertexData, class EdgeData>
+typename SegmentMesh<VertexData, EdgeData>::TriangleType&
+SegmentMesh<VertexData, EdgeData>::getTriangle(size_t id)
+{
+ SURGSIM_FAILURE() << "No triangles present in segment mesh.";
+ return TriangleMeshType::getTriangle(id);
+}
+
+template <class VertexData, class EdgeData>
+void SegmentMesh<VertexData, EdgeData>::removeTriangle(size_t id)
+{
+ SURGSIM_FAILURE() << "No triangles present in segment mesh.";
+}
+
+template <class VertexData, class EdgeData>
+std::array<SurgSim::Math::Vector3d, 3>
+SegmentMesh<VertexData, EdgeData>::getTrianglePositions(size_t id) const
+{
+ using SurgSim::Math::Vector3d;
+ SURGSIM_FAILURE() << "No triangles present in segment mesh.";
+ std::array<Vector3d, 3> result =
+ {
+ {
+ Vector3d::Zero(),
+ Vector3d::Zero(),
+ Vector3d::Zero()
+ }
+ };
+ return result;
+}
+
+template <class VertexData, class EdgeData>
+void SegmentMesh<VertexData, EdgeData>::doClearTriangles()
+{
+ SURGSIM_FAILURE() << "No triangles present in segment mesh.";
+}
+
+template <class VertexData, class EdgeData>
+void SegmentMesh<VertexData, EdgeData>::doClear()
+{
+ TriangleMeshType::doClearEdges();
+ TriangleMeshType::doClearVertices();
+}
+
+template <class VertexData, class EdgeData>
+void SurgSim::DataStructures::SegmentMesh<VertexData, EdgeData>::createDefaultEdges()
+{
+ doClearEdges();
+ for (size_t i = 0; i < getNumVertices() - 1; ++i)
+ {
+ std::array<size_t, 2> vertices = { i, i + 1 };
+ EdgeType edge(vertices);
+ addEdge(edge);
+ }
+}
+
+
+template <class VertexData, class EdgeData>
+bool SegmentMesh<VertexData, EdgeData>::save(const std::string& fileName,
+ bool asPhysics,
+ double radius,
+ double massDensity,
+ double poissonRatio,
+ double youngsModulus)
+{
+ std::fstream out(fileName, std::ios::out);
+
+ if (out.is_open())
+ {
+ out << "ply" << std::endl;
+ out << "format ascii 1.0" << std::endl;
+ out << "comment Created by OpenSurgSim, www.opensurgsim.org" << std::endl;
+ out << "element vertex " << getNumVertices() << std::endl;
+ out << "property float x\nproperty float y\nproperty float z" << std::endl;
+ if (asPhysics)
+ {
+ out << "element 1d_element " << getNumEdges() << std::endl;
+ out << "property list uint uint vertex_indices" << std::endl;
+ out << "element radius 1" << std::endl;
+ out << "property double value" << std::endl;
+ out << "element material 1" << std::endl;
+ out << "property double mass_density" << std::endl;
+ out << "property double poisson_ratio" << std::endl;
+ out << "property double young_modulus" << std::endl;
+ out << "element boundary_condition 0" << std::endl;
+ out << "property uint vertex_index" << std::endl;
+ }
+ else
+ {
+ out << "element edge " << getNumEdges() << std::endl;
+ out << "property uint vertex1" << std::endl;
+ out << "property uint vertex2" << std::endl;
+ }
+ out << "end_header" << std::endl;
+ for (const auto& vertex : getVertices())
+ {
+ out << vertex.position[0] << " " << vertex.position[1] << " " << vertex.position[2] << std::endl;
+ }
+
+ for (const auto& edge : getEdges())
+ {
+ if (asPhysics)
+ {
+ out << "2 ";
+ }
+ out << edge.verticesId[0] << " " << edge.verticesId[1] << std::endl;
+ }
+ if (asPhysics)
+ {
+ out << radius << std::endl;
+ out << massDensity << " " << poissonRatio << " " << youngsModulus << std::endl;
+ }
+
+ if (out.bad())
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) << __FUNCTION__
+ << "There was a problem writing " << fileName;
+ }
+
+ out.close();
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) << __FUNCTION__
+ << "Could not open " << fileName << " for writing.";
+ return false;
+ }
+ return true;
+}
+
+
+} // namespace DataStructures
+} // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_SEGMENTMESH_INL_H
diff --git a/SurgSim/DataStructures/SegmentMesh.cpp b/SurgSim/DataStructures/SegmentMesh.cpp
new file mode 100644
index 0000000..694f1b8
--- /dev/null
+++ b/SurgSim/DataStructures/SegmentMesh.cpp
@@ -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.
+
+#include "SurgSim/DataStructures/SegmentMesh.h"
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+
+template <>
+std::string SegmentMesh<EmptyData, EmptyData>::TriangleMeshType::m_className =
+ "SurgSim::DataStructures::SegmentMeshPlain";
+
+} // namespace DataStructures
+} // namespace SurgSim
diff --git a/SurgSim/DataStructures/SegmentMesh.h b/SurgSim/DataStructures/SegmentMesh.h
new file mode 100644
index 0000000..911521a
--- /dev/null
+++ b/SurgSim/DataStructures/SegmentMesh.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_DATASTRUCTURES_SEGMENTMESH_H
+#define SURGSIM_DATASTRUCTURES_SEGMENTMESH_H
+
+#include "SurgSim/DataStructures/SegmentEmptyData.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+/// Class to hold the type of a SegmentMesh.
+///
+/// \tparam VertexData Type of extra data stored in each vertex
+/// \tparam EdgeData Type of extra data stored in each edge
+/// \sa TriangleMesh
+template <class VertexData, class EdgeData>
+class SegmentMesh : public TriangleMesh<VertexData, EdgeData, SegmentEmptyData>
+{
+public:
+ /// TriangleMesh type for convenience
+ typedef TriangleMesh<VertexData, EdgeData, SegmentEmptyData> TriangleMeshType;
+ /// Edge type for convenience (Ids of the 2 vertices)
+ typedef typename TriangleMeshType::EdgeType EdgeType;
+ /// Triangle type for convenience (Ids of the 3 vertices)
+ typedef typename TriangleMeshType::TriangleType TriangleType;
+
+ /// Constructor. The mesh is initially empty (no vertices, no edges).
+ SegmentMesh();
+
+ /// Copy constructor when the template data is the same type
+ /// \param other the mesh to copy from
+ SegmentMesh(const SegmentMesh<VertexData, EdgeData>& other);
+
+ /// Copy constructor when the template data is a different type
+ /// \tparam V Type of extra data stored in each vertex
+ /// \tparam E Type of extra data stored in each edge
+ /// \param other The mesh to be copied from. Vertex and edge data will not be copied
+ /// \note: Data of the input mesh, i.e. VertexDataSource, EdgeDataSource will not be copied.
+ template <class V, class E>
+ explicit SegmentMesh(const SegmentMesh<V, E>& other);
+
+ /// Destructor
+ virtual ~SegmentMesh();
+
+ /// Move Constructor
+ /// \param other Constructor source
+ SegmentMesh(SegmentMesh&& other);
+
+ /// Copy Assignment
+ /// \param other Assignment source
+ SegmentMesh<VertexData, EdgeData>& operator=(
+ const SegmentMesh<VertexData, EdgeData>& other);
+
+ /// Move Assignment
+ /// \param other Assignment source
+ SegmentMesh<VertexData, EdgeData>& operator=(
+ SegmentMesh<VertexData, EdgeData>&& other);
+
+ /// Creates edges for all vertices in the mesh connecting all the points consecutively
+ /// \note This will clear all the current edges
+ void createDefaultEdges();
+
+ /// Save the current structure to a ply file
+ /// \param fileName Name of the file for writing
+ /// \param asPhysics Format the file to be used as a physics file, otherwise the file will be ply format
+ /// with 'edge' as the edge element
+ /// \param radius if asPhysics is true, this will be used as the radius for the ply file
+ /// \param massDensity if asPhysics is true, this will be used as the massDensity for the ply file
+ /// \param poissonRatio if asPhysics is true, this will be used as the poissonRatio for the ply file
+ /// \param youngsModulus if asPhysics is true, this will be used as the youngsModulus for the ply file
+ /// \return true if the file was written successfully
+ bool save(const std::string& fileName,
+ bool asPhysics = true,
+ double radius = 0.0001,
+ double massDensity = 900,
+ double poissonRatio = 0.45,
+ double youngsModulus = 1.75e9);
+
+ using TriangleMesh<VertexData, EdgeData, SegmentEmptyData>::addEdge;
+ using TriangleMesh<VertexData, EdgeData, SegmentEmptyData>::doClearEdges;
+ using TriangleMesh<VertexData, EdgeData, SegmentEmptyData>::getNumEdges;
+ using TriangleMesh<VertexData, EdgeData, SegmentEmptyData>::getNumVertices;
+ using TriangleMesh<VertexData, EdgeData, SegmentEmptyData>::getEdges;
+ using TriangleMesh<VertexData, EdgeData, SegmentEmptyData>::getVertices;
+
+ ///@{
+ /// Functions that need to assert, because they deal with triangles.
+ size_t addTriangle(const TriangleType& triangle);
+ size_t getNumTriangles() const;
+ const std::vector<TriangleType>& getTriangles() const;
+ std::vector<TriangleType>& getTriangles();
+ const TriangleType& getTriangle(size_t id) const;
+ TriangleType& getTriangle(size_t id);
+ void removeTriangle(size_t id);
+ std::array<SurgSim::Math::Vector3d, 3> getTrianglePositions(size_t id) const;
+ void doClearTriangles() override;
+ ///@}
+
+private:
+ /// Clear mesh to return to an empty state (no vertices, no edges).
+ void doClear() override;
+};
+
+typedef SegmentMesh<EmptyData, EmptyData> SegmentMeshPlain;
+
+} // namespace DataStructures
+} // namespace SurgSim
+
+#include "SurgSim/DataStructures/SegmentMesh-inl.h"
+
+#endif // SURGSIM_DATASTRUCTURES_SEGMENTMESH_H
diff --git a/SurgSim/DataStructures/TriangleMesh-inl.h b/SurgSim/DataStructures/TriangleMesh-inl.h
index 4f5216a..01c183c 100644
--- a/SurgSim/DataStructures/TriangleMesh-inl.h
+++ b/SurgSim/DataStructures/TriangleMesh-inl.h
@@ -12,24 +12,381 @@
// WITHOUT WARRANTIES OR 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
+#include "SurgSim/Framework/Log.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)
+template <class VertexData, class EdgeData, class TriangleData>
+TriangleMesh<VertexData, EdgeData, TriangleData>::TriangleMesh()
+{
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+TriangleMesh<VertexData, EdgeData, TriangleData>::TriangleMesh(
+ const TriangleMesh<VertexData, EdgeData, TriangleData>& other) :
+ Vertices<VertexData>::Vertices(other),
+ SurgSim::Framework::Asset(),
+ m_edges(other.getEdges()),
+ m_triangles(other.getTriangles()),
+ m_freeTriangles(other.m_freeTriangles)
+{
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+template <class V, class E, class T>
+TriangleMesh<VertexData, EdgeData, TriangleData>::TriangleMesh(const TriangleMesh<V, E, T>& other) :
+ Vertices<VertexData>::Vertices(other),
+ SurgSim::Framework::Asset()
+{
+ m_edges.reserve(other.getEdges().size());
+ for (auto& edge : other.getEdges())
+ {
+ addEdge(EdgeType(edge));
+ }
+
+ size_t index = 0;
+ m_triangles.reserve(other.getTriangles().size());
+ for (auto& triangle : other.getTriangles())
+ {
+ addTriangle(TriangleType(triangle));
+ if (!triangle.isValid)
+ {
+ m_freeTriangles.push_back(index);
+ }
+ ++index;
+ }
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+TriangleMesh<VertexData, EdgeData, TriangleData>::~TriangleMesh()
+{
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+std::string TriangleMesh<VertexData, EdgeData, TriangleData>::getClassName() const
+{
+ return m_className;
+}
+
+
+template <class VertexData, class EdgeData, class TriangleData>
+size_t TriangleMesh<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 TriangleMesh<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 TriangleMesh<VertexData, EdgeData, TriangleData>::getNumEdges() const
+{
+ return m_edges.size();
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+size_t TriangleMesh<VertexData, EdgeData, TriangleData>::getNumTriangles() const
+{
+ return m_triangles.size() - m_freeTriangles.size();
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+const std::vector<typename TriangleMesh<VertexData, EdgeData, TriangleData>::EdgeType>&
+TriangleMesh<VertexData, EdgeData, TriangleData>::getEdges() const
+{
+ return m_edges;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+std::vector<typename TriangleMesh<VertexData, EdgeData, TriangleData>::EdgeType>&
+TriangleMesh<VertexData, EdgeData, TriangleData>::getEdges()
+{
+ return m_edges;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+const std::vector<typename TriangleMesh<VertexData, EdgeData, TriangleData>::TriangleType>&
+TriangleMesh<VertexData, EdgeData, TriangleData>::getTriangles() const
+{
+ return m_triangles;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+std::vector<typename TriangleMesh<VertexData, EdgeData, TriangleData>::TriangleType>&
+TriangleMesh<VertexData, EdgeData, TriangleData>::getTriangles()
+{
+ return m_triangles;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+const typename TriangleMesh<VertexData, EdgeData, TriangleData>::EdgeType&
+TriangleMesh<VertexData, EdgeData, TriangleData>::getEdge(size_t id) const
+{
+ return m_edges[id];
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+typename TriangleMesh<VertexData, EdgeData, TriangleData>::EdgeType&
+TriangleMesh<VertexData, EdgeData, TriangleData>::getEdge(size_t id)
+{
+ return m_edges[id];
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+std::array<SurgSim::Math::Vector3d, 2>
+TriangleMesh<VertexData, EdgeData, TriangleData>::getEdgePositions(size_t id) const
+{
+ auto& ids = getEdge(id).verticesId;
+ std::array<SurgSim::Math::Vector3d, 2> result =
+ {{
+ Vertices<VertexData>::getVertex(ids[0]).position,
+ Vertices<VertexData>::getVertex(ids[1]).position
+ }};
+
+ return result;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+const typename TriangleMesh<VertexData, EdgeData, TriangleData>::TriangleType&
+TriangleMesh<VertexData, EdgeData, TriangleData>::getTriangle(size_t id) const
+{
+ auto const& triangle = m_triangles[id];
+ SURGSIM_ASSERT(triangle.isValid)
+ << "Attempted to access invalid or deleted triangle " << id << " have " << getNumTriangles();
+ return triangle;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+typename TriangleMesh<VertexData, EdgeData, TriangleData>::TriangleType&
+TriangleMesh<VertexData, EdgeData, TriangleData>::getTriangle(size_t id)
+{
+ auto& triangle = m_triangles[id];
+ SURGSIM_ASSERT(triangle.isValid) << "Attempted to access invalid or deleted triangle.";
+ return triangle;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+void TriangleMesh<VertexData, EdgeData, TriangleData>::removeTriangle(size_t id)
+{
+ auto& triangle = m_triangles[id];
+ if (triangle.isValid)
+ {
+ triangle.isValid = false;
+ m_freeTriangles.push_back(id);
+ }
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+std::array<SurgSim::Math::Vector3d, 3>
+TriangleMesh<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 TriangleMesh<VertexData, EdgeData, TriangleData>::isValid() const
+{
+ typedef typename TriangleMesh<VertexData, EdgeData, TriangleData>::EdgeType EdgeType;
+ typedef typename TriangleMesh<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 TriangleMesh<VertexData, EdgeData, TriangleData>::doClearEdges()
+{
+ m_edges.clear();
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+void TriangleMesh<VertexData, EdgeData, TriangleData>::doClearTriangles()
+{
+ m_triangles.clear();
+ m_freeTriangles.clear();
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+bool TriangleMesh<VertexData, EdgeData, TriangleData>::isEqual(const Vertices<VertexData>& mesh) const
+{
+ const TriangleMesh& triangleMesh = static_cast<const TriangleMesh&>(mesh);
+ return Vertices<VertexData>::isEqual(triangleMesh) && m_edges == triangleMesh.getEdges() &&
+ m_triangles == triangleMesh.getTriangles();
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+bool TriangleMesh<VertexData, EdgeData, TriangleData>::doLoad(const std::string& fileName)
+{
+ PlyReader reader(fileName);
+ if (! reader.isValid())
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger())
+ << "'" << fileName << "' is an invalid .ply file.";
+ return false;
+ }
+
+ typedef TriangleMesh<VertexData, EdgeData, TriangleData> MeshType;
+ auto delegate = std::make_shared<TriangleMeshPlyReaderDelegate<MeshType>>(this->shared_from_this());
+ if (! reader.parseWithDelegate(delegate))
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger())
+ << "The input file '" << fileName << "' does not have the property required by triangle mesh.";
+ return false;
+ }
+
+ return true;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+void TriangleMesh<VertexData, EdgeData, TriangleData>::doClear()
+{
+ doClearTriangles();
+ doClearEdges();
+ doClearVertices();
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+TriangleMesh<VertexData, EdgeData, TriangleData>::TriangleMesh(TriangleMesh&& other) :
+ Vertices<VertexData>::Vertices(std::move(other))
+{
+ doClearTriangles();
+ doClearEdges();
+ std::swap(m_triangles, other.m_triangles);
+ std::swap(m_edges, other.m_edges);
+ std::swap(m_freeTriangles, other.m_freeTriangles);
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+TriangleMesh<VertexData, EdgeData, TriangleData>& TriangleMesh<VertexData, EdgeData, TriangleData>::operator=(const
+ TriangleMesh<VertexData, EdgeData, TriangleData>& other)
+{
+ Vertices<VertexData>::operator=(other);
+ m_triangles = other.m_triangles;
+ m_edges = other.m_edges;
+ m_freeTriangles = other.m_freeTriangles;
+ return *this;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+TriangleMesh<VertexData, EdgeData, TriangleData>& TriangleMesh<VertexData, EdgeData, TriangleData>::operator=
+(TriangleMesh<VertexData, EdgeData, TriangleData>&& other)
{
- calculateNormals();
+ Vertices<VertexData>::operator=(std::move(other));
+ doClearTriangles();
+ doClearEdges();
+ std::swap(m_triangles, other.m_triangles);
+ std::swap(m_edges, other.m_edges);
+ std::swap(m_freeTriangles, other.m_freeTriangles);
+ return *this;
+}
+
+template <class VertexData, class EdgeData, class TriangleData>
+void SurgSim::DataStructures::TriangleMesh<VertexData, EdgeData, TriangleData>::save(const std::string& fileName)
+{
+ std::fstream out(fileName, std::ios::out);
+
+ if (out.is_open())
+ {
+ out << "ply" << std::endl;
+ out << "format ascii 1.0" << std::endl;
+ out << "comment Created by OpenSurgSim, www.opensurgsim.org" << std::endl;
+ out << "element vertex " << getNumVertices() << std::endl;
+ out << "property float x\nproperty float y\nproperty float z" << std::endl;
+ out << "element face " << getNumTriangles() << std::endl;
+ out << "property list uchar uint vertex_indices" << std::endl;
+ out << "end_header" << std::endl;
+
+ for (const auto& vertex : getVertices())
+ {
+ out << vertex.position[0] << " " << vertex.position[1] << " " << vertex.position[2] << std::endl;
+ }
+
+ for (const auto& tri : getTriangles())
+ {
+ out << "3 " << tri.verticesId[0] << " " << tri.verticesId[1] << " " << tri.verticesId[2] << std::endl;
+ }
+
+ if (out.bad())
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) << __FUNCTION__
+ << "There was a problem writing " << fileName;
+ }
+
+ out.close();
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) << __FUNCTION__
+ << "Could not open " << fileName << " for writing.";
+ }
+
}
-}; // namespace DataStructures
-}; // namespace SurgSim
-#endif // SURGSIM_DATASTRUCTURES_TRIANGLEMESH_INL_H
\ No newline at end of file
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_TRIANGLEMESH_INL_H
diff --git a/SurgSim/DataStructures/TriangleMesh.cpp b/SurgSim/DataStructures/TriangleMesh.cpp
index 7e975b9..ad713b4 100644
--- a/SurgSim/DataStructures/TriangleMesh.cpp
+++ b/SurgSim/DataStructures/TriangleMesh.cpp
@@ -13,90 +13,17 @@
// 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)
+namespace DataStructures
{
- 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;
- }
+template<>
+std::string TriangleMeshPlain::m_className = "SurgSim::DataStructures::TriangleMeshPlain";
- 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
-}; // namespace DataStructures
-}; // namespace SurgSim
diff --git a/SurgSim/DataStructures/TriangleMesh.h b/SurgSim/DataStructures/TriangleMesh.h
index b72221b..569ad2f 100644
--- a/SurgSim/DataStructures/TriangleMesh.h
+++ b/SurgSim/DataStructures/TriangleMesh.h
@@ -16,92 +16,241 @@
#ifndef SURGSIM_DATASTRUCTURES_TRIANGLEMESH_H
#define SURGSIM_DATASTRUCTURES_TRIANGLEMESH_H
+#include <array>
+#include <memory>
+
#include "SurgSim/DataStructures/EmptyData.h"
-#include "SurgSim/DataStructures/TriangleMeshBase.h"
+#include "SurgSim/DataStructures/MeshElement.h"
+#include "SurgSim/DataStructures/NormalData.h"
+#include "SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h"
+#include "SurgSim/DataStructures/Vertices.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
+/// Basic class for storing Triangle Meshes, handling basic vertex, edge, and triangle functionality.
+///
+/// TriangleMesh 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 TriangleMesh, but may use a TriangleMesh 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 TriangleMesh.
+///
+/// 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 TriangleMesh : public Vertices<VertexData>, public SurgSim::Framework::Asset,
+ public std::enable_shared_from_this<TriangleMesh<VertexData, EdgeData, TriangleData>>
{
+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;
- 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);
- }
-};
+ /// Constructor. The mesh is initially empty (no vertices, no edges, no triangles).
+ TriangleMesh();
-typedef TriangleMeshBase<EmptyData, EmptyData, EmptyData> TriangleMeshPlain;
+ /// Copy constructor when the template data is the same type
+ /// \param other the mesh to copy from
+ TriangleMesh(const TriangleMesh<VertexData, EdgeData, TriangleData>& other);
-/// 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:
+ /// Copy constructor when the template data is a different type
+ /// \tparam V Type of extra data stored in each vertex
+ /// \tparam E Type of extra data stored in each edge
+ /// \tparam T Type of extra data stored in each triangle
+ /// \param other The mesh to be copied from. Vertex, edge and triangle data will not be copied
+ /// \note: Data of the input mesh, i.e. VertexDataSource, EdgeDataSource and TrianleDataSource will not be copied.
+ template <class V, class E, class T>
+ explicit TriangleMesh(const TriangleMesh<V, E, T>& other);
- /// Constructor
- TriangleMesh();
+ /// Destructor
+ virtual ~TriangleMesh();
+
+ /// Move Constructor
+ /// \param other Constructor source
+ TriangleMesh(TriangleMesh&& other);
+
+ /// Copy Assignment
+ /// \param other Assignment source
+ TriangleMesh<VertexData, EdgeData, TriangleData>& operator=(
+ const TriangleMesh<VertexData, EdgeData, TriangleData>& other);
+
+ /// Move Assignment
+ /// \param other Assignment source
+ TriangleMesh<VertexData, EdgeData, TriangleData>& operator=(
+ TriangleMesh<VertexData, EdgeData, TriangleData>&& other);
+
+
+ std::string getClassName() const override;
+
+ /// 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;
- /// 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);
+ /// 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);
+
+ /// Returns an array of the edge's vertices' positions
+ /// \param id the id of the edge
+ /// \return an array of the edge's vertices' positions
+ std::array<SurgSim::Math::Vector3d, 2> getEdgePositions(size_t id) const;
+
+ /// 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 TriangleMesh is valid (valid vertex Ids used in all MeshElements)
+ /// \return True if the TriangleMesh is valid, False otherwise (the topology is then broken)
+ bool isValid() const;
+
+ /// Save the triangle mesh in the ply format
+ /// \param fileName the filename where to save
+ void save(const std::string& fileName);
protected:
- virtual void doUpdate() override;
+ /// 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 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
+ /// \return True if the vertices are equals, False otherwise
+ virtual bool isEqual(const Vertices<VertexData>& mesh) const;
+
+ bool doLoad(const std::string& fileName) override;
+
+ using Vertices<VertexData>::doClearVertices;
+
+ static std::string m_className;
+
+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;
+ using Vertices<VertexData>::getNumVertices;
+ using Vertices<VertexData>::getVertices;
- virtual bool doLoad(const std::string& fileName) override;
};
-}; // namespace DataStructures
-}; // namespace SurgSim
+typedef TriangleMesh<EmptyData, EmptyData, EmptyData> TriangleMeshPlain;
+
+}; // namespace DataStructures
+}; // namespace SurgSim
#include "SurgSim/DataStructures/TriangleMesh-inl.h"
-#endif // SURGSIM_DATASTRUCTURES_TRIANGLEMESH_H
+#endif // SURGSIM_DATASTRUCTURES_TRIANGLEMESH_H
diff --git a/SurgSim/DataStructures/TriangleMeshBase-inl.h b/SurgSim/DataStructures/TriangleMeshBase-inl.h
deleted file mode 100644
index 75cd7cb..0000000
--- a/SurgSim/DataStructures/TriangleMeshBase-inl.h
+++ /dev/null
@@ -1,263 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
deleted file mode 100644
index 5e8cf3a..0000000
--- a/SurgSim/DataStructures/TriangleMeshBase.h
+++ /dev/null
@@ -1,211 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
index 9977115..23dbd19 100644
--- a/SurgSim/DataStructures/TriangleMeshPlyReaderDelegate-inl.h
+++ b/SurgSim/DataStructures/TriangleMeshPlyReaderDelegate-inl.h
@@ -16,16 +16,27 @@
#ifndef SURGSIM_DATASTRUCTURES_TRIANGLEMESHPLYREADERDELEGATE_INL_H
#define SURGSIM_DATASTRUCTURES_TRIANGLEMESHPLYREADERDELEGATE_INL_H
+#include <cstddef>
+
+#include "SurgSim/Math/Vector.h"
+
template <class M>
SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::TriangleMeshPlyReaderDelegate() :
- m_mesh(std::make_shared<M>())
+ m_mesh(std::make_shared<M>()),
+ m_hasTextureCoordinates(false),
+ m_hasFaces(false),
+ m_hasEdges(false)
+
{
}
template <class M>
SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::TriangleMeshPlyReaderDelegate(std::shared_ptr<M> mesh) :
- m_mesh(mesh)
+ m_mesh(mesh),
+ m_hasTextureCoordinates(false),
+ m_hasFaces(false),
+ m_hasEdges(false)
{
SURGSIM_ASSERT(mesh != nullptr) << "The mesh cannot be null.";
mesh->clear();
@@ -59,17 +70,37 @@ bool SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::registerDelegate
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));
+
+ if (m_hasFaces)
+ {
+ // 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(ListData, indices),
+ PlyReader::TYPE_UNSIGNED_INT,
+ offsetof(ListData, count));
+ }
+
+
+ if (m_hasEdges)
+ {
+ // Edge Processing
+ reader->requestElement("1d_element",
+ std::bind(&TriangleMeshPlyReaderDelegate::beginEdges, this,
+ std::placeholders::_1, std::placeholders::_2),
+ std::bind(&TriangleMeshPlyReaderDelegate::processEdge, this, std::placeholders::_1),
+ std::bind(&TriangleMeshPlyReaderDelegate::endEdges, this, std::placeholders::_1));
+ reader->requestListProperty("1d_element", "vertex_indices",
+ PlyReader::TYPE_UNSIGNED_INT,
+ offsetof(ListData, indices),
+ PlyReader::TYPE_UNSIGNED_INT,
+ offsetof(ListData, count));
+ }
reader->setEndParseFileCallback(std::bind(&TriangleMeshPlyReaderDelegate::endFile, this));
@@ -81,12 +112,17 @@ bool SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::fileIsAcceptable
{
bool result = true;
+ m_hasFaces = reader.hasProperty("face", "vertex_indices") &&
+ !reader.isScalar("face", "vertex_indices");
+
+ m_hasEdges = reader.hasProperty("1d_element", "vertex_indices") &&
+ !reader.isScalar("1d_element", "vertex_indices");
+
// 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");
+ result = result && (m_hasFaces || m_hasEdges);
return result;
}
@@ -121,24 +157,24 @@ void* SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::beginFaces(
const std::string& elementName,
size_t faceCount)
{
- m_faceData.overrun = 0l;
- return &m_faceData;
+ m_listData.overrun = 0l;
+ return &m_listData;
}
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());
+ SURGSIM_ASSERT(m_listData.count == 3) << "Can only process triangle meshes.";
+ std::copy(m_listData.indices, m_listData.indices + 3, m_face.begin());
- typename M::TriangleType triangle(m_indices);
+ typename M::TriangleType triangle(m_face);
m_mesh->addTriangle(triangle);
}
template <class M>
void SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::endFaces(const std::string& elementName)
{
- SURGSIM_ASSERT(m_faceData.overrun == 0l)
+ SURGSIM_ASSERT(m_listData.overrun == 0l)
<< "There was an overrun while reading the face structures, it is likely that data "
<< "has become corrupted.";
}
@@ -155,5 +191,33 @@ bool SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::hasTextureCoordi
return m_hasTextureCoordinates;
}
+template <class M>
+void* SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::beginEdges(const std::string& elementName,
+ size_t edgeCount)
+{
+ m_listData.overrun = 0l;
+ return &m_listData;
+}
+
+
+template <class M>
+void SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::processEdge(const std::string& elementName)
+{
+ SURGSIM_ASSERT(m_listData.count == 2) << "Edges have to have 2 points.";
+ std::copy(m_listData.indices, m_listData.indices + 2, m_edge.begin());
+
+ typename M::EdgeType edge(m_edge);
+ m_mesh->addEdge(edge);
+}
+
+
+template <class M>
+void SurgSim::DataStructures::TriangleMeshPlyReaderDelegate<M>::endEdges(const std::string& elementName)
+{
+ SURGSIM_ASSERT(m_listData.overrun == 0l)
+ << "There was an overrun while reading the face structures, it is likely that data "
+ << "has become corrupted.";
+}
+
#endif
diff --git a/SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h b/SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h
index 02b1fc5..fccde2a 100644
--- a/SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h
+++ b/SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h
@@ -22,7 +22,6 @@
#include "SurgSim/DataStructures/EmptyData.h"
#include "SurgSim/DataStructures/PlyReader.h"
#include "SurgSim/DataStructures/PlyReaderDelegate.h"
-#include "SurgSim/DataStructures/TriangleMeshBase.h"
namespace SurgSim
{
@@ -51,12 +50,12 @@ public:
/// 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;
+ 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;
+ bool fileIsAcceptable(const PlyReader& reader) override;
/// Callback function, begin the processing of vertices.
/// \param elementName Name of the element.
@@ -86,6 +85,12 @@ public:
/// \param elementName Name of the element.
void endFaces(const std::string& elementName);
+ void* beginEdges(const std::string& elementName, size_t edgeCount);
+
+ void processEdge(const std::string& elementName);
+
+ void endEdges(const std::string& elementName);
+
/// Callback function to finalize processing of the mesh
void endFile();
@@ -108,27 +113,34 @@ protected:
} m_vertexData;
/// Internal structure, the received for data from the "face" element
- struct FaceData
+ struct ListData
{
- unsigned int edgeCount;
+ unsigned int count;
unsigned int* indices;
int64_t overrun; ///< Used to check for buffer overruns
- } m_faceData;
+ } m_listData;
/// 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;
+ std::array<size_t, 3> m_face;
+ std::array<size_t, 2> m_edge;
private:
/// Set to true if s/t coordinates are found in the .ply file
bool m_hasTextureCoordinates;
+ /// Set to true if faces are found in the .ply file
+ bool m_hasFaces;
+
+ /// Set to true if edges are found in the .ply file
+ bool m_hasEdges;
};
-}
-}
+
+};
+};
#include "SurgSim/DataStructures/TriangleMeshPlyReaderDelegate-inl.h"
diff --git a/SurgSim/DataStructures/TriangleMeshUtilities-inl.h b/SurgSim/DataStructures/TriangleMeshUtilities-inl.h
deleted file mode 100644
index d4a2a1f..0000000
--- a/SurgSim/DataStructures/TriangleMeshUtilities-inl.h
+++ /dev/null
@@ -1,35 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
deleted file mode 100644
index ac1fa3b..0000000
--- a/SurgSim/DataStructures/TriangleMeshUtilities.cpp
+++ /dev/null
@@ -1,30 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 830b55f..0000000
--- a/SurgSim/DataStructures/TriangleMeshUtilities.h
+++ /dev/null
@@ -1,43 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/AabbTreeNodeTests.cpp b/SurgSim/DataStructures/UnitTests/AabbTreeNodeTests.cpp
index ec891d1..444cd1b 100644
--- a/SurgSim/DataStructures/UnitTests/AabbTreeNodeTests.cpp
+++ b/SurgSim/DataStructures/UnitTests/AabbTreeNodeTests.cpp
@@ -76,13 +76,13 @@ class TestVisitor : public TreeVisitor
public:
virtual ~TestVisitor() {}
- virtual bool handle(TreeNode* node) override
+ bool handle(TreeNode* node) override
{
throw std::logic_error("The method or operation is not implemented.");
return false;
}
- virtual bool handle(AabbTreeNode* node) override
+ bool handle(AabbTreeNode* node) override
{
return true;
}
diff --git a/SurgSim/DataStructures/UnitTests/AabbTreeTests.cpp b/SurgSim/DataStructures/UnitTests/AabbTreeTests.cpp
index 9d910c7..297ec59 100644
--- a/SurgSim/DataStructures/UnitTests/AabbTreeTests.cpp
+++ b/SurgSim/DataStructures/UnitTests/AabbTreeTests.cpp
@@ -20,9 +20,7 @@
#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/DataStructures/TriangleMesh.h"
#include "SurgSim/Framework/Runtime.h"
#include "SurgSim/Framework/Timer.h"
#include "SurgSim/Math/Aabb.h"
@@ -67,14 +65,12 @@ TEST(AabbTreeTests, AddTest)
TEST(AabbTreeTests, BuildTest)
{
- SurgSim::Framework::ApplicationData data("config.txt");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("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);
+ auto mesh = std::make_shared<TriangleMeshPlain>();
+ mesh->load("Geometry/arm_collision.ply");
- SurgSim::Framework::Timer timer;
for (size_t i = 0; i < mesh->getNumTriangles(); ++i)
{
auto triangle = mesh->getTriangle(i);
@@ -82,9 +78,29 @@ TEST(AabbTreeTests, BuildTest)
mesh->getVertex(triangle.verticesId[0]).position,
mesh->getVertex(triangle.verticesId[1]).position,
mesh->getVertex(triangle.verticesId[2]).position));
- tree->add(aabb, i);
+ tree->add(std::move(aabb), i);
}
- timer.endFrame();
+}
+
+TEST(AabbTreeTests, BatchBuildTest)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ auto tree = std::make_shared<AabbTree>(3);
+
+ auto mesh = std::make_shared<TriangleMeshPlain>();
+ mesh->load("Geometry/arm_collision.ply");
+
+ std::list<AabbTreeData::Item> items;
+ 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));
+ items.emplace_back(std::make_pair(std::move(aabb), i));
+ }
+ tree->set(std::move(items));
}
TEST(AabbTreeTests, EasyIntersectionTest)
@@ -125,12 +141,11 @@ TEST(AabbTreeTests, EasyIntersectionTest)
TEST(AabbTreeTests, MeshIntersectionTest)
{
- SurgSim::Framework::ApplicationData data("config.txt");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("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);
+ auto mesh = std::make_shared<TriangleMeshPlain>();
+ mesh->load("Geometry/arm_collision.ply");
Aabbd expectedBigBox;
@@ -167,13 +182,13 @@ class TreeLeavesVisitor : public TreeVisitor
{
public:
- virtual bool handle(TreeNode* node) override
+ bool handle(TreeNode* node) override
{
SURGSIM_FAILURE() << "Function " << __FUNCTION__ << " not implemented";
return false;
}
- virtual bool handle(NodeType* node) override
+ bool handle(NodeType* node) override
{
if (node->getNumChildren() == 0)
{
@@ -202,20 +217,21 @@ static typename std::list<PairTypeLhs>::const_iterator getEquivalentPair(const s
TEST(AabbTreeTests, SpatialJoinTest)
{
- SurgSim::Framework::Runtime runtime("config.txt");
- const std::string fileName = "MeshShapeData/staple_collision.ply";
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ const std::string fileName = "Geometry/staple_collision.ply";
auto meshA = std::make_shared<SurgSim::Math::MeshShape>();
- EXPECT_NO_THROW(meshA->load(fileName));
+ ASSERT_NO_THROW(meshA->load(fileName));
auto meshB = std::make_shared<SurgSim::Math::MeshShape>();
- EXPECT_NO_THROW(meshB->load(fileName));
+ ASSERT_NO_THROW(meshB->load(fileName));
RigidTransform3d rhsPose = SurgSim::Math::makeRigidTranslation(Vector3d(0.005, 0.0, 0.0));
- meshB->getMesh()->copyWithTransform(rhsPose, *meshA->getMesh());
+ meshB->transform(rhsPose);
- meshA->updateAabbTree();
- meshB->updateAabbTree();
+ // update the AABB trees
+ meshA->update();
+ meshB->update();
auto aabbA = meshA->getAabbTree();
auto aabbB = meshB->getAabbTree();
@@ -280,4 +296,4 @@ TEST(AabbTreeTests, SpatialJoinTest)
}
} // namespace DataStructure
-} // namespace SurgSim
\ No newline at end of file
+} // namespace SurgSim
diff --git a/SurgSim/DataStructures/UnitTests/CMakeLists.txt b/SurgSim/DataStructures/UnitTests/CMakeLists.txt
index 7c30abb..5c81bd2 100644
--- a/SurgSim/DataStructures/UnitTests/CMakeLists.txt
+++ b/SurgSim/DataStructures/UnitTests/CMakeLists.txt
@@ -1,5 +1,5 @@
# This file is a part of the OpenSurgSim project.
-# Copyright 2012-2013, SimQuest Solutions Inc.
+# Copyright 2012-2016, SimQuest Solutions Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -15,7 +15,6 @@
include_directories (
${gtest_SOURCE_DIR}/include
- ${OSG_INCLUDE_DIR}
)
set(UNIT_TEST_SOURCES
@@ -25,6 +24,12 @@ set(UNIT_TEST_SOURCES
BufferedValueTests.cpp
DataGroupTests.cpp
DataStructuresConvertTests.cpp
+ Grid1DTests.cpp
+ Grid2DTests.cpp
+ Grid3DTests.cpp
+ GridTests.cpp
+ GroupsTests.cpp
+ ImageMapTest.cpp
ImageTest.cpp
IndexDirectoryTests.cpp
IndexedLocalCoordinateTest.cpp
@@ -37,25 +42,22 @@ set(UNIT_TEST_SOURCES
OctreeNodeTests.cpp
OptionalValueTests.cpp
PlyReaderTests.cpp
+ SegmentMeshTest.cpp
TetrahedronMeshTest.cpp
- TriangleMeshBaseTest.cpp
TriangleMeshTest.cpp
)
set(UNIT_TEST_HEADERS
+ GridTests.h
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"
diff --git a/SurgSim/DataStructures/UnitTests/Data/PlyReaderTests/Cube_with_physics.ply b/SurgSim/DataStructures/UnitTests/Data/PlyReaderTests/Cube_with_physics.ply
new file mode 100644
index 0000000..4e1816c
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/Data/PlyReaderTests/Cube_with_physics.ply
@@ -0,0 +1,52 @@
+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 uint nature
+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
+011 3 0 1 2
+012 3 3 4 5
+020 3 6 7 8
+022 3 9 10 11
+023 3 12 13 14
+031 3 15 16 17
+032 3 18 0 2
+033 3 19 20 21
+000 3 16 22 17
+099 3 4 23 5
+010 3 24 12 14
+001 3 10 25 11
diff --git a/SurgSim/DataStructures/UnitTests/Data/SegmentMeshTest/segmentmesh.ply b/SurgSim/DataStructures/UnitTests/Data/SegmentMeshTest/segmentmesh.ply
new file mode 100644
index 0000000..c01378c
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/Data/SegmentMeshTest/segmentmesh.ply
@@ -0,0 +1,18 @@
+ply
+format ascii 1.0
+comment Created by hand
+element vertex 4
+property double x
+property double y
+property double z
+element 1d_element 3
+property list uint uint vertex_indices
+end_header
+1 2 3
+4 5 6
+7 8 9
+11 11 12
+2 0 1
+2 1 2
+2 2 3
+
diff --git a/SurgSim/DataStructures/UnitTests/DataGroupTests.cpp b/SurgSim/DataStructures/UnitTests/DataGroupTests.cpp
index 1c7ea97..d761a25 100644
--- a/SurgSim/DataStructures/UnitTests/DataGroupTests.cpp
+++ b/SurgSim/DataStructures/UnitTests/DataGroupTests.cpp
@@ -50,6 +50,7 @@ TEST(DataGroupTests, CanConstruct)
builder.addInteger("test");
builder.addBoolean("test");
builder.addString("test");
+ builder.addImage("test");
builder.addCustom("test");
DataGroup data = builder.createData();
EXPECT_TRUE(data.poses().hasEntry("test"));
@@ -81,6 +82,7 @@ TEST(DataGroupTests, CanCreateShared)
builder.addInteger("test");
builder.addBoolean("test");
builder.addString("test");
+ builder.addImage("test");
builder.addCustom("test");
std::shared_ptr<DataGroup> data = builder.createSharedData();
@@ -108,6 +110,7 @@ TEST(DataGroupTests, PutName)
builder.addInteger("integer");
builder.addBoolean("boolean");
builder.addString("string");
+ builder.addImage("image");
builder.addCustom("mock_data");
DataGroup data = builder.createData();
@@ -121,6 +124,8 @@ TEST(DataGroupTests, PutName)
Mock3DData<double> mockData(10,10,10);
mockData.set(5, 5, 5, 1.2345);
+ DataGroup::ImageType mockImage(3,3,1);
+
data.poses().set("pose", pose);
data.vectors().set("vector", vector);
data.matrices().set("matrix", matrix);
@@ -128,6 +133,7 @@ TEST(DataGroupTests, PutName)
data.integers().set("integer", 123);
data.booleans().set("boolean", true);
data.strings().set("string", "string");
+ data.images().set("image", mockImage);
data.customData().set("mock_data", mockData);
EXPECT_TRUE(data.poses().hasEntry("pose"));
@@ -151,6 +157,9 @@ TEST(DataGroupTests, PutName)
EXPECT_TRUE(data.strings().hasEntry("string"));
EXPECT_TRUE(data.strings().hasData("string"));
+ EXPECT_TRUE(data.images().hasEntry("image"));
+ EXPECT_TRUE(data.images().hasData("image"));
+
EXPECT_TRUE(data.customData().hasEntry("mock_data"));
EXPECT_TRUE(data.customData().hasData("mock_data"));
}
@@ -166,6 +175,7 @@ TEST(DataGroupTests, GetName)
builder.addInteger("integer");
builder.addBoolean("boolean");
builder.addString("string");
+ builder.addImage("image");
builder.addCustom("mock_data");
DataGroup data = builder.createData();
@@ -180,6 +190,11 @@ TEST(DataGroupTests, GetName)
mockData.set(5, 5, 5, 1.23);
mockData.set(1, 2, 3, 4.56);
+ DataGroup::ImageType mockImage(3,3,1);
+ mockImage.getChannel(0) << 0, 3, 6,
+ 1, 4, 7,
+ 2, 5, 8;
+
data.poses().set("pose", pose);
data.vectors().set("vector", vector);
data.matrices().set("matrix", matrix);
@@ -187,6 +202,7 @@ TEST(DataGroupTests, GetName)
data.integers().set("integer", 123);
data.booleans().set("boolean", true);
data.strings().set("string", "string");
+ data.images().set("image", mockImage);
data.customData().set("mock_data", mockData);
{
@@ -228,6 +244,19 @@ TEST(DataGroupTests, GetName)
EXPECT_EQ("string", value);
}
{
+ DataGroup::ImageType value;
+ EXPECT_TRUE(data.images().get("image", &value));
+ EXPECT_NEAR(0.0f, value.getData()[0], EPSILON);
+ EXPECT_NEAR(1.0f, value.getData()[1], EPSILON);
+ EXPECT_NEAR(2.0f, value.getData()[2], EPSILON);
+ EXPECT_NEAR(3.0f, value.getData()[3], EPSILON);
+ EXPECT_NEAR(4.0f, value.getData()[4], EPSILON);
+ EXPECT_NEAR(5.0f, value.getData()[5], EPSILON);
+ EXPECT_NEAR(6.0f, value.getData()[6], EPSILON);
+ EXPECT_NEAR(7.0f, value.getData()[7], EPSILON);
+ EXPECT_NEAR(8.0f, value.getData()[8], EPSILON);
+ }
+ {
Mock3DData<double> value;
EXPECT_TRUE(data.customData().get("mock_data", &value));
EXPECT_NEAR(1.23, value.get(5, 5, 5), EPSILON);
@@ -243,11 +272,13 @@ TEST(DataGroupTests, ResetAll)
builder.addScalar("second");
builder.addString("third");
builder.addCustom("fourth");
+ builder.addImage("fifth");
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.images().set("fifth", DataGroup::ImageType(10,10,2));
data.resetAll();
@@ -262,6 +293,9 @@ TEST(DataGroupTests, ResetAll)
EXPECT_TRUE(data.customData().hasEntry("fourth"));
EXPECT_FALSE(data.customData().hasData("fourth"));
+
+ EXPECT_TRUE(data.images().hasEntry("fifth"));
+ EXPECT_FALSE(data.images().hasData("fifth"));
}
/// Resetting one data entry at a time.
@@ -390,7 +424,8 @@ TEST(DataGroupTests, DataGroupCopier)
ASSERT_THROW(targetData = sourceData, SurgSim::Framework::AssertionFailure);
- DataGroupCopier copier(sourceData, targetData);
+ ASSERT_THROW(DataGroupCopier badCopier(sourceData, nullptr), 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));
@@ -405,7 +440,8 @@ TEST(DataGroupTests, DataGroupCopier)
ASSERT_TRUE(targetData.booleans().set("test2", !testBoolean2));
// Copy the data.
- copier.copy();
+ ASSERT_THROW(copier.copy(sourceData, nullptr), SurgSim::Framework::AssertionFailure);
+ ASSERT_NO_THROW(copier.copy(sourceData, &targetData));
RigidTransform3d outTestPose;
ASSERT_TRUE(targetData.poses().get("test", &outTestPose));
diff --git a/SurgSim/DataStructures/UnitTests/DataStructuresConvertTests.cpp b/SurgSim/DataStructures/UnitTests/DataStructuresConvertTests.cpp
index 63bff0a..525777e 100644
--- a/SurgSim/DataStructures/UnitTests/DataStructuresConvertTests.cpp
+++ b/SurgSim/DataStructures/UnitTests/DataStructuresConvertTests.cpp
@@ -20,7 +20,36 @@
#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
+{
+
+
+class EmptyComponent : public SurgSim::Framework::Component
+{
+public:
+ explicit EmptyComponent(const std::string& name) : SurgSim::Framework::Component(name)
+ {
+
+ }
+
+ SURGSIM_CLASSNAME(EmptyComponent);
+
+ bool doInitialize() override
+ {
+ return true;
+ }
+
+ bool doWakeUp() override
+ {
+ return true;
+ }
+};
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, EmptyComponent, EmptyComponent);
+
+}
+
namespace SurgSim
{
@@ -28,7 +57,6 @@ namespace SurgSim
namespace DataStructures
{
-using SurgSim::Graphics::OsgBoxRepresentation;
template <typename Type, size_t Size>
void testStdArraySerialization(const std::array<Type, Size>& value)
@@ -54,7 +82,7 @@ void testStdArraySerialization(const std::array<Type, Size>& value)
{
SCOPED_TRACE("Decode into smaller array");
- typedef std::array<Type, Size - 1> SmallerArray;
+ typedef std::array < Type, Size - 1 > SmallerArray;
// Encode
YAML::Node node;
@@ -68,7 +96,7 @@ void testStdArraySerialization(const std::array<Type, Size>& value)
{
SCOPED_TRACE("Decode into larger array");
- typedef std::array<Type, Size + 1> LargerArray;
+ typedef std::array < Type, Size + 1 > LargerArray;
// Encode
YAML::Node node;
@@ -130,11 +158,11 @@ TEST(DataStructuresConvertTests, StdUnorderedMapTests)
{
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;
+ typedef std::unordered_map<int, std::shared_ptr<EmptyComponent>> TestMapType;
TestMapType originalMap;
- auto mockComponent = std::make_shared<OsgBoxRepresentation>("OsgBoxRepresentation");
- auto mockComponent2 = std::make_shared<OsgBoxRepresentation>("MockComponent2");
+ auto mockComponent = std::make_shared<EmptyComponent>("Component1");
+ auto mockComponent2 = std::make_shared<EmptyComponent>("Component2");
originalMap.insert(TestMapType::value_type(1, mockComponent));
originalMap.insert(TestMapType::value_type(2, mockComponent2));
@@ -150,25 +178,25 @@ TEST(DataStructuresConvertTests, StdUnorderedMapTests)
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();
- });
+ [&it](const std::pair<int, std::shared_ptr<EmptyComponent>>& 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;
+ typedef std::unordered_map<int, std::unordered_set<std::shared_ptr<EmptyComponent>>> TestMapType2;
TestMapType2 originalMap;
- std::unordered_set<std::shared_ptr<OsgBoxRepresentation>> set1;
- std::unordered_set<std::shared_ptr<OsgBoxRepresentation>> set2;
+ std::unordered_set<std::shared_ptr<EmptyComponent>> set1;
+ std::unordered_set<std::shared_ptr<EmptyComponent>> set2;
- auto mockComponent = std::make_shared<OsgBoxRepresentation>("OsgBoxRepresentation");
- auto mockComponent2 = std::make_shared<OsgBoxRepresentation>("MockComponent2");
- auto mockComponent3 = std::make_shared<OsgBoxRepresentation>("MockComponent3");
+ auto mockComponent = std::make_shared<EmptyComponent>("Component1");
+ auto mockComponent2 = std::make_shared<EmptyComponent>("Component2");
+ auto mockComponent3 = std::make_shared<EmptyComponent>("Component3");
set1.insert(mockComponent);
set2.insert(mockComponent2);
@@ -192,10 +220,10 @@ TEST(DataStructuresConvertTests, StdUnorderedMapTests)
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();
- });
+ [&item](const std::shared_ptr<EmptyComponent> rep)
+ {
+ return rep->getName() == (*item)->getName();
+ });
EXPECT_NE(std::end(representationSet), match);
}
}
@@ -224,11 +252,11 @@ TEST(DataStructuresConvertTests, StdUnorderedSetTests)
{
SCOPED_TRACE("Serialization of std::unordered_set<> of std::shared_ptr<>");
- typedef std::unordered_set<std::shared_ptr<OsgBoxRepresentation>> TestSetType;
+ typedef std::unordered_set<std::shared_ptr<EmptyComponent>> TestSetType;
TestSetType originalSet;
- auto mockComponent = std::make_shared<OsgBoxRepresentation>("OsgBoxRepresentation");
- auto mockComponent2 = std::make_shared<OsgBoxRepresentation>("MockComponent2");
+ auto mockComponent = std::make_shared<EmptyComponent>("Component1");
+ auto mockComponent2 = std::make_shared<EmptyComponent>("Component2");
originalSet.insert(mockComponent);
originalSet.insert(mockComponent2);
@@ -244,10 +272,10 @@ TEST(DataStructuresConvertTests, StdUnorderedSetTests)
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();
- });
+ [&it](const std::shared_ptr<EmptyComponent>& item)
+ {
+ return (*it)->getName() == item->getName();
+ });
EXPECT_NE(std::end(newSet), result);
}
}
diff --git a/SurgSim/DataStructures/UnitTests/Grid1DTests.cpp b/SurgSim/DataStructures/UnitTests/Grid1DTests.cpp
new file mode 100644
index 0000000..face86b
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/Grid1DTests.cpp
@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/DataStructures/Grid.h"
+#include "SurgSim/DataStructures/UnitTests/GridTests.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <typename T>
+class Grid1DTestBase : public GridTestBase<T, 1>
+{
+public:
+ static const size_t dimension = 1u;
+ typedef T TypeElement;
+};
+
+typedef ::testing::Types<size_t, int, float, double, ElementTest> MyTypes;
+TYPED_TEST_CASE(Grid1DTestBase, MyTypes);
+
+TYPED_TEST(Grid1DTestBase, ConstructorTest)
+{
+ typedef typename TestFixture::GridType GridType;
+
+ ASSERT_NO_THROW({GridType grid(this->m_size, this->m_aabb1Cell);});
+ ASSERT_NO_THROW({GridType grid(this->m_size, this->m_aabb);});
+
+ GridType grid(this->m_size, this->m_aabb);
+ ASSERT_EQ(0u, grid.getActiveCells().size());
+ ASSERT_EQ(0u, grid.getCellIds().size());
+ ASSERT_TRUE(grid.getSize().isApprox(this->m_size));
+ ASSERT_TRUE(grid.getAABB().isApprox(this->m_aabb));
+}
+
+TYPED_TEST(Grid1DTestBase, addElementTest)
+{
+ typedef typename TestFixture::GridType GridType;
+ typedef typename TestFixture::TypeElement TypeElement;
+
+ GridType grid(this->m_size, this->m_aabb);
+
+ // Add an element outside of the grid
+ auto positionMin = this->m_aabb.max() - this->m_aabb.sizes() * 1.001;
+ grid.addElement(TypeElement(), positionMin);
+ ASSERT_EQ(0u, grid.getActiveCells().size());
+ ASSERT_EQ(0u, grid.getCellIds().size());
+
+ // Add an element outside of the grid
+ auto positionMax = this->m_aabb.min() + this->m_aabb.sizes() * 1.001;
+ grid.addElement(TypeElement(), positionMax);
+ ASSERT_EQ(0u, grid.getActiveCells().size());
+ ASSERT_EQ(0u, grid.getCellIds().size());
+
+ // Add an element inside of the grid
+ TypeElement e0(0), e1(1);
+ grid.addElement(e0, Eigen::Matrix<double, TestFixture::dimension, 1>::Zero());
+ ASSERT_EQ(1u, grid.getActiveCells().size());
+ ASSERT_EQ(1u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_ANY_THROW(grid.getCellIds().at(e1));
+
+ // Add an element inside of the grid, in the same cell
+ grid.addElement(e1, Eigen::Matrix<double, TestFixture::dimension, 1>::Zero());
+ ASSERT_EQ(1u, grid.getActiveCells().size());
+ ASSERT_EQ(2u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+
+ // Add an element inside of the grid, in a different cell
+ TypeElement e2(2);
+ grid.addElement(e2, this->m_size * 1.5);
+ ASSERT_EQ(2u, grid.getActiveCells().size());
+ ASSERT_EQ(3u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_NO_THROW(grid.getCellIds().at(e2));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e2]);
+ ASSERT_NE(grid.getCellIds()[e1], grid.getCellIds()[e2]);
+}
+
+TYPED_TEST(Grid1DTestBase, NeighborsTest)
+{
+ typedef typename TestFixture::GridType GridType;
+ typedef typename TestFixture::TypeElement TypeElement;
+
+ GridType grid(this->m_size, this->m_aabb);
+
+ // Build the following grid:
+ // Cell(e0, e1) <- neighbor -> Cell(e2) <- neighbor -> Cell(e3) <- ..not neighbor.. -> Cell(e4)
+
+ Eigen::Matrix<double, TestFixture::dimension, 1> position =
+ Eigen::Matrix<double, TestFixture::dimension, 1>::Zero();
+
+ // Add an element inside of the grid
+ TypeElement e0(0), e1(1);
+ grid.addElement(e0, position);
+ ASSERT_EQ(1u, grid.getActiveCells().size());
+ ASSERT_EQ(1u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_ANY_THROW(grid.getCellIds().at(e1));
+
+ // Add an element inside of the grid, in the same cell
+ grid.addElement(e1, position);
+ ASSERT_EQ(1u, grid.getActiveCells().size());
+ ASSERT_EQ(2u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+
+ // Add an element inside of the grid, in a different cell
+ position[0] += this->m_size[0] * 1.1; // Next cell on the 1st dimension
+ TypeElement e2(2);
+ grid.addElement(e2, position);
+ ASSERT_EQ(2u, grid.getActiveCells().size());
+ ASSERT_EQ(3u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_NO_THROW(grid.getCellIds().at(e2));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e2]);
+ ASSERT_NE(grid.getCellIds()[e1], grid.getCellIds()[e2]);
+
+ // Add an element inside of the grid, in a different cell
+ position[0] += this->m_size[0] * 1.1; // Next cell on the 1st dimension
+ TypeElement e3(3);
+ grid.addElement(e3, position);
+ ASSERT_EQ(3u, grid.getActiveCells().size());
+ ASSERT_EQ(4u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_NO_THROW(grid.getCellIds().at(e2));
+ ASSERT_NO_THROW(grid.getCellIds().at(e3));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e2]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e3]);
+ ASSERT_NE(grid.getCellIds()[e2], grid.getCellIds()[e3]);
+
+ // Add an element inside of the grid, far away from all other elements
+ position[0] += this->m_size[0] * 2.1; // Few cells further on the 1st dimension
+ TypeElement e4(4);
+ grid.addElement(e4, position);
+ ASSERT_EQ(4u, grid.getActiveCells().size());
+ ASSERT_EQ(5u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_NO_THROW(grid.getCellIds().at(e2));
+ ASSERT_NO_THROW(grid.getCellIds().at(e3));
+ ASSERT_NO_THROW(grid.getCellIds().at(e4));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e2]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e3]);
+ ASSERT_NE(grid.getCellIds()[e2], grid.getCellIds()[e3]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e4]);
+ ASSERT_NE(grid.getCellIds()[e1], grid.getCellIds()[e4]);
+ ASSERT_NE(grid.getCellIds()[e2], grid.getCellIds()[e4]);
+ ASSERT_NE(grid.getCellIds()[e3], grid.getCellIds()[e4]);
+
+ ASSERT_NO_THROW(grid.getNeighbors(e0));
+ ASSERT_NO_THROW(grid.getNeighbors(e1));
+ ASSERT_NO_THROW(grid.getNeighbors(e2));
+ ASSERT_NO_THROW(grid.getNeighbors(e3));
+ ASSERT_NO_THROW(grid.getNeighbors(e4));
+
+ // e0's neighbors = {e0, e1, e2}
+ auto& e0Neighbors = grid.getNonConstNeighbors(e0);
+ ASSERT_EQ(3u, e0Neighbors.size());
+ ASSERT_TRUE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e0) != e0Neighbors.end());
+ ASSERT_TRUE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e1) != e0Neighbors.end());
+ ASSERT_TRUE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e2) != e0Neighbors.end());
+ ASSERT_FALSE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e3) != e0Neighbors.end());
+ ASSERT_FALSE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e4) != e0Neighbors.end());
+
+ // e1's neighbors = {e0, e1, e2}
+ auto& e1Neighbors = grid.getNonConstNeighbors(e1);
+ ASSERT_EQ(3u, e1Neighbors.size());
+ ASSERT_TRUE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e0) != e1Neighbors.end());
+ ASSERT_TRUE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e1) != e1Neighbors.end());
+ ASSERT_TRUE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e2) != e1Neighbors.end());
+ ASSERT_FALSE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e3) != e1Neighbors.end());
+ ASSERT_FALSE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e4) != e1Neighbors.end());
+
+ // e2's neighbors = {e0, e1, e2, e3}
+ auto& e2Neighbors = grid.getNonConstNeighbors(e2);
+ ASSERT_EQ(4u, e2Neighbors.size());
+ ASSERT_TRUE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e0) != e2Neighbors.end());
+ ASSERT_TRUE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e1) != e2Neighbors.end());
+ ASSERT_TRUE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e2) != e2Neighbors.end());
+ ASSERT_TRUE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e3) != e2Neighbors.end());
+ ASSERT_FALSE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e4) != e2Neighbors.end());
+
+ // e3's neighbors = {e2, e3}
+ auto& e3Neighbors = grid.getNonConstNeighbors(e3);
+ ASSERT_EQ(2u, e3Neighbors.size());
+ ASSERT_FALSE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e0) != e3Neighbors.end());
+ ASSERT_FALSE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e1) != e3Neighbors.end());
+ ASSERT_TRUE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e2) != e3Neighbors.end());
+ ASSERT_TRUE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e3) != e3Neighbors.end());
+ ASSERT_FALSE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e4) != e3Neighbors.end());
+
+ // e4's neighbors = {e4}
+ auto& e4Neighbors = grid.getNonConstNeighbors(e4);
+ ASSERT_EQ(1u, e4Neighbors.size());
+ ASSERT_FALSE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e0) != e4Neighbors.end());
+ ASSERT_FALSE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e1) != e4Neighbors.end());
+ ASSERT_FALSE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e2) != e4Neighbors.end());
+ ASSERT_FALSE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e3) != e4Neighbors.end());
+ ASSERT_TRUE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e4) != e4Neighbors.end());
+
+ // Test element not in the grid has no neighbors
+ TypeElement e5(5);
+ ASSERT_EQ(0u, grid.getNeighbors(e5).size());
+}
+
+TYPED_TEST(Grid1DTestBase, ResetTest)
+{
+ typedef typename TestFixture::GridType GridType;
+ typedef typename TestFixture::TypeElement TypeElement;
+
+ GridType grid(this->m_size, this->m_aabb);
+
+ TypeElement e0(0), e1(1);
+ grid.addElement(e0, Eigen::Matrix<double, TestFixture::dimension, 1>::Zero());
+ grid.addElement(e1, Eigen::Matrix<double, TestFixture::dimension, 1>::Zero());
+ ASSERT_NE(0u, grid.getActiveCells().size());
+ ASSERT_NE(0u, grid.getCellIds().size());
+ ASSERT_NE(0u, grid.getNeighbors(e0).size());
+ ASSERT_NE(0u, grid.getNeighbors(e1).size());
+
+ ASSERT_NO_THROW(grid.reset());
+ ASSERT_EQ(0u, grid.getActiveCells().size());
+ ASSERT_EQ(0u, grid.getCellIds().size());
+ ASSERT_EQ(0u, grid.getNeighbors(e0).size());
+ ASSERT_EQ(0u, grid.getNeighbors(e1).size());
+}
+
+}; // namespace DataStructures
+}; // namespace SurgSim
diff --git a/SurgSim/DataStructures/UnitTests/Grid2DTests.cpp b/SurgSim/DataStructures/UnitTests/Grid2DTests.cpp
new file mode 100644
index 0000000..2ef0619
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/Grid2DTests.cpp
@@ -0,0 +1,258 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Vector.h"
+#include "SurgSim/DataStructures/Grid.h"
+#include "SurgSim/DataStructures/UnitTests/GridTests.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <typename T>
+class Grid2DTestBase : public GridTestBase<T, 2>
+{
+public:
+ /// The test parameters
+ typedef T TypeElement;
+ static const size_t dimension = 2u;
+};
+
+typedef ::testing::Types<size_t, int, float, double, ElementTest> MyTypes;
+TYPED_TEST_CASE(Grid2DTestBase, MyTypes);
+
+TYPED_TEST(Grid2DTestBase, ConstructorTest)
+{
+ typedef typename TestFixture::GridType GridType;
+
+ ASSERT_NO_THROW({GridType grid(this->m_size, this->m_aabb1Cell);});
+ ASSERT_NO_THROW({GridType grid(this->m_size, this->m_aabb);});
+ ASSERT_NO_THROW({GridType grid(this->m_size, this->m_aabbBig);});
+
+ GridType grid(this->m_size, this->m_aabb);
+ ASSERT_EQ(0u, grid.getActiveCells().size());
+ ASSERT_EQ(0u, grid.getCellIds().size());
+ ASSERT_TRUE(grid.getSize().isApprox(this->m_size));
+ ASSERT_TRUE(grid.getAABB().isApprox(this->m_aabb));
+}
+
+TYPED_TEST(Grid2DTestBase, addElementTest)
+{
+ typedef typename TestFixture::GridType GridType;
+ typedef typename TestFixture::TypeElement TypeElement;
+
+ GridType grid(this->m_size, this->m_aabb);
+
+ // Add an element outside of the grid
+ auto positionMin = this->m_aabb.max() - this->m_aabb.sizes() * 1.001;
+ grid.addElement(TypeElement(), positionMin);
+ ASSERT_EQ(0u, grid.getActiveCells().size());
+ ASSERT_EQ(0u, grid.getCellIds().size());
+
+ // Add an element outside of the grid
+ auto positionMax = this->m_aabb.min() + this->m_aabb.sizes() * 1.001;
+ grid.addElement(TypeElement(), positionMax);
+ ASSERT_EQ(0u, grid.getActiveCells().size());
+ ASSERT_EQ(0u, grid.getCellIds().size());
+
+ // Add an element inside of the grid
+ TypeElement e0(0), e1(1);
+ grid.addElement(e0, Eigen::Matrix<double, TestFixture::dimension, 1>::Zero());
+ ASSERT_EQ(1u, grid.getActiveCells().size());
+ ASSERT_EQ(1u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_ANY_THROW(grid.getCellIds().at(e1));
+
+ // Add an element inside of the grid, in the same cell
+ grid.addElement(e1, Eigen::Matrix<double, TestFixture::dimension, 1>::Zero());
+ ASSERT_EQ(1u, grid.getActiveCells().size());
+ ASSERT_EQ(2u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+
+ // Add an element inside of the grid, in a different cell
+ TypeElement e2(2);
+ grid.addElement(e2, this->m_size * 1.5);
+ ASSERT_EQ(2u, grid.getActiveCells().size());
+ ASSERT_EQ(3u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_NO_THROW(grid.getCellIds().at(e2));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e2]);
+ ASSERT_NE(grid.getCellIds()[e1], grid.getCellIds()[e2]);
+}
+
+TYPED_TEST(Grid2DTestBase, NeighborsTest)
+{
+ typedef typename TestFixture::GridType GridType;
+ typedef typename TestFixture::TypeElement TypeElement;
+
+ GridType grid(this->m_size, this->m_aabb);
+
+ // Build the following grid:
+ // Cell(e0, e1) <- neighbor -> Cell(e2) <- neighbor -> Cell(e3) <- ..not neighbor.. -> Cell(e4)
+
+ Eigen::Matrix<double, TestFixture::dimension, 1> position =
+ Eigen::Matrix<double, TestFixture::dimension, 1>::Zero();
+
+ // Add an element inside of the grid
+ TypeElement e0(0), e1(1);
+ grid.addElement(e0, position);
+ ASSERT_EQ(1u, grid.getActiveCells().size());
+ ASSERT_EQ(1u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_ANY_THROW(grid.getCellIds().at(e1));
+
+ // Add an element inside of the grid, in the same cell
+ grid.addElement(e1, position);
+ ASSERT_EQ(1u, grid.getActiveCells().size());
+ ASSERT_EQ(2u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+
+ // Add an element inside of the grid, in a different cell
+ position[0] += this->m_size[0] * 1.1; // Next cell on the 1st dimension
+ TypeElement e2(2);
+ grid.addElement(e2, position);
+ ASSERT_EQ(2u, grid.getActiveCells().size());
+ ASSERT_EQ(3u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_NO_THROW(grid.getCellIds().at(e2));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e2]);
+ ASSERT_NE(grid.getCellIds()[e1], grid.getCellIds()[e2]);
+
+ // Add an element inside of the grid, in a different cell
+ position[0] += this->m_size[0] * 1.1; // Next cell on the 1st dimension
+ TypeElement e3(3);
+ grid.addElement(e3, position);
+ ASSERT_EQ(3u, grid.getActiveCells().size());
+ ASSERT_EQ(4u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_NO_THROW(grid.getCellIds().at(e2));
+ ASSERT_NO_THROW(grid.getCellIds().at(e3));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e2]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e3]);
+ ASSERT_NE(grid.getCellIds()[e2], grid.getCellIds()[e3]);
+
+ // Add an element inside of the grid, far away from all other elements
+ position[0] += this->m_size[0] * 2.1; // Few cells further on the 1st dimension
+ TypeElement e4(4);
+ grid.addElement(e4, position);
+ ASSERT_EQ(4u, grid.getActiveCells().size());
+ ASSERT_EQ(5u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_NO_THROW(grid.getCellIds().at(e2));
+ ASSERT_NO_THROW(grid.getCellIds().at(e3));
+ ASSERT_NO_THROW(grid.getCellIds().at(e4));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e2]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e3]);
+ ASSERT_NE(grid.getCellIds()[e2], grid.getCellIds()[e3]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e4]);
+ ASSERT_NE(grid.getCellIds()[e1], grid.getCellIds()[e4]);
+ ASSERT_NE(grid.getCellIds()[e2], grid.getCellIds()[e4]);
+ ASSERT_NE(grid.getCellIds()[e3], grid.getCellIds()[e4]);
+
+ ASSERT_NO_THROW(grid.getNeighbors(e0));
+ ASSERT_NO_THROW(grid.getNeighbors(e1));
+ ASSERT_NO_THROW(grid.getNeighbors(e2));
+ ASSERT_NO_THROW(grid.getNeighbors(e3));
+ ASSERT_NO_THROW(grid.getNeighbors(e4));
+
+ // e0's neighbors = {e0, e1, e2}
+ auto& e0Neighbors = grid.getNonConstNeighbors(e0);
+ ASSERT_EQ(3u, e0Neighbors.size());
+ ASSERT_TRUE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e0) != e0Neighbors.end());
+ ASSERT_TRUE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e1) != e0Neighbors.end());
+ ASSERT_TRUE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e2) != e0Neighbors.end());
+ ASSERT_FALSE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e3) != e0Neighbors.end());
+ ASSERT_FALSE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e4) != e0Neighbors.end());
+
+ // e1's neighbors = {e0, e1, e2}
+ auto& e1Neighbors = grid.getNonConstNeighbors(e1);
+ ASSERT_EQ(3u, e1Neighbors.size());
+ ASSERT_TRUE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e0) != e1Neighbors.end());
+ ASSERT_TRUE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e1) != e1Neighbors.end());
+ ASSERT_TRUE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e2) != e1Neighbors.end());
+ ASSERT_FALSE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e3) != e1Neighbors.end());
+ ASSERT_FALSE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e4) != e1Neighbors.end());
+
+ // e2's neighbors = {e0, e1, e2, e3}
+ auto& e2Neighbors = grid.getNonConstNeighbors(e2);
+ ASSERT_EQ(4u, e2Neighbors.size());
+ ASSERT_TRUE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e0) != e2Neighbors.end());
+ ASSERT_TRUE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e1) != e2Neighbors.end());
+ ASSERT_TRUE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e2) != e2Neighbors.end());
+ ASSERT_TRUE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e3) != e2Neighbors.end());
+ ASSERT_FALSE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e4) != e2Neighbors.end());
+
+ // e3's neighbors = {e2, e3}
+ auto& e3Neighbors = grid.getNonConstNeighbors(e3);
+ ASSERT_EQ(2u, e3Neighbors.size());
+ ASSERT_FALSE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e0) != e3Neighbors.end());
+ ASSERT_FALSE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e1) != e3Neighbors.end());
+ ASSERT_TRUE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e2) != e3Neighbors.end());
+ ASSERT_TRUE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e3) != e3Neighbors.end());
+ ASSERT_FALSE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e4) != e3Neighbors.end());
+
+ // e4's neighbors = {e4}
+ auto& e4Neighbors = grid.getNonConstNeighbors(e4);
+ ASSERT_EQ(1u, e4Neighbors.size());
+ ASSERT_FALSE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e0) != e4Neighbors.end());
+ ASSERT_FALSE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e1) != e4Neighbors.end());
+ ASSERT_FALSE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e2) != e4Neighbors.end());
+ ASSERT_FALSE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e3) != e4Neighbors.end());
+ ASSERT_TRUE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e4) != e4Neighbors.end());
+
+ // Test element not in the grid has no neighbors
+ TypeElement e5(5);
+ ASSERT_EQ(0u, grid.getNeighbors(e5).size());
+}
+
+TYPED_TEST(Grid2DTestBase, ResetTest)
+{
+ typedef typename TestFixture::GridType GridType;
+ typedef typename TestFixture::TypeElement TypeElement;
+
+ GridType grid(this->m_size, this->m_aabb);
+
+ TypeElement e0(0), e1(1);
+ grid.addElement(e0, Eigen::Matrix<double, TestFixture::dimension, 1>::Zero());
+ grid.addElement(e1, Eigen::Matrix<double, TestFixture::dimension, 1>::Zero());
+ ASSERT_NE(0u, grid.getActiveCells().size());
+ ASSERT_NE(0u, grid.getCellIds().size());
+ ASSERT_NE(0u, grid.getNeighbors(e0).size());
+ ASSERT_NE(0u, grid.getNeighbors(e1).size());
+
+ ASSERT_NO_THROW(grid.reset());
+ ASSERT_EQ(0u, grid.getActiveCells().size());
+ ASSERT_EQ(0u, grid.getCellIds().size());
+ ASSERT_EQ(0u, grid.getNeighbors(e0).size());
+ ASSERT_EQ(0u, grid.getNeighbors(e1).size());
+}
+
+}; // namespace DataStructures
+}; // namespace SurgSim
diff --git a/SurgSim/DataStructures/UnitTests/Grid3DTests.cpp b/SurgSim/DataStructures/UnitTests/Grid3DTests.cpp
new file mode 100644
index 0000000..e588349
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/Grid3DTests.cpp
@@ -0,0 +1,415 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Vector.h"
+#include "SurgSim/DataStructures/Grid.h"
+#include "SurgSim/DataStructures/UnitTests/GridTests.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <typename T>
+class Grid3DTestBase : public GridTestBase<T, 3>
+{
+public:
+ /// The test parameters
+ typedef T TypeElement;
+ static const size_t dimension = 3u;
+};
+
+typedef ::testing::Types<size_t, int, float, double, ElementTest> MyTypes;
+TYPED_TEST_CASE(Grid3DTestBase, MyTypes);
+
+TYPED_TEST(Grid3DTestBase, ConstructorTest)
+{
+ typedef typename TestFixture::GridType GridType;
+
+ ASSERT_NO_THROW({GridType grid(this->m_size, this->m_aabb1Cell);});
+ ASSERT_NO_THROW({GridType grid(this->m_size, this->m_aabb);});
+ ASSERT_NO_THROW({GridType grid(this->m_size, this->m_aabbBig);});
+
+ GridType grid(this->m_size, this->m_aabb);
+ ASSERT_EQ(0u, grid.getActiveCells().size());
+ ASSERT_EQ(0u, grid.getCellIds().size());
+ ASSERT_TRUE(grid.getSize().isApprox(this->m_size));
+ ASSERT_TRUE(grid.getAABB().isApprox(this->m_aabb));
+}
+
+TYPED_TEST(Grid3DTestBase, addElementTest)
+{
+ typedef typename TestFixture::GridType GridType;
+ typedef typename TestFixture::TypeElement TypeElement;
+
+ GridType grid(this->m_size, this->m_aabb);
+
+ // Add an element outside of the grid
+ auto positionMin = this->m_aabb.max() - this->m_aabb.sizes() * 1.001;
+ grid.addElement(TypeElement(), positionMin);
+ ASSERT_EQ(0u, grid.getActiveCells().size());
+ ASSERT_EQ(0u, grid.getCellIds().size());
+
+ // Add an element outside of the grid
+ auto positionMax = this->m_aabb.min() + this->m_aabb.sizes() * 1.001;
+ grid.addElement(TypeElement(), positionMax);
+ ASSERT_EQ(0u, grid.getActiveCells().size());
+ ASSERT_EQ(0u, grid.getCellIds().size());
+
+ // Add an element inside of the grid
+ TypeElement e0(0), e1(1);
+ grid.addElement(e0, Eigen::Matrix<double, TestFixture::dimension, 1>::Zero());
+ ASSERT_EQ(1u, grid.getActiveCells().size());
+ ASSERT_EQ(1u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_ANY_THROW(grid.getCellIds().at(e1));
+
+ // Add an element inside of the grid, in the same cell
+ grid.addElement(e1, Eigen::Matrix<double, TestFixture::dimension, 1>::Zero());
+ ASSERT_EQ(1u, grid.getActiveCells().size());
+ ASSERT_EQ(2u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+
+ // Add an element inside of the grid, in a different cell
+ TypeElement e2(2);
+ grid.addElement(e2, this->m_size * 1.5);
+ ASSERT_EQ(2u, grid.getActiveCells().size());
+ ASSERT_EQ(3u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_NO_THROW(grid.getCellIds().at(e2));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e2]);
+ ASSERT_NE(grid.getCellIds()[e1], grid.getCellIds()[e2]);
+}
+
+TYPED_TEST(Grid3DTestBase, NeighborsTest)
+{
+ typedef typename TestFixture::GridType GridType;
+ typedef typename TestFixture::TypeElement TypeElement;
+
+ GridType grid(this->m_size, this->m_aabb);
+
+ // Build the following grid:
+ // Cell(e0, e1) <- neighbor -> Cell(e2) <- neighbor -> Cell(e3) <- ..not neighbor.. -> Cell(e4)
+
+ Eigen::Matrix<double, TestFixture::dimension, 1> position =
+ Eigen::Matrix<double, TestFixture::dimension, 1>::Zero();
+
+ // Add an element inside of the grid
+ TypeElement e0(0), e1(1);
+ grid.addElement(e0, position);
+ ASSERT_EQ(1u, grid.getActiveCells().size());
+ ASSERT_EQ(1u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_ANY_THROW(grid.getCellIds().at(e1));
+
+ // Add an element inside of the grid, in the same cell
+ grid.addElement(e1, position);
+ ASSERT_EQ(1u, grid.getActiveCells().size());
+ ASSERT_EQ(2u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+
+ // Add an element inside of the grid, in a different cell
+ position[0] += this->m_size[0] * 1.1; // Next cell on the 1st dimension
+ TypeElement e2(2);
+ grid.addElement(e2, position);
+ ASSERT_EQ(2u, grid.getActiveCells().size());
+ ASSERT_EQ(3u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_NO_THROW(grid.getCellIds().at(e2));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e2]);
+ ASSERT_NE(grid.getCellIds()[e1], grid.getCellIds()[e2]);
+
+ // Add an element inside of the grid, in a different cell
+ position[0] += this->m_size[0] * 1.1; // Next cell on the 1st dimension
+ TypeElement e3(3);
+ grid.addElement(e3, position);
+ ASSERT_EQ(3u, grid.getActiveCells().size());
+ ASSERT_EQ(4u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_NO_THROW(grid.getCellIds().at(e2));
+ ASSERT_NO_THROW(grid.getCellIds().at(e3));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e2]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e3]);
+ ASSERT_NE(grid.getCellIds()[e2], grid.getCellIds()[e3]);
+
+ // Add an element inside of the grid, far away from all other elements
+ position[0] += this->m_size[0] * 2.1; // Few cells further on the 1st dimension
+ TypeElement e4(4);
+ grid.addElement(e4, position);
+ ASSERT_EQ(4u, grid.getActiveCells().size());
+ ASSERT_EQ(5u, grid.getCellIds().size());
+ ASSERT_NO_THROW(grid.getCellIds().at(e0));
+ ASSERT_NO_THROW(grid.getCellIds().at(e1));
+ ASSERT_NO_THROW(grid.getCellIds().at(e2));
+ ASSERT_NO_THROW(grid.getCellIds().at(e3));
+ ASSERT_NO_THROW(grid.getCellIds().at(e4));
+ ASSERT_EQ(grid.getCellIds()[e0], grid.getCellIds()[e1]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e2]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e3]);
+ ASSERT_NE(grid.getCellIds()[e2], grid.getCellIds()[e3]);
+ ASSERT_NE(grid.getCellIds()[e0], grid.getCellIds()[e4]);
+ ASSERT_NE(grid.getCellIds()[e1], grid.getCellIds()[e4]);
+ ASSERT_NE(grid.getCellIds()[e2], grid.getCellIds()[e4]);
+ ASSERT_NE(grid.getCellIds()[e3], grid.getCellIds()[e4]);
+
+ ASSERT_NO_THROW(grid.getNeighbors(e0));
+ ASSERT_NO_THROW(grid.getNeighbors(e1));
+ ASSERT_NO_THROW(grid.getNeighbors(e2));
+ ASSERT_NO_THROW(grid.getNeighbors(e3));
+ ASSERT_NO_THROW(grid.getNeighbors(e4));
+
+ // e0's neighbors = {e0, e1, e2}
+ auto& e0Neighbors = grid.getNonConstNeighbors(e0);
+ ASSERT_EQ(3u, e0Neighbors.size());
+ ASSERT_TRUE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e0) != e0Neighbors.end());
+ ASSERT_TRUE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e1) != e0Neighbors.end());
+ ASSERT_TRUE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e2) != e0Neighbors.end());
+ ASSERT_FALSE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e3) != e0Neighbors.end());
+ ASSERT_FALSE(std::find(e0Neighbors.begin(), e0Neighbors.end(), e4) != e0Neighbors.end());
+
+ // e1's neighbors = {e0, e1, e2}
+ auto& e1Neighbors = grid.getNonConstNeighbors(e1);
+ ASSERT_EQ(3u, e1Neighbors.size());
+ ASSERT_TRUE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e0) != e1Neighbors.end());
+ ASSERT_TRUE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e1) != e1Neighbors.end());
+ ASSERT_TRUE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e2) != e1Neighbors.end());
+ ASSERT_FALSE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e3) != e1Neighbors.end());
+ ASSERT_FALSE(std::find(e1Neighbors.begin(), e1Neighbors.end(), e4) != e1Neighbors.end());
+
+ // e2's neighbors = {e0, e1, e2, e3}
+ auto& e2Neighbors = grid.getNonConstNeighbors(e2);
+ ASSERT_EQ(4u, e2Neighbors.size());
+ ASSERT_TRUE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e0) != e2Neighbors.end());
+ ASSERT_TRUE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e1) != e2Neighbors.end());
+ ASSERT_TRUE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e2) != e2Neighbors.end());
+ ASSERT_TRUE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e3) != e2Neighbors.end());
+ ASSERT_FALSE(std::find(e2Neighbors.begin(), e2Neighbors.end(), e4) != e2Neighbors.end());
+
+ // e3's neighbors = {e2, e3}
+ auto& e3Neighbors = grid.getNonConstNeighbors(e3);
+ ASSERT_EQ(2u, e3Neighbors.size());
+ ASSERT_FALSE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e0) != e3Neighbors.end());
+ ASSERT_FALSE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e1) != e3Neighbors.end());
+ ASSERT_TRUE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e2) != e3Neighbors.end());
+ ASSERT_TRUE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e3) != e3Neighbors.end());
+ ASSERT_FALSE(std::find(e3Neighbors.begin(), e3Neighbors.end(), e4) != e3Neighbors.end());
+
+ // e4's neighbors = {e4}
+ auto& e4Neighbors = grid.getNonConstNeighbors(e4);
+ ASSERT_EQ(1u, e4Neighbors.size());
+ ASSERT_FALSE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e0) != e4Neighbors.end());
+ ASSERT_FALSE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e1) != e4Neighbors.end());
+ ASSERT_FALSE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e2) != e4Neighbors.end());
+ ASSERT_FALSE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e3) != e4Neighbors.end());
+ ASSERT_TRUE(std::find(e4Neighbors.begin(), e4Neighbors.end(), e4) != e4Neighbors.end());
+
+ // Test element not in the grid has no neighbors
+ TypeElement e5(5);
+ ASSERT_EQ(0u, grid.getNeighbors(e5).size());
+}
+
+TYPED_TEST(Grid3DTestBase, NeighborsPosition)
+{
+ typedef typename TestFixture::GridType GridType;
+ typedef typename TestFixture::TypeElement TypeElement;
+
+ GridType grid(this->m_size, this->m_aabb);
+
+ // Build the following grid:
+ // Cell(e0, e1) <- neighbor -> Cell(e2) <- neighbor -> Cell(e3) <- ..not neighbor.. -> Cell(e4)
+
+ Eigen::Matrix<double, TestFixture::dimension, 1> position =
+ Eigen::Matrix<double, TestFixture::dimension, 1>::Zero();
+
+ // Add an element inside of the grid
+ TypeElement e0(0), e1(1);
+ grid.addElement(e0, position);
+ auto positione0 = position;
+
+ // Add an element inside of the grid, in the same cell
+ grid.addElement(e1, position);
+
+ // Add an element inside of the grid, in a different cell
+ position[0] += this->m_size[0] * 1.1; // Next cell on the 1st dimension
+ TypeElement e2(2);
+ grid.addElement(e2, position);
+
+ // Add an element inside of the grid, in a different cell
+ position[0] += this->m_size[0] * 1.1; // Next cell on the 1st dimension
+ TypeElement e3(3);
+ grid.addElement(e3, position);
+
+ position[0] += this->m_size[0] * 1.1; // Next cell on the 1st dimension
+ auto positionEmpty = position;
+
+ // Add an element inside of the grid, far away from all other elements
+ position[0] += this->m_size[0] * 1.1; // Few cells further on the 1st dimension
+ TypeElement e4(4);
+ grid.addElement(e4, position);
+
+ {
+ // e0's neighbors = {e0, e1, e2}
+ // e0's position has a grid cell in it, should return neighbors of e0
+ auto& neighbors = grid.getNeighbors(positione0);
+ EXPECT_EQ(3u, neighbors.size());
+ EXPECT_TRUE(std::find(neighbors.begin(), neighbors.end(), e0) != neighbors.end());
+ EXPECT_TRUE(std::find(neighbors.begin(), neighbors.end(), e1) != neighbors.end());
+ EXPECT_TRUE(std::find(neighbors.begin(), neighbors.end(), e2) != neighbors.end());
+ }
+
+ {
+ // Move the position just a little bit away from e0, should still result in the e0 cell
+ auto positionNote0 = positione0 * 1.01;
+ auto& neighbors = grid.getNeighbors(positionNote0);
+ EXPECT_EQ(3u, neighbors.size());
+ EXPECT_TRUE(std::find(neighbors.begin(), neighbors.end(), e0) != neighbors.end());
+ EXPECT_TRUE(std::find(neighbors.begin(), neighbors.end(), e1) != neighbors.end());
+ EXPECT_TRUE(std::find(neighbors.begin(), neighbors.end(), e2) != neighbors.end());
+ }
+
+ {
+ // This cell does not exist but should return e3 and e4 as neighbors, it's not cached
+ auto& neighbors = grid.getNeighbors(positionEmpty);
+ EXPECT_EQ(2u, neighbors.size());
+ EXPECT_TRUE(std::find(neighbors.begin(), neighbors.end(), e3) != neighbors.end());
+ EXPECT_TRUE(std::find(neighbors.begin(), neighbors.end(), e4) != neighbors.end());
+ }
+
+ {
+ // This cell does not exist but should return e3 and e4 as neighbors, this is from
+ // the cache now
+ auto& neighbors = grid.getNeighbors(positionEmpty);
+ EXPECT_EQ(2u, neighbors.size());
+ EXPECT_TRUE(std::find(neighbors.begin(), neighbors.end(), e3) != neighbors.end());
+ EXPECT_TRUE(std::find(neighbors.begin(), neighbors.end(), e4) != neighbors.end());
+ }
+
+ {
+ // This is outside of the AABB
+ position = this->m_size * 3;
+ auto& neighbors = grid.getNeighbors(position);
+ EXPECT_EQ(0u, neighbors.size());
+ }
+
+}
+
+TYPED_TEST(Grid3DTestBase, Neighbors3DTest)
+{
+ typedef typename TestFixture::GridType GridType;
+ typedef typename TestFixture::TypeElement TypeElement;
+
+ GridType grid(this->m_size, this->m_aabb);
+
+ // Build a grid where we have 1 element per cell and 1 cell has all its neighbors populated
+ // The grid content would be for dimension x=0 (similar on dimension x=1 and x=2):
+ // Cell(e000) Cell(e001) Cell(e002)
+ // Cell(e010) Cell(e011) Cell(e012)
+ // Cell(e020) Cell(e021) Cell(e022)
+
+ // Add the 27 elements inside the grid on 27 different cells forming a cube
+ Eigen::Matrix<double, 3, 1> position = Eigen::Matrix<double, 3, 1>::Zero();
+ Number<size_t, 3, 3> number; // 3 digits in base 3 => covers 27 different numbers
+ const TypeElement element[27] = {
+ TypeElement(0), TypeElement(1), TypeElement(2),
+ TypeElement(3), TypeElement(4), TypeElement(5),
+ TypeElement(6), TypeElement(7), TypeElement(8),
+ TypeElement(9), TypeElement(10), TypeElement(11),
+ TypeElement(12), TypeElement(13), TypeElement(14),
+ TypeElement(15), TypeElement(16), TypeElement(17),
+ TypeElement(18), TypeElement(19), TypeElement(20),
+ TypeElement(21), TypeElement(22), TypeElement(23),
+ TypeElement(24), TypeElement(25), TypeElement(26)};
+
+ for(size_t X = 0; X < 3; ++X)
+ {
+ for(size_t Y = 0; Y < 3; ++Y)
+ {
+ for(size_t Z = 0; Z < 3; ++Z)
+ {
+ SurgSim::Math::Vector3d offset(this->m_size[0] * X, this->m_size[1] * Y, this->m_size[2] * Z);
+ grid.addElement(element[number.toDecimal()], position + offset);
+ number.next();
+ }
+ }
+ }
+
+ ASSERT_EQ(27u, grid.getActiveCells().size()); ///< 27 cells
+ ASSERT_EQ(27u, grid.getCellIds().size()); ///< 27 elements
+ for (size_t elementId = 0; elementId < 27; elementId++)
+ {
+ ASSERT_NO_THROW(grid.getCellIds().at(element[elementId]));
+ for (size_t otherElementId = 0; otherElementId < 27; otherElementId++)
+ {
+ if (elementId == otherElementId)
+ {
+ continue;
+ }
+ /// Each cell contains only 1 unique element and each element is associated to a unique cell
+ ASSERT_NE(grid.getCellIds()[element[elementId]], grid.getCellIds()[element[otherElementId]]);
+ }
+ }
+
+ for (size_t elementId = 0; elementId < 27; elementId++)
+ {
+ ASSERT_NO_THROW(grid.getNeighbors(element[elementId]));
+ ASSERT_GT(grid.getNeighbors(element[elementId]).size(), 0u);
+ }
+
+ // The central element should have all the elements for neighbors (including itself)
+ auto& e111Neighbors = grid.getNonConstNeighbors(element[13]);
+ ASSERT_EQ(27u, e111Neighbors.size());
+ for (size_t elementId = 0; elementId < 27; elementId++)
+ {
+ auto found = std::find(e111Neighbors.begin(), e111Neighbors.end(), element[elementId]);
+ ASSERT_NE(e111Neighbors.end(), found);
+ }
+}
+
+TYPED_TEST(Grid3DTestBase, ResetTest)
+{
+ typedef typename TestFixture::GridType GridType;
+ typedef typename TestFixture::TypeElement TypeElement;
+
+ GridType grid(this->m_size, this->m_aabb);
+
+ TypeElement e0(0), e1(1);
+ grid.addElement(e0, Eigen::Matrix<double, TestFixture::dimension, 1>::Zero());
+ grid.addElement(e1, Eigen::Matrix<double, TestFixture::dimension, 1>::Zero());
+ ASSERT_NE(0u, grid.getActiveCells().size());
+ ASSERT_NE(0u, grid.getCellIds().size());
+ ASSERT_NE(0u, grid.getNeighbors(e0).size());
+ ASSERT_NE(0u, grid.getNeighbors(e1).size());
+
+ ASSERT_NO_THROW(grid.reset());
+ ASSERT_EQ(0u, grid.getActiveCells().size());
+ ASSERT_EQ(0u, grid.getCellIds().size());
+ ASSERT_EQ(0u, grid.getNeighbors(e0).size());
+ ASSERT_EQ(0u, grid.getNeighbors(e1).size());
+}
+
+}; // namespace DataStructures
+}; // namespace SurgSim
diff --git a/SurgSim/DataStructures/UnitTests/GridTests.cpp b/SurgSim/DataStructures/UnitTests/GridTests.cpp
new file mode 100644
index 0000000..4f55bc8
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/GridTests.cpp
@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/Grid.h"
+
+TEST(Grid, PowerOf3StaticAssertionTest)
+{
+ static_assert(SurgSim::DataStructures::powerOf3<0>::value == 1, "3^0 != 1");
+ static_assert(SurgSim::DataStructures::powerOf3<1>::value == 3, "3^1 != 3");
+ static_assert(SurgSim::DataStructures::powerOf3<2>::value == 9, "3^2 != 9");
+ static_assert(SurgSim::DataStructures::powerOf3<3>::value == 27, "3^3 != 27");
+}
diff --git a/SurgSim/DataStructures/UnitTests/GridTests.h b/SurgSim/DataStructures/UnitTests/GridTests.h
new file mode 100644
index 0000000..5187521
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/GridTests.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_DATASTRUCTURES_UNITTESTS_GRIDTESTS_H
+#define SURGSIM_DATASTRUCTURES_UNITTESTS_GRIDTESTS_H
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/DataStructures/Grid.h"
+#include "SurgSim/DataStructures/UnitTests/MockObjects.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <typename T, size_t N>
+class GridTestBase : public testing::Test
+{
+public:
+ /// The test parameters
+ typedef T TypeElement;
+ static const size_t dimension = N;
+
+ /// The grid type for this test
+ typedef MockGrid<TypeElement, dimension> GridType;
+
+ /// Useful vector type of various vectors
+ typedef Eigen::Matrix<double, dimension, 1> VectorND;
+
+ /// Useful variables
+ VectorND m_size;
+
+ /// Grid min and max n-d vectors
+ Eigen::AlignedBox<double, dimension> m_aabb;
+ Eigen::AlignedBox<double, dimension> m_aabb1Cell;
+ Eigen::AlignedBox<double, dimension> m_aabbBig;
+
+ void SetUp() override
+ {
+ m_size = VectorND::LinSpaced(0.6, 1.67);
+
+ VectorND numCells = VectorND::LinSpaced(16.45, 257.43);
+
+ m_aabb.min() = -(numCells.cwiseProduct(m_size) * 0.5);
+ m_aabb.max() = numCells.cwiseProduct(m_size) * 0.5;
+
+ m_aabb1Cell.min() = -(m_size * 0.5);
+ m_aabb1Cell.max() = m_size * 0.5;
+
+ size_t architectureSize = sizeof(size_t) * 8;
+ m_aabbBig.min() = -static_cast<double>(static_cast<size_t>(1u) << (architectureSize - 2)) * m_size * 0.5;
+ m_aabbBig.max() = static_cast<double>(static_cast<size_t>(1u) << (architectureSize - 2)) * m_size * 0.5;
+ }
+};
+
+}; // namespace DataStructures
+}; // namespace SurgSim
+
+#endif // SURGSIM_DATASTRUCTURES_UNITTESTS_GRIDTESTS_H
diff --git a/SurgSim/DataStructures/UnitTests/GroupsTests.cpp b/SurgSim/DataStructures/UnitTests/GroupsTests.cpp
new file mode 100644
index 0000000..c61acd0
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/GroupsTests.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.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/Groups.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Testing/Utilities.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+TEST(GroupsTests, InitTest)
+{
+ // The comma in the template is throwing off the macro, predefine the types for testing
+ typedef Groups<int, int> TypeA;
+ EXPECT_NO_THROW(TypeA groups;);
+
+ typedef Groups<std::string, int> TypeB;
+ EXPECT_NO_THROW({TypeB groups;});
+
+ typedef Groups <std::string, std::shared_ptr<int>> TypeC;
+ EXPECT_NO_THROW({TypeC groups;});
+}
+
+TEST(GroupsTest, DefaultInitialization)
+{
+ Groups<std::string, int> groups;
+
+ EXPECT_TRUE(groups.getGroups().empty());
+
+ EXPECT_TRUE(groups.getGroups(1).empty());
+
+ EXPECT_TRUE(groups.getMembers("None").empty());
+}
+
+TEST(GroupsTest, Clear)
+{
+ Groups<std::string, std::shared_ptr<Framework::BasicSceneElement>> groups;
+
+ auto element1 = std::make_shared<Framework::BasicSceneElement>("One");
+ EXPECT_TRUE(groups.add("One", element1));
+ EXPECT_TRUE(groups.add("Two", element1));
+
+ groups.clear();
+
+ EXPECT_EQ(0L, groups.getGroups().size());
+ EXPECT_EQ(0L, groups.getGroups(element1).size());
+ EXPECT_EQ(0L, groups.getMembers("One").size());
+ EXPECT_EQ(0L, groups.getMembers("Two").size());
+}
+
+TEST(GroupsTests, AddElement)
+{
+ Groups<std::string, std::shared_ptr<Framework::BasicSceneElement>> groups;
+
+ auto element1 = std::make_shared<Framework::BasicSceneElement>("One");
+ auto element2 = std::make_shared<Framework::BasicSceneElement>("Two");
+
+ EXPECT_TRUE(groups.add("One", element1));
+
+ EXPECT_EQ(1L, groups.getGroups().size());
+ EXPECT_EQ(1L, groups.getGroups(element1).size());
+ EXPECT_EQ(1L, groups.getMembers("One").size());
+
+ EXPECT_FALSE(groups.add("One", element1));
+
+ EXPECT_EQ(1L, groups.getGroups().size());
+ EXPECT_EQ(1L, groups.getGroups(element1).size());
+ EXPECT_EQ(1L, groups.getMembers("One").size());
+
+ groups.add("Two", element1);
+
+ EXPECT_EQ(2L, groups.getGroups().size());
+ EXPECT_EQ(2L, groups.getGroups(element1).size());
+ EXPECT_EQ(1L, groups.getMembers("One").size());
+
+ groups.add("Two", element2);
+
+ EXPECT_EQ(2L, groups.getGroups().size());
+ EXPECT_EQ(2L, groups.getGroups(element1).size());
+ EXPECT_EQ(1L, groups.getGroups(element2).size());
+ EXPECT_EQ(1L, groups.getMembers("One").size());
+ EXPECT_EQ(2L, groups.getMembers("Two").size());
+
+
+ auto members = groups.getMembers("Two");
+
+
+ auto names = groups.getGroups();
+
+ EXPECT_TRUE(SurgSim::Testing::doesContain(members, element1));
+ EXPECT_TRUE(SurgSim::Testing::doesContain(members, element2));
+
+ EXPECT_TRUE(SurgSim::Testing::doesContain(names, "One"));
+ EXPECT_TRUE(SurgSim::Testing::doesContain(names, "Two"));
+}
+
+TEST(GroupsTests, MultiAddTest)
+{
+ Groups<std::string, std::shared_ptr<Framework::BasicSceneElement>> groups;
+
+ auto element1 = std::make_shared<Framework::BasicSceneElement>("One");
+ auto element2 = std::make_shared<Framework::BasicSceneElement>("Two");
+
+ std::vector<std::string> names;
+ names.push_back("One");
+ names.push_back("Two");
+
+ groups.add(names, element1);
+ EXPECT_EQ(2L, groups.getGroups().size());
+ EXPECT_EQ(2L, groups.getGroups(element1).size());
+}
+
+TEST(GroupsTests, AddGroups)
+{
+ Groups<std::string, int> groups;
+
+ Groups<std::string, int> sourceGroups;
+
+ groups.add("One", 1);
+ groups.add("Two", 1);
+
+ sourceGroups.add("One", 2);
+ sourceGroups.add("Two", 1);
+ sourceGroups.add("Three", 1);
+
+ EXPECT_FALSE(groups.add(groups));
+
+ EXPECT_EQ(2L, groups.getGroups().size());
+ EXPECT_EQ(2L, groups.getGroups(1).size());
+
+ EXPECT_TRUE(groups.add(sourceGroups));
+
+ EXPECT_EQ(3L, groups.getGroups().size());
+ EXPECT_EQ(3L, groups.getGroups(1).size());
+ EXPECT_EQ(1L, groups.getGroups(2).size());
+ EXPECT_EQ(2L, groups.getMembers("One").size());
+ EXPECT_EQ(1L, groups.getMembers("Two").size());
+ EXPECT_EQ(1L, groups.getMembers("Three").size());
+}
+
+TEST(GroupsTests, BracketOperator)
+{
+ Groups<std::string, std::shared_ptr<Framework::BasicSceneElement>> groups;
+
+ auto element1 = std::make_shared<Framework::BasicSceneElement>("One");
+ auto element2 = std::make_shared<Framework::BasicSceneElement>("Two");
+
+
+ EXPECT_EQ(0L, groups["One"].size());
+
+ groups.add("One", element1);
+ groups.add("One", element2);
+
+ EXPECT_EQ(2L, groups["One"].size());
+}
+
+TEST(GroupsTests, ValidRemoves)
+{
+ Groups<std::string, std::shared_ptr<Framework::BasicSceneElement>> groups;
+
+ auto element1 = std::make_shared<Framework::BasicSceneElement>("One");
+ auto element2 = std::make_shared<Framework::BasicSceneElement>("Two");
+ auto element3 = std::make_shared<Framework::BasicSceneElement>("Three");
+
+ groups.add("One", element1);
+ groups.add("Two", element1);
+ groups.add("Three", element1);
+ groups.add("One", element2);
+ groups.add("Two", element2);
+
+ // Just check for the expected state ... (is tested above)
+ EXPECT_EQ(3L, groups.getGroups().size());
+ EXPECT_EQ(3L, groups.getGroups(element1).size());
+ EXPECT_EQ(2L, groups.getGroups(element2).size());
+ EXPECT_EQ(2L, groups.getMembers("One").size());
+ EXPECT_EQ(2L, groups.getMembers("Two").size());
+ EXPECT_EQ(1L, groups.getMembers("Three").size());
+
+ EXPECT_TRUE(groups.remove("Three", element1));
+
+ EXPECT_EQ(2L, groups.getGroups().size());
+ EXPECT_EQ(2L, groups.getGroups(element1).size());
+ EXPECT_EQ(2L, groups.getGroups(element2).size());
+ EXPECT_EQ(2L, groups.getMembers("One").size());
+ EXPECT_EQ(2L, groups.getMembers("Two").size());
+ EXPECT_EQ(0L, groups.getMembers("Three").size());
+
+ EXPECT_TRUE(groups.remove("One", element2));
+
+ EXPECT_EQ(2L, groups.getGroups().size());
+ EXPECT_EQ(2L, groups.getGroups(element1).size());
+ EXPECT_EQ(1L, groups.getGroups(element2).size());
+ EXPECT_EQ(1L, groups.getMembers("One").size());
+ EXPECT_EQ(2L, groups.getMembers("Two").size());
+ EXPECT_EQ(0L, groups.getMembers("Three").size());
+
+ groups.remove("One", element1);
+ groups.remove("Two", element1);
+ groups.remove("Two", element2);
+
+ EXPECT_EQ(0L, groups.getGroups().size());
+ EXPECT_EQ(0L, groups.getGroups(element1).size());
+ EXPECT_EQ(0L, groups.getGroups(element2).size());
+ EXPECT_EQ(0L, groups.getMembers("One").size());
+ EXPECT_EQ(0L, groups.getMembers("Two").size());
+ EXPECT_EQ(0L, groups.getMembers("Three").size());
+
+}
+
+TEST(GroupsTests, InvalidRemoves)
+{
+ Groups<std::string, std::shared_ptr<Framework::BasicSceneElement>> groups;
+
+ auto element1 = std::make_shared<Framework::BasicSceneElement>("One");
+ auto element2 = std::make_shared<Framework::BasicSceneElement>("Two");
+ auto element3 = std::make_shared<Framework::BasicSceneElement>("Three");
+
+ groups.add("One", element1);
+ groups.add("Two", element1);
+ groups.add("Three", element1);
+ groups.add("One", element2);
+ groups.add("Two", element2);
+
+ // Remove non existing element from group should not do anything
+ EXPECT_FALSE(groups.remove("One", element3));
+ EXPECT_EQ(2L, groups.getMembers("One").size());
+ EXPECT_EQ(0L, groups.getGroups(element3).size());
+
+ // Removing from a non existing group should not do anything
+ EXPECT_FALSE(groups.remove("None", element1));
+ EXPECT_EQ(3L, groups.getGroups().size());
+ EXPECT_EQ(3L, groups.getGroups(element1).size());
+
+ // Removing a non exisiting member from a nonexisting group should not do anything
+ EXPECT_FALSE(groups.remove("None", element3));
+ EXPECT_EQ(3L, groups.getGroups().size());
+ EXPECT_EQ(0L, groups.getGroups(element3).size());
+
+ // Removing a non exisitng member categorically should not do anything
+ EXPECT_FALSE(groups.remove(element3));
+ EXPECT_EQ(3L, groups.getGroups().size());
+ EXPECT_EQ(0L, groups.getGroups(element3).size());
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/DataStructures/UnitTests/ImageMapTest.cpp b/SurgSim/DataStructures/UnitTests/ImageMapTest.cpp
new file mode 100644
index 0000000..c227026
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/ImageMapTest.cpp
@@ -0,0 +1,178 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 ImageMap class.
+
+#include "gtest/gtest.h"
+
+#include "SurgSim/DataStructures/ImageMap.h"
+
+
+namespace
+{
+double epsilon = 1e-10;
+}
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <class T>
+class ImageMapTests : public testing::Test
+{
+public:
+ typedef T Scalar;
+};
+
+typedef ::testing::Types<unsigned char, char, unsigned int, int, float, double> ImageMapTestTypes;
+TYPED_TEST_CASE(ImageMapTests, ImageMapTestTypes);
+
+TYPED_TEST(ImageMapTests, Construct)
+{
+ typedef typename TestFixture::Scalar T;
+
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+ ASSERT_NO_THROW({ImageMap<T> image(3, 3, 1, array);});
+}
+
+TYPED_TEST(ImageMapTests, Copy)
+{
+ typedef typename TestFixture::Scalar T;
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+ ImageMap<T> image(3, 3, 1, array);
+ ImageMap<T> newImage(image);
+ EXPECT_EQ(image.getSize(), newImage.getSize());
+ EXPECT_EQ(image.getData(), newImage.getData());
+}
+
+TYPED_TEST(ImageMapTests, Assign)
+{
+ typedef typename TestFixture::Scalar T;
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+ ImageMap<T> image(3, 3, 1, array);
+
+ T newArray[] = {0, 1, 2, 3};
+ ImageMap<T> newImage(2, 2, 1, newArray);
+
+ EXPECT_NE(image.getSize(), newImage.getSize());
+ EXPECT_NE(image.getData(), newImage.getData());
+ newImage = image;
+ EXPECT_EQ(image.getSize(), newImage.getSize());
+ EXPECT_EQ(image.getData(), newImage.getData());
+}
+
+TYPED_TEST(ImageMapTests, Accessors)
+{
+ typedef typename TestFixture::Scalar T;
+
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+ ImageMap<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(ImageMapTests, Move)
+{
+ typedef typename TestFixture::Scalar T;
+
+ {
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+ ImageMap<T> oldImage(3, 3, 1, array);
+ T* const dataPtr = oldImage.getData();
+ ImageMap<T> newImage = std::move(oldImage);
+
+ EXPECT_EQ(dataPtr, newImage.getData());
+ EXPECT_EQ(3, newImage.getWidth());
+ EXPECT_EQ(3, newImage.getHeight());
+ EXPECT_EQ(1, newImage.getNumChannels());
+ }
+ {
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+ ImageMap<T> oldImage(3, 1, 3, array);
+ T* const dataPtr = oldImage.getData();
+ ImageMap<T> newImage(std::move(oldImage));
+
+ EXPECT_EQ(dataPtr, newImage.getData());
+ EXPECT_EQ(3, newImage.getWidth());
+ EXPECT_EQ(1, newImage.getHeight());
+ EXPECT_EQ(3, newImage.getNumChannels());
+ }
+}
+
+TYPED_TEST(ImageMapTests, ArrayMapping)
+{
+ typedef typename TestFixture::Scalar T;
+
+ {
+ SCOPED_TRACE("Modifying the array should modify the data in the ImageMap");
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7};
+ ImageMap<T> image(2, 2, 2, array);
+ array[2] = 20;
+ EXPECT_NEAR(20, image.getData()[2], epsilon);
+ }
+ {
+ SCOPED_TRACE("Modifying the ImageMap with getData should modify the array");
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7};
+ ImageMap<T> image(2, 2, 2, array);
+ image.getData()[3] = 30;
+ EXPECT_NEAR(30, array[3], epsilon);
+ }
+ {
+ SCOPED_TRACE("Modifying the ImageMap with operator() should modify the array");
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7};
+ ImageMap<T> image(2, 2, 2, array);
+ image(1, 1) << 100, 101;
+ EXPECT_NEAR(100, array[6], epsilon);
+ EXPECT_NEAR(101, array[7], epsilon);
+ }
+ {
+ SCOPED_TRACE("Modifying the ImageMap with getChannel() should modify the array");
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7};
+ ImageMap<T> image(2, 2, 2, array);
+ image.getChannel(1) << 100, 101, 110, 111;
+ EXPECT_NEAR(100, array[1], epsilon);
+ EXPECT_NEAR(110, array[3], epsilon);
+ EXPECT_NEAR(101, array[5], epsilon);
+ EXPECT_NEAR(111, array[7], epsilon);
+ }
+ {
+ SCOPED_TRACE("Modifying the ImageMap with getAsVector() should modify the array");
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7};
+ ImageMap<T> image(2, 2, 2, array);
+ image.getAsVector() << 100, 101, 102, 103, 104, 105, 106, 107;
+ EXPECT_NEAR(100, array[0], epsilon);
+ EXPECT_NEAR(101, array[1], epsilon);
+ EXPECT_NEAR(102, array[2], epsilon);
+ EXPECT_NEAR(103, array[3], epsilon);
+ EXPECT_NEAR(104, array[4], epsilon);
+ EXPECT_NEAR(105, array[5], epsilon);
+ EXPECT_NEAR(106, array[6], epsilon);
+ EXPECT_NEAR(107, array[7], epsilon);
+ }
+}
+
+};
+};
diff --git a/SurgSim/DataStructures/UnitTests/ImageTest.cpp b/SurgSim/DataStructures/UnitTests/ImageTest.cpp
index 461b25f..50d8983 100644
--- a/SurgSim/DataStructures/UnitTests/ImageTest.cpp
+++ b/SurgSim/DataStructures/UnitTests/ImageTest.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2012-2013, SimQuest Solutions Inc.
+// Copyright 2012-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -21,12 +21,12 @@
#include "SurgSim/DataStructures/Image.h"
#include "SurgSim/Math/Matrix.h"
+
namespace
{
double epsilon = 1e-10;
}
-
namespace SurgSim
{
namespace DataStructures
@@ -55,6 +55,18 @@ TYPED_TEST(ImageTests, Construct)
ASSERT_NO_THROW({Image<T> image(3, 3, 1, array);});
}
+TYPED_TEST(ImageTests, ConstructFromOtherType)
+{
+ typedef typename TestFixture::Scalar T;
+
+ double array[] = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0};
+ Image<T> image(3, 3, 1, array);
+ for (int i = 0; i < 9; i++)
+ {
+ EXPECT_NEAR(static_cast<T>(array[i]), image.getData()[i], epsilon);
+ }
+}
+
TYPED_TEST(ImageTests, Copy)
{
typedef typename TestFixture::Scalar T;
@@ -166,7 +178,48 @@ TYPED_TEST(ImageTests, PointerAccess)
}
}
-TYPED_TEST(ImageTests, EigenAccess)
+TYPED_TEST(ImageTests, PixelAccess)
+{
+ typedef typename TestFixture::Scalar T;
+ typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;
+
+ {
+ Image<T> image(20, 30, 3);
+ EXPECT_THROW(image(20, 10), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(image(10, 40), SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(image(1, 20));
+ EXPECT_NO_THROW(image(10, 29));
+ }
+ {
+ Image<T> image(300, 300, 3);
+ image.setChannel(0, Matrix::Constant(300, 300, T(0)));
+ image.setChannel(1, Matrix::Constant(300, 300, T(1)));
+ image.setChannel(2, Matrix::Constant(300, 300, T(2)));
+
+ Eigen::Matrix<T, 3, 1> pixelValue(T(0), T(1), T(2));
+ EXPECT_TRUE(pixelValue.isApprox(image(1, 2)));
+ EXPECT_TRUE(pixelValue.isApprox(image(100, 50)));
+ EXPECT_TRUE(pixelValue.isApprox(image(50, 200)));
+ }
+ {
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7};
+ Image<T> image(2, 2, 2, array);
+ typedef Eigen::Matrix<T, 2, 1> PixelType;
+ EXPECT_TRUE(PixelType(T(0), T(1)).isApprox(image(0, 0)));
+ EXPECT_TRUE(PixelType(T(2), T(3)).isApprox(image(1, 0)));
+ EXPECT_TRUE(PixelType(T(4), T(5)).isApprox(image(0, 1)));
+ EXPECT_TRUE(PixelType(T(6), T(7)).isApprox(image(1, 1)));
+ }
+ {
+ Image<T> image(10, 10, 2);
+ image.getAsVector().setConstant(T(0));
+ Eigen::Matrix<T, 2, 1> pixelValue(T(10), T(20));
+ image(5, 5) = pixelValue;
+ EXPECT_TRUE(pixelValue.isApprox(image(5, 5)));
+ }
+}
+
+TYPED_TEST(ImageTests, ChannelAccess)
{
typedef typename TestFixture::Scalar T;
typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;
@@ -179,10 +232,17 @@ TYPED_TEST(ImageTests, EigenAccess)
EXPECT_THROW(image.getChannel(100), SurgSim::Framework::AssertionFailure);
}
{
+ Image<T> image(10, 20, 3);
+ EXPECT_THROW(image.setChannel(100, Matrix::Constant(10, 20, T(0))), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(image.setChannel(0, Matrix::Constant(20, 10, T(0))), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(image.setChannel(2, Matrix::Constant(20, 20, T(0))), SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(image.setChannel(2, Matrix::Constant(10, 20, T(0))));
+ }
+ {
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));
+ image.setChannel(0, Matrix::Constant(300, 300, T(0)));
+ image.setChannel(1, Matrix::Constant(300, 300, T(1)));
+ image.setChannel(2, Matrix::Constant(300, 300, T(2)));
for (int i = 0; i < 300*300; i++)
{
@@ -206,7 +266,77 @@ TYPED_TEST(ImageTests, EigenAccess)
EXPECT_NEAR(30, matrix(3, 0), epsilon);
EXPECT_NEAR(54, matrix(5, 4), epsilon);
}
-}
+ {
+ Image<T> image(10, 30, 1);
+ image.setChannel(0, Matrix::Constant(10, 30, T(1)));
+ image.setChannel(0, image.getChannel(0) * T(2));
+ EXPECT_TRUE(image.getChannel(0).isApprox(Matrix::Constant(10, 30, T(2))));
+
+ image.setChannel(0, image.getChannel(0).array() + T(3));
+ EXPECT_TRUE(image.getChannel(0).isApprox(Matrix::Constant(10, 30, T(5))));
+ }
+ {
+ Image<T> image(10, 30, 1);
+ image.getChannel(0) = Matrix::Constant(10, 30, T(1));
+ image.getChannel(0) *= 2;
+ EXPECT_TRUE(image.getChannel(0).isApprox(Matrix::Constant(10, 30, T(2))));
+ image.getChannel(0) = image.getChannel(0).array() + T(3);
+ EXPECT_TRUE(image.getChannel(0).isApprox(Matrix::Constant(10, 30, T(5))));
+ }
}
+
+TYPED_TEST(ImageTests, VectorAccess)
+{
+ typedef typename TestFixture::Scalar T;
+ typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;
+
+ T array[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+ {
+ Image<T> image(3, 3, 1, array);
+ for (int i = 0; i < 9; i++)
+ {
+ auto vector = image.getAsVector();
+ EXPECT_NEAR(array[i], vector[i], epsilon);
+ }
+ }
+ {
+ Image<T> image(3, 1, 3, array);
+ for (int i = 0; i < 9; i++)
+ {
+ auto vector = image.getAsVector();
+ EXPECT_NEAR(array[i], vector[i], epsilon);
+ }
+ }
+ {
+ Image<T> image(1, 3, 3, array);
+ for (int i = 0; i < 9; i++)
+ {
+ auto vector = image.getAsVector();
+ EXPECT_NEAR(array[i], vector[i], epsilon);
+ }
+ }
+ {
+ Image<T> image(10, 20, 3);
+ EXPECT_THROW(image.setAsVector(Matrix::Constant(2, 1, T(0))), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(image.setAsVector(Matrix::Constant(10*20, 1, T(0))), SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(image.setAsVector(Matrix::Constant(10*20*3, 1, T(0))));
+ }
+ {
+ Image<T> image(3,3,1);
+ image.setAsVector(Matrix::Constant(9, 1, T(7)));
+ EXPECT_TRUE(image.getAsVector().isApprox(Matrix::Constant(9, 1, T(7))));
+ EXPECT_TRUE(image.getChannel(0).isApprox(Matrix::Constant(3, 3, T(7))));
+
+ image.setAsVector(image.getAsVector() * T(2));
+ EXPECT_TRUE(image.getAsVector().isApprox(Matrix::Constant(9, 1, T(14))));
+ EXPECT_TRUE(image.getChannel(0).isApprox(Matrix::Constant(3, 3, T(14))));
+
+ image.getAsVector() *= T(2);
+ EXPECT_TRUE(image.getAsVector().isApprox(Matrix::Constant(9, 1, T(28))));
+ EXPECT_TRUE(image.getChannel(0).isApprox(Matrix::Constant(3, 3, T(28))));
+ }
}
+
+};
+};
diff --git a/SurgSim/DataStructures/UnitTests/IndexedLocalCoordinateTest.cpp b/SurgSim/DataStructures/UnitTests/IndexedLocalCoordinateTest.cpp
index 06ad886..324b84a 100644
--- a/SurgSim/DataStructures/UnitTests/IndexedLocalCoordinateTest.cpp
+++ b/SurgSim/DataStructures/UnitTests/IndexedLocalCoordinateTest.cpp
@@ -61,5 +61,33 @@ TEST(IndexedLocalCoordinateTest, IndexedLocalCoordinate)
}
}
+TEST(IndexedLocalCoordinateTest, IsApprox)
+{
+ using SurgSim::Math::Vector3d;
+ double epsilon = 1e-12;
+
+ IndexedLocalCoordinate null6(6u, Vector3d::Zero());
+ IndexedLocalCoordinate almostNull6(6u, Vector3d(0.0, 0.0, epsilon / 2.0));
+ IndexedLocalCoordinate null5(5u, Vector3d::Zero());
+ IndexedLocalCoordinate almostNull5(5u, Vector3d(0.0, 0.0, epsilon / 2.0));
+
+ EXPECT_TRUE(null6.isApprox(null6, epsilon));
+ EXPECT_TRUE(null6.isApprox(almostNull6, epsilon));
+ EXPECT_FALSE(null6.isApprox(null5, epsilon));
+ EXPECT_FALSE(null6.isApprox(almostNull5, epsilon));
+
+ IndexedLocalCoordinate one6(6u, Vector3d::Ones());
+ IndexedLocalCoordinate almostOne6(6u, Vector3d::Ones() + Vector3d(0.0, 0.0, epsilon / 2.0));
+ IndexedLocalCoordinate one5(5u, Vector3d::Ones());
+ IndexedLocalCoordinate two6(6u, Vector3d::Constant(2.0));
+ IndexedLocalCoordinate three7(7u, Vector3d::Constant(3.0));
+
+ EXPECT_TRUE(one6.isApprox(one6, epsilon));
+ EXPECT_TRUE(one6.isApprox(almostOne6, epsilon));
+ EXPECT_FALSE(one6.isApprox(one5, epsilon));
+ EXPECT_FALSE(one6.isApprox(two6, epsilon));
+ EXPECT_FALSE(one6.isApprox(three7, epsilon));
+}
+
} // namespace Collision
} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/DataStructures/UnitTests/LocationTests.cpp b/SurgSim/DataStructures/UnitTests/LocationTests.cpp
index bb08f2b..0dccc34 100644
--- a/SurgSim/DataStructures/UnitTests/LocationTests.cpp
+++ b/SurgSim/DataStructures/UnitTests/LocationTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -25,44 +25,238 @@ namespace DataStructures
TEST(LocationTests, Constructor)
{
- SurgSim::Math::Vector3d rigidLocalPosition = SurgSim::Math::Vector3d::Ones();
- SurgSim::DataStructures::OctreePath octreeNodePath;
+ Math::Vector3d rigidLocalPosition = Math::Vector3d::Ones();
+ 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));
+ size_t index(3);
+ IndexedLocalCoordinate triangleMeshLocalCoordinate(1, Math::Vector2d(4.0, 5.0));
+ IndexedLocalCoordinate elementMeshLocalCoordinate(1, Math::Vector4d::Ones());
EXPECT_NO_THROW({Location location(rigidLocalPosition);});
EXPECT_NO_THROW({Location location(octreeNodePath);});
- EXPECT_NO_THROW({Location location(meshLocalCoordinate);});
+ EXPECT_NO_THROW({Location location(index);});
+ EXPECT_NO_THROW({Location location(triangleMeshLocalCoordinate, Location::TRIANGLE);});
+ EXPECT_NO_THROW({Location location(elementMeshLocalCoordinate, Location::ELEMENT);});
+ EXPECT_THROW({Location location(elementMeshLocalCoordinate, \
+ static_cast<Location::Type>(Location::ELEMENT + 10));}, Framework::AssertionFailure);
{
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_FALSE(location.octreeNodePath.hasValue());
+ EXPECT_FALSE(location.index.hasValue());
+ EXPECT_FALSE(location.triangleMeshLocalCoordinate.hasValue());
+ EXPECT_FALSE(location.elementMeshLocalCoordinate.hasValue());
+
EXPECT_TRUE(location.rigidLocalPosition.getValue().isApprox(rigidLocalPosition));
+
+ Location location1(location);
+ EXPECT_TRUE(location1.rigidLocalPosition.hasValue());
+ EXPECT_FALSE(location1.octreeNodePath.hasValue());
+ EXPECT_FALSE(location1.index.hasValue());
+ EXPECT_FALSE(location1.triangleMeshLocalCoordinate.hasValue());
+ EXPECT_FALSE(location1.elementMeshLocalCoordinate.hasValue());
+
+ EXPECT_TRUE(location1.rigidLocalPosition.getValue().isApprox(rigidLocalPosition));
}
{
SCOPED_TRACE("Using octree node path");
Location location(octreeNodePath);
- EXPECT_FALSE(location.meshLocalCoordinate.hasValue());
+
+ EXPECT_EQ(octreeNodePath, location.octreeNodePath.getValue());
+ EXPECT_FALSE(location.rigidLocalPosition.hasValue());
EXPECT_TRUE(location.octreeNodePath.hasValue());
+ EXPECT_FALSE(location.index.hasValue());
+ EXPECT_FALSE(location.triangleMeshLocalCoordinate.hasValue());
+ EXPECT_FALSE(location.elementMeshLocalCoordinate.hasValue());
+
+ Location location1(location);
+ EXPECT_FALSE(location1.rigidLocalPosition.hasValue());
+ EXPECT_TRUE(location1.octreeNodePath.hasValue());
+ EXPECT_FALSE(location1.index.hasValue());
+ EXPECT_FALSE(location1.triangleMeshLocalCoordinate.hasValue());
+ EXPECT_FALSE(location1.elementMeshLocalCoordinate.hasValue());
+
+ EXPECT_EQ(octreeNodePath, location1.octreeNodePath.getValue());
+ }
+
+ {
+ SCOPED_TRACE("Using index");
+ Location location(index);
+
+ EXPECT_EQ(index, location.index.getValue());
EXPECT_FALSE(location.rigidLocalPosition.hasValue());
- EXPECT_EQ(octreeNodePath, location.octreeNodePath.getValue());
+ EXPECT_FALSE(location.octreeNodePath.hasValue());
+ EXPECT_TRUE(location.index.hasValue());
+ EXPECT_FALSE(location.triangleMeshLocalCoordinate.hasValue());
+ EXPECT_FALSE(location.elementMeshLocalCoordinate.hasValue());
+
+ Location location1(location);
+ EXPECT_FALSE(location1.rigidLocalPosition.hasValue());
+ EXPECT_FALSE(location1.octreeNodePath.hasValue());
+ EXPECT_TRUE(location1.index.hasValue());
+ EXPECT_FALSE(location1.triangleMeshLocalCoordinate.hasValue());
+ EXPECT_FALSE(location1.elementMeshLocalCoordinate.hasValue());
+
+ EXPECT_EQ(index, location1.index.getValue());
}
{
- SCOPED_TRACE("Using mesh local coordinate");
- Location location(meshLocalCoordinate);
- EXPECT_TRUE(location.meshLocalCoordinate.hasValue());
+ SCOPED_TRACE("Using triangle mesh local coordinate");
+ Location location(triangleMeshLocalCoordinate, Location::TRIANGLE);
+ EXPECT_FALSE(location.rigidLocalPosition.hasValue());
EXPECT_FALSE(location.octreeNodePath.hasValue());
+ EXPECT_FALSE(location.index.hasValue());
+ EXPECT_TRUE(location.triangleMeshLocalCoordinate.hasValue());
+ EXPECT_FALSE(location.elementMeshLocalCoordinate.hasValue());
+
+ EXPECT_EQ(triangleMeshLocalCoordinate.index, location.triangleMeshLocalCoordinate.getValue().index);
+ EXPECT_TRUE(location.triangleMeshLocalCoordinate.getValue().coordinate.isApprox(\
+ triangleMeshLocalCoordinate.coordinate));
+
+ Location location1(location);
+ EXPECT_FALSE(location1.rigidLocalPosition.hasValue());
+ EXPECT_FALSE(location1.octreeNodePath.hasValue());
+ EXPECT_FALSE(location1.index.hasValue());
+ EXPECT_TRUE(location1.triangleMeshLocalCoordinate.hasValue());
+ EXPECT_FALSE(location1.elementMeshLocalCoordinate.hasValue());
+
+ EXPECT_EQ(triangleMeshLocalCoordinate.index, location1.triangleMeshLocalCoordinate.getValue().index);
+ EXPECT_TRUE(location1.triangleMeshLocalCoordinate.getValue().coordinate.isApprox(\
+ triangleMeshLocalCoordinate.coordinate));
+ }
+
+ {
+ SCOPED_TRACE("Using element mesh local coordinate");
+ Location location(elementMeshLocalCoordinate, Location::ELEMENT);
EXPECT_FALSE(location.rigidLocalPosition.hasValue());
- EXPECT_EQ(meshLocalCoordinate.index, location.meshLocalCoordinate.getValue().index);
- EXPECT_TRUE(location.meshLocalCoordinate.getValue().coordinate.isApprox(meshLocalCoordinate.coordinate));
+ EXPECT_FALSE(location.octreeNodePath.hasValue());
+ EXPECT_FALSE(location.index.hasValue());
+ EXPECT_FALSE(location.triangleMeshLocalCoordinate.hasValue());
+ EXPECT_TRUE(location.elementMeshLocalCoordinate.hasValue());
+
+ EXPECT_EQ(elementMeshLocalCoordinate.index, location.elementMeshLocalCoordinate.getValue().index);
+ EXPECT_TRUE(location.elementMeshLocalCoordinate.getValue().coordinate.isApprox(\
+ elementMeshLocalCoordinate.coordinate));
+
+ Location location1(location);
+ EXPECT_FALSE(location1.rigidLocalPosition.hasValue());
+ EXPECT_FALSE(location1.octreeNodePath.hasValue());
+ EXPECT_FALSE(location1.index.hasValue());
+ EXPECT_FALSE(location1.triangleMeshLocalCoordinate.hasValue());
+ EXPECT_TRUE(location1.elementMeshLocalCoordinate.hasValue());
+
+ EXPECT_EQ(elementMeshLocalCoordinate.index, location1.elementMeshLocalCoordinate.getValue().index);
+ EXPECT_TRUE(location1.elementMeshLocalCoordinate.getValue().coordinate.isApprox(\
+ elementMeshLocalCoordinate.coordinate));
+ }
+}
+
+TEST(LocationTests, IsApprox)
+{
+ double epsilon = 1e-15;
+
+ {
+ SCOPED_TRACE("Rigid location");
+ Location rigidLocationOnes(Math::Vector3d::Ones());
+ Location rigidLocationZero(Math::Vector3d::Zero());
+ Location rigidLocationAlmostOnes(Math::Vector3d(1.0, 1.0, 1 + epsilon * 0.5));
+ Location rigidLocationAlmostZero(Math::Vector3d(0.0, 0.0, epsilon * 0.5));
+
+ EXPECT_FALSE(rigidLocationOnes.isApprox(rigidLocationZero, epsilon));
+ EXPECT_TRUE(rigidLocationOnes.isApprox(rigidLocationOnes, epsilon));
+ EXPECT_TRUE(rigidLocationOnes.isApprox(rigidLocationAlmostOnes, epsilon));
+ EXPECT_FALSE(rigidLocationOnes.isApprox(rigidLocationAlmostZero, epsilon));
+
+ EXPECT_TRUE(rigidLocationZero.isApprox(rigidLocationZero, epsilon));
+ EXPECT_FALSE(rigidLocationZero.isApprox(rigidLocationOnes, epsilon));
+ EXPECT_FALSE(rigidLocationZero.isApprox(rigidLocationAlmostOnes, epsilon));
+ EXPECT_TRUE(rigidLocationZero.isApprox(rigidLocationAlmostZero, epsilon));
+ }
+
+ {
+ SCOPED_TRACE("Octree location");
+ std::array<int, 3> path1 = {{0, 1, 2}};
+ std::array<int, 3> path2 = {{2, 1, 0}};
+ Location octreeLocation1(OctreePath(path1.begin(), path1.end()));
+ Location octreeLocation2(OctreePath(path2.begin(), path2.end()));
+
+ EXPECT_TRUE(octreeLocation1.isApprox(octreeLocation1));
+ EXPECT_TRUE(octreeLocation2.isApprox(octreeLocation2));
+ EXPECT_FALSE(octreeLocation1.isApprox(octreeLocation2));
+ }
+
+ {
+ SCOPED_TRACE("Index location");
+ Location indexLocationOne(1);
+ Location indexLocationZero(0);
+
+ EXPECT_TRUE(indexLocationOne.isApprox(indexLocationOne));
+ EXPECT_TRUE(indexLocationZero.isApprox(indexLocationZero));
+ EXPECT_FALSE(indexLocationZero.isApprox(indexLocationOne));
}
+
+ {
+ SCOPED_TRACE("TriangleMesh location");
+ auto vector001 = Math::Vector(3);
+ vector001 << 0, 0, 1;
+ auto vector100 = Math::Vector(3);
+ vector100 << 1, 0, 0;
+ auto vector010 = Math::Vector(3);
+ vector010 << 0, 1, 1;
+ IndexedLocalCoordinate triangleBarycentricCoord11(9, vector001);
+ IndexedLocalCoordinate triangleBarycentricCoord12(9, vector100);
+ IndexedLocalCoordinate triangleBarycentricCoord2(0, vector010);
+ Location triangleMeshLocation11(triangleBarycentricCoord11, Location::TRIANGLE);
+ Location triangleMeshLocation12(triangleBarycentricCoord12, Location::TRIANGLE);
+ Location triangleMeshLocation2(triangleBarycentricCoord2, Location::TRIANGLE);
+
+ EXPECT_TRUE(triangleMeshLocation11.isApprox(triangleMeshLocation11));
+ EXPECT_FALSE(triangleMeshLocation11.isApprox(triangleMeshLocation12));
+ EXPECT_FALSE(triangleMeshLocation11.isApprox(triangleMeshLocation2));
+ }
+
+ {
+ SCOPED_TRACE("Element location");
+ auto vector01 = Math::Vector(2);
+ vector01 << 0, 1;
+ auto vector10 = Math::Vector(2);
+ vector10 << 1, 0;
+ IndexedLocalCoordinate triangleBarycentricCoord11(9, vector01);
+ IndexedLocalCoordinate triangleBarycentricCoord12(9, vector10);
+ IndexedLocalCoordinate triangleBarycentricCoord2(0, vector10);
+ Location triangleMeshLocation11(triangleBarycentricCoord11, Location::ELEMENT);
+ Location triangleMeshLocation12(triangleBarycentricCoord12, Location::ELEMENT);
+ Location triangleMeshLocation2(triangleBarycentricCoord2, Location::ELEMENT);
+
+ EXPECT_TRUE(triangleMeshLocation11.isApprox(triangleMeshLocation11));
+ EXPECT_FALSE(triangleMeshLocation11.isApprox(triangleMeshLocation12));
+ EXPECT_FALSE(triangleMeshLocation11.isApprox(triangleMeshLocation2));
+ }
+}
+
+TEST(LocationTests, get)
+{
+ Location loc1;
+
+ EXPECT_FALSE(loc1.get(Location::TRIANGLE).hasValue());
+ EXPECT_FALSE(loc1.get(Location::ELEMENT).hasValue());
+
+ loc1.triangleMeshLocalCoordinate = IndexedLocalCoordinate(0, Math::Vector3d::Zero());
+ EXPECT_TRUE(loc1.get(Location::TRIANGLE).hasValue());
+ EXPECT_FALSE(loc1.get(Location::ELEMENT).hasValue());
+
+ Location loc2;
+
+ loc2.elementMeshLocalCoordinate = IndexedLocalCoordinate(0, Math::Vector3d::Zero());
+ EXPECT_FALSE(loc2.get(Location::TRIANGLE).hasValue());
+ EXPECT_TRUE(loc2.get(Location::ELEMENT).hasValue());
+
}
}; // namespace DataStructures
diff --git a/SurgSim/DataStructures/UnitTests/MockObjects.h b/SurgSim/DataStructures/UnitTests/MockObjects.h
index 26ec965..6af6614 100644
--- a/SurgSim/DataStructures/UnitTests/MockObjects.h
+++ b/SurgSim/DataStructures/UnitTests/MockObjects.h
@@ -16,13 +16,52 @@
#ifndef SURGSIM_DATASTRUCTURES_UNITTESTS_MOCKOBJECTS_H
#define SURGSIM_DATASTRUCTURES_UNITTESTS_MOCKOBJECTS_H
+#include <array>
+#include <limits>
+
+#include "SurgSim/DataStructures/Grid.h"
#include "SurgSim/DataStructures/TetrahedronMesh.h"
-#include "SurgSim/DataStructures/TriangleMeshBase.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
#include "SurgSim/DataStructures/Vertices.h"
#include "SurgSim/Math/Vector.h"
-#include <array>
-#include <limits>
+class ElementTest
+{
+public:
+ int m_number;
+ std::string m_string;
+
+ ElementTest() : m_number(-1), m_string("Empty"){}
+
+ explicit ElementTest(int i) : m_number(i) { std::stringstream s; s << i; m_string = s.str(); }
+
+ ElementTest(const ElementTest& e) : m_number(e.m_number), m_string(e.m_string){}
+
+ bool operator ==(const ElementTest& e) const
+ {
+ return m_number == e.m_number && m_string.compare(e.m_string) == 0;
+ }
+};
+
+// Add a hash function for this class
+namespace std
+{
+template <>
+struct hash<ElementTest>
+{
+ std::size_t operator()(const ElementTest& k) const
+ {
+ using std::size_t;
+ using std::hash;
+ using std::string;
+
+ // Compute individual hash values for first,
+ // second and third and combine them using XOR
+ // and bit shifting:
+ return (hash<string>()(k.m_string) ^ (hash<int>()(k.m_number) << 1));
+ }
+};
+}; // namespace std
template<typename T>
class Mock3DData
@@ -78,6 +117,12 @@ public:
m_normal(normal)
{
}
+
+ /// Constructor
+ MockVertexData()
+ {
+ }
+
/// Destructor
virtual ~MockVertexData()
{
@@ -119,6 +164,12 @@ public:
m_id(id)
{
}
+
+ /// Constructor
+ MockEdgeData()
+ {
+ }
+
/// Destructor
virtual ~MockEdgeData()
{
@@ -154,6 +205,12 @@ public:
m_edges(edges)
{
}
+
+ /// Constructor
+ MockTriangleData()
+ {
+ }
+
/// Destructor
virtual ~MockTriangleData()
{
@@ -284,87 +341,10 @@ public:
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()
+ bool doUpdate() override
{
++m_numUpdates;
+ return true;
}
/// Number of updates performed on the mesh
@@ -460,15 +440,42 @@ public:
private:
/// Provides update functionality, which just increments the number of updates
- virtual void doUpdate()
+ bool doUpdate() override
{
++m_numUpdates;
+ return true;
}
/// Number of updates performed on the mesh
int m_numUpdates;
};
+template <typename T, size_t N>
+class MockGrid : public SurgSim::DataStructures::Grid<T, N>
+{
+public:
+ typedef typename SurgSim::DataStructures::Grid<T, N>::CellContent CellContent;
+ typedef typename SurgSim::DataStructures::Grid<T, N>::NDId NDId;
+ typedef typename SurgSim::DataStructures::Grid<T, N>::NDIdHash NDIdHash;
+
+ MockGrid(const Eigen::Matrix<double, N, 1>& cellSize, const Eigen::AlignedBox<double, N>& bounds) :
+ SurgSim::DataStructures::Grid<T,N>(cellSize, bounds)
+ {}
+
+ std::unordered_map<NDId, CellContent, NDIdHash>& getActiveCells() { return this->m_activeCells; }
+
+ std::unordered_map<T, NDId>& getCellIds() { return this->m_cellIds; }
+
+ std::vector<T>& getNonConstNeighbors(const T& element)
+ {
+ return const_cast<std::vector<T>&>(this->getNeighbors(element));
+ }
+
+ Eigen::Matrix<double, N, 1> getSize() const { return this->m_size; }
+
+ Eigen::AlignedBox<double, N> getAABB() const { return this->m_aabb; }
+};
+
namespace wrappers
{
template <typename IteratorType>
diff --git a/SurgSim/DataStructures/UnitTests/NamedDataTests.cpp b/SurgSim/DataStructures/UnitTests/NamedDataTests.cpp
index 4af8ea0..e56e61f 100644
--- a/SurgSim/DataStructures/UnitTests/NamedDataTests.cpp
+++ b/SurgSim/DataStructures/UnitTests/NamedDataTests.cpp
@@ -16,6 +16,8 @@
/// \file
/// Tests for the NamedData<T> class.
+#include <string>
+
#include "SurgSim/DataStructures/IndexDirectory.h"
#include "SurgSim/DataStructures/NamedData.h"
#include "SurgSim/DataStructures/NamedDataBuilder.h"
@@ -403,3 +405,35 @@ TEST(NamedDataTests, CacheIndex)
data.cacheIndex(name + "2", &index2);
EXPECT_EQ(-1, index2);
}
+
+TEST(NamedDataTests, SetRValue)
+{
+ NamedDataBuilder<std::string> builder;
+ builder.addEntry("test");
+ {
+ NamedData<std::string> data;
+ data = builder.createData();
+
+ std::string setValue = "value";
+ std::string getValue;
+ data.set("test", std::move(setValue));
+ EXPECT_EQ("", setValue);
+
+ data.get("test", &getValue);
+ EXPECT_EQ("value", getValue);
+ }
+ {
+ NamedData<std::string> data;
+ data = builder.createData();
+
+ std::string setValue = "value";
+ std::string getValue;
+ int index = data.getIndex("test");
+
+ data.set(index, std::move(setValue));
+ EXPECT_EQ("", setValue);
+
+ data.get(index, &getValue);
+ EXPECT_EQ("value", getValue);
+ }
+}
diff --git a/SurgSim/DataStructures/UnitTests/OctreeNodeTests.cpp b/SurgSim/DataStructures/UnitTests/OctreeNodeTests.cpp
index ad3ed0f..b89da68 100644
--- a/SurgSim/DataStructures/UnitTests/OctreeNodeTests.cpp
+++ b/SurgSim/DataStructures/UnitTests/OctreeNodeTests.cpp
@@ -39,6 +39,9 @@ struct MockData
typedef OctreeNode<MockData> OctreeNodeType;
+template<>
+std::string OctreeNodeType::m_className = "OctreeNode<MockData>";
+
namespace
{
const double epsilon = 1e-14;
@@ -163,8 +166,8 @@ TEST(OctreeNodeTests, AddNodes)
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->addData(Vector3d(1.0, 1.0, 1.0), levels, data));
+ EXPECT_TRUE(octree->addData(Vector3d(-4.0, 5.0, -7.0), levels, data));
EXPECT_TRUE(octree->hasChildren());
EXPECT_TRUE(octree->isActive());
@@ -192,7 +195,7 @@ TEST(OctreeNodeTests, Data)
EXPECT_FALSE(octree.hasChildren());
EXPECT_FALSE(octree.isActive());
- EXPECT_TRUE(octree.addData(Vector3d(1.0, 1.0, 1.0), expectedData, levels));
+ EXPECT_TRUE(octree.addData(Vector3d(1.0, 1.0, 1.0), levels, expectedData));
EXPECT_FALSE(octree.hasChildren());
EXPECT_TRUE(octree.isActive());
@@ -225,23 +228,32 @@ TEST(OctreeNodeTests, OctreePath)
EXPECT_EQ(previous, octree->getNode(path, true));
}
+struct Data1
+{
+ std::string name;
+};
+
+template <>
+std::string OctreeNode<Data1>::m_className = "OctreeNode<Data1>";
+
+struct Data2
+{
+ double value;
+};
+
+template <>
+std::string OctreeNode<Data2>::m_className = "OctreeNode<Data2>";
+
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);
+ octree1->addData(Vector3d(1.0, 1.0, 1.0), 1, dataRoot);
Data1 dataChild = {"child"};
- octree1->addData(Vector3d(0.5, 0.5, 0.5), dataChild, 2);
+ octree1->addData(Vector3d(0.5, 0.5, 0.5), 2, dataChild);
{
SCOPED_TRACE("Copying with different Data Type");
@@ -300,37 +312,11 @@ TEST(OctreeNodeTests, EmptyData)
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_NO_THROW(octree->load("Geometry/staple.ply", appData));
ASSERT_TRUE(nullptr != octree);
auto boundingBox = octree->getBoundingBox();
@@ -350,6 +336,9 @@ TEST(OctreeNodeTests, DoLoadOctree)
EXPECT_TRUE(octree->getChild(0)->getChild(2)->hasChildren());
EXPECT_TRUE(octree->getChild(0)->getChild(2)->getChild(2)->isActive());
+
+ Vector3d leafSize = octree->getChild(0)->getChild(0)->getChild(0)->getChild(0)->getBoundingBox().sizes();
+ EXPECT_TRUE(leafSize.isApprox(Vector3d(0.001, 0.001, 0.001)));
}
TEST(OctreeNodeTests, NeighborhoodTestSimple)
diff --git a/SurgSim/DataStructures/UnitTests/OptionalValueTests.cpp b/SurgSim/DataStructures/UnitTests/OptionalValueTests.cpp
index eabf34a..6a99990 100644
--- a/SurgSim/DataStructures/UnitTests/OptionalValueTests.cpp
+++ b/SurgSim/DataStructures/UnitTests/OptionalValueTests.cpp
@@ -161,6 +161,20 @@ void testOptionalValueSerialization(Type value)
// Verify
EXPECT_FALSE(newOptionalValue.hasValue());
}
+
+ // Test for scalar assignment, assigning an optional value from a node that contains
+ // only the value of the correct type should succeed and not throw
+ {
+ OptionalValue<Type> optionalValue;
+ YAML::Node node;
+ node = value;
+
+ EXPECT_NO_THROW(optionalValue = node.as<OptionalValue<Type>>());
+
+ // Verify
+ EXPECT_TRUE(optionalValue.hasValue());
+ EXPECT_EQ(value, *optionalValue);
+ }
}
TEST(OptionalValueTests, Serialization)
diff --git a/SurgSim/DataStructures/UnitTests/PlyReaderTests.cpp b/SurgSim/DataStructures/UnitTests/PlyReaderTests.cpp
index 3ae45d9..13b44ca 100644
--- a/SurgSim/DataStructures/UnitTests/PlyReaderTests.cpp
+++ b/SurgSim/DataStructures/UnitTests/PlyReaderTests.cpp
@@ -25,16 +25,16 @@ using SurgSim::Math::Vector3d;
namespace
{
-std::string findFile(std::string filename)
+std::string findFile(const std::string& filename)
{
std::vector<std::string> paths;
- paths.push_back("Data/PlyReaderTests");
+ paths.push_back("Data/");
SurgSim::Framework::ApplicationData data(paths);
return data.findFile(filename);
}
-typedef SurgSim::DataStructures::TriangleMesh MeshType;
+typedef SurgSim::DataStructures::TriangleMeshPlain MeshType;
}
@@ -50,9 +50,9 @@ class PlyReaderTests : public ::testing::Test
TEST_F(PlyReaderTests, InitTest)
{
ASSERT_NO_THROW(PlyReader("xxx"));
- ASSERT_NO_THROW(PlyReader(findFile("Cube.ply")));
+ ASSERT_NO_THROW(PlyReader(findFile("PlyReaderTests/Cube_with_physics.ply")));
- PlyReader reader(findFile("Cube.ply"));
+ PlyReader reader(findFile("PlyReaderTests/Cube_with_physics.ply"));
EXPECT_TRUE(reader.isValid());
PlyReader reader2(findFile("xxx"));
@@ -61,7 +61,7 @@ TEST_F(PlyReaderTests, InitTest)
TEST_F(PlyReaderTests, FindElementsAndProperties)
{
- PlyReader reader(findFile("Cube.ply"));
+ PlyReader reader(findFile("PlyReaderTests/Cube_with_physics.ply"));
EXPECT_TRUE(reader.hasElement("vertex"));
EXPECT_TRUE(reader.hasElement("face"));
@@ -69,6 +69,7 @@ TEST_F(PlyReaderTests, FindElementsAndProperties)
EXPECT_TRUE(reader.hasProperty("vertex", "x"));
EXPECT_TRUE(reader.hasProperty("vertex", "y"));
+ EXPECT_TRUE(reader.hasProperty("face", "nature"));
EXPECT_TRUE(reader.hasProperty("face", "vertex_indices"));
EXPECT_FALSE(reader.hasProperty("xxx", "vertex_indices"));
EXPECT_FALSE(reader.hasProperty("vertex", "vertex_indices"));
@@ -76,7 +77,7 @@ TEST_F(PlyReaderTests, FindElementsAndProperties)
TEST_F(PlyReaderTests, IsScalar)
{
- PlyReader reader(findFile("Testdata.ply"));
+ PlyReader reader(findFile("PlyReaderTests/Testdata.ply"));
EXPECT_TRUE(reader.isScalar("vertex", "x"));
EXPECT_FALSE(reader.isScalar("face", "vertex_indices"));
@@ -130,6 +131,7 @@ public:
}
faces.push_back(face);
extras.push_back(faceData.extra);
+ natures.push_back(faceData.nature);
}
@@ -146,6 +148,7 @@ public:
unsigned int faceCount;
unsigned int* faces;
int extra;
+ unsigned int nature;
int64_t overrun;
};
@@ -161,13 +164,40 @@ public:
std::vector<Vector3d> vertices;
std::vector<std::vector<unsigned int>> faces;
std::vector<int> extras;
-
+ std::vector<unsigned int> natures;
};
+TEST_F(PlyReaderTests, ElementTwoPropertyTest)
+{
+ TestData testData;
+ PlyReader reader(findFile("PlyReaderTests/Cube_with_physics.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.requestScalarProperty("face", "nature",
+ PlyReader::TYPE_UNSIGNED_INT,
+ offsetof(TestData::FaceData, nature)));
+
+ EXPECT_TRUE(reader.requestListProperty("face", "vertex_indices",
+ PlyReader::TYPE_UNSIGNED_INT,
+ offsetof(TestData::FaceData, faces),
+ PlyReader::TYPE_UNSIGNED_INT,
+ offsetof(TestData::FaceData, faceCount)));
+
+ ASSERT_NO_THROW(reader.parseFile());
+
+ EXPECT_EQ(11, testData.natures[0]);
+ EXPECT_EQ(0, testData.natures[8]);
+}
+
TEST_F(PlyReaderTests, ScalarReadTest)
{
TestData testData;
- PlyReader reader(findFile("Testdata.ply"));
+ PlyReader reader(findFile("PlyReaderTests/Testdata.ply"));
EXPECT_TRUE(reader.requestElement("vertex",
std::bind(&TestData::beginVertices, &testData,
std::placeholders::_1, std::placeholders::_2),
@@ -211,7 +241,7 @@ TEST_F(PlyReaderTests, ScalarReadTest)
TEST_F(PlyReaderTests, ListReadTest)
{
TestData testData;
- PlyReader reader(findFile("Testdata.ply"));
+ PlyReader reader(findFile("PlyReaderTests/Testdata.ply"));
EXPECT_TRUE(reader.requestElement("face",
std::bind(&TestData::beginFaces, &testData,
std::placeholders::_1, std::placeholders::_2),
@@ -250,7 +280,7 @@ TEST_F(PlyReaderTests, ListReadTest)
TEST_F(PlyReaderTests, TriangleMeshDelegateTest)
{
- PlyReader reader(findFile("Cube.ply"));
+ PlyReader reader(findFile("PlyReaderTests/Cube_with_physics.ply"));
auto delegate = std::make_shared<TriangleMeshPlyReaderDelegate<MeshType>>();
// parseWithDeletegate() will first call setDelegate(), then parseFile() if previous step successed.
@@ -275,4 +305,4 @@ TEST_F(PlyReaderTests, TriangleMeshDelegateTest)
}
} // DataStructures
-} // SurgSim
\ No newline at end of file
+} // SurgSim
diff --git a/SurgSim/DataStructures/UnitTests/SegmentMeshTest.cpp b/SurgSim/DataStructures/UnitTests/SegmentMeshTest.cpp
new file mode 100644
index 0000000..f673d93
--- /dev/null
+++ b/SurgSim/DataStructures/UnitTests/SegmentMeshTest.cpp
@@ -0,0 +1,617 @@
+// 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 <random>
+#include <boost/filesystem.hpp>
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/MeshElement.h"
+#include "SurgSim/DataStructures/SegmentMesh.h"
+#include "SurgSim/DataStructures/UnitTests/MockObjects.h"
+#include "SurgSim/DataStructures/Vertex.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/ApplicationData.h"
+
+
+using SurgSim::DataStructures::EmptyData;
+using SurgSim::DataStructures::SegmentMesh;
+using SurgSim::DataStructures::SegmentMeshPlain;
+using SurgSim::Math::Vector3d;
+
+
+/// Triangle Mesh for testing using MockVertexData, MockEdgeData, and MockTriangleData
+class MockSegmentMesh : public SurgSim::DataStructures::SegmentMesh<MockVertexData, MockEdgeData>
+{
+public:
+ /// Vertex type for convenience
+ typedef SegmentMesh<MockVertexData, MockEdgeData>::VertexType VertexType;
+ /// Edge type for convenience
+ typedef SegmentMesh<MockVertexData, MockEdgeData>::EdgeType EdgeType;
+ /// Triangle type for convenience
+ typedef SegmentMesh<MockVertexData, MockEdgeData>::TriangleType TriangleType;
+
+ /// Constructor. Start out with no vertices and 0 updates
+ MockSegmentMesh() :
+ SurgSim::DataStructures::SegmentMesh<MockVertexData, MockEdgeData>(),
+ m_numUpdates(0)
+ {
+ }
+
+ SURGSIM_CLASSNAME(MockSegmentMesh);
+
+ /// Destructor
+ virtual ~MockSegmentMesh()
+ {
+ }
+
+ /// 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);
+ }
+
+ /// 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
+ bool doUpdate() override
+ {
+ ++m_numUpdates;
+ return true;
+ }
+
+ /// Number of updates performed on the mesh
+ int m_numUpdates;
+};
+
+template<>
+std::string SurgSim::DataStructures::SegmentMesh<MockVertexData, MockEdgeData>
+::TriangleMesh::m_className = "MockSegmentMesh";
+
+class SegmentMeshTest : 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 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);
+ 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 (printEdges)
+ {
+ std::cout << "Test Edges:\n";
+ }
+
+ for (size_t i = 0; i < numVertices - 1; ++i)
+ {
+ std::array<size_t, 2> edgeVertices = {{ i, i + 1 }};
+ testEdgeVertices.push_back(edgeVertices);
+ if (printEdges)
+ {
+ std::cout << "\t" << i << ": (" << formatIterator(edgeVertices, ", ") << ")\n";
+ }
+ }
+ }
+
+ void TearDown()
+ {
+ boost::filesystem::remove("export.ply");
+ }
+
+ /// 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;
+};
+
+
+TEST_F(SegmentMeshTest, InitTest)
+{
+ ASSERT_NO_THROW({MockSegmentMesh mesh;});
+ ASSERT_NO_THROW({SegmentMeshPlain mesh;});
+}
+
+TEST_F(SegmentMeshTest, ClassNameTest)
+{
+ MockSegmentMesh mesh;
+ EXPECT_EQ("MockSegmentMesh", mesh.getClassName());
+ SegmentMeshPlain meshPlain;
+ EXPECT_EQ("SurgSim::DataStructures::SegmentMeshPlain", meshPlain.getClassName());
+}
+
+TEST_F(SegmentMeshTest, CreateVerticesTest)
+{
+ MockSegmentMesh 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<MockSegmentMesh::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<MockSegmentMesh::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());
+ }
+ }
+}
+
+TEST_F(SegmentMeshTest, isValidTest)
+{
+ MockSegmentMesh 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 vertices
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ mesh.createVertex(testPositions[i], testNormals[i]);
+ }
+
+ EXPECT_TRUE(mesh.isValid());
+}
+
+TEST_F(SegmentMeshTest, SetVertexPositionsTest)
+{
+ MockSegmentMesh 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_THROW(mesh.setVertexPositions(testPositions), SurgSim::Framework::AssertionFailure);
+}
+
+TEST_F(SegmentMeshTest, ClearTest)
+{
+ MockSegmentMesh 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());
+
+ /// 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());
+ }
+
+ 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());
+
+ /// 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());
+}
+
+TEST_F(SegmentMeshTest, UpdateTest)
+{
+ MockSegmentMesh mesh;
+
+ EXPECT_EQ(0, mesh.getNumUpdates());
+
+ for (int i = 0; i < 10; ++i)
+ {
+ mesh.update();
+ EXPECT_EQ(i + 1, mesh.getNumUpdates());
+ }
+}
+
+TEST_F(SegmentMeshTest, ComparisonTest)
+{
+ /// Create mesh using test data
+ MockSegmentMesh 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]));
+ }
+
+ /// Create same mesh again
+ MockSegmentMesh 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]));
+ }
+
+ /// Create mesh with test data, but each vertex has position and normal of (0,0,0) to make them different
+ MockSegmentMesh 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]));
+ }
+
+ /// Create mesh with test data, but reverse each edge's vertex order to make them different
+ MockSegmentMesh 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));
+ }
+
+ /// 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);
+}
+
+
+TEST_F(SegmentMeshTest, CopyConstructorTest)
+{
+ MockSegmentMesh 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());
+ }
+
+ SurgSim::DataStructures::SegmentMeshPlain 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);
+ }
+}
+
+TEST_F(SegmentMeshTest, GetEdgePositions)
+{
+ MockSegmentMesh mesh;
+
+ // Initialization
+ auto normal = testNormals.begin();
+ for (auto position = testPositions.begin(); position != testPositions.end(); ++position)
+ {
+ mesh.createVertex(*position, *normal++);
+ }
+
+ for (auto vertices = testEdgeVertices.begin(); vertices != testEdgeVertices.end(); ++vertices)
+ {
+ mesh.createEdge(*vertices);
+ }
+
+ // Testing
+ for (size_t id = 0; id < mesh.getEdges().size(); ++id)
+ {
+ auto verticesPositions = mesh.getEdgePositions(id);
+
+ auto& vertexIds = mesh.getEdge(id).verticesId;
+
+ EXPECT_TRUE(mesh.getVertex(vertexIds[0]).position.isApprox(verticesPositions[0]));
+ EXPECT_TRUE(mesh.getVertex(vertexIds[1]).position.isApprox(verticesPositions[1]));
+ }
+}
+
+
+TEST_F(SegmentMeshTest, AssertingFunctions)
+{
+ MockSegmentMesh mesh;
+
+ // Initialization
+ auto normal = testNormals.begin();
+ for (auto position = testPositions.begin(); position != testPositions.end(); ++position)
+ {
+ mesh.createVertex(*position, *normal++);
+ }
+
+ for (auto vertices = testEdgeVertices.begin(); vertices != testEdgeVertices.end(); ++vertices)
+ {
+ mesh.createEdge(*vertices);
+ }
+
+ // Testing
+ std::array<size_t, 3> triangleIds = {{0, 1, 2}};
+ MockSegmentMesh::TriangleType triangle(triangleIds);
+ EXPECT_THROW(mesh.addTriangle(triangle), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(mesh.getNumTriangles(), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(mesh.getTriangles(), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(mesh.getTriangle(0), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(mesh.removeTriangle(0), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(mesh.getTrianglePositions(0), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(mesh.doClearTriangles(), SurgSim::Framework::AssertionFailure);
+}
+
+TEST_F(SegmentMeshTest, CreateDefaultedges)
+{
+ MockSegmentMesh mesh;
+ for (size_t i = 0; i < 10; ++i)
+ {
+ mesh.createVertex(Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0));
+ }
+
+ mesh.createDefaultEdges();
+ EXPECT_EQ(9, mesh.getNumEdges());
+ for (size_t i = 0; i < mesh.getNumEdges(); ++i)
+ {
+ EXPECT_EQ(i, mesh.getEdge(i).verticesId[0]);
+ EXPECT_EQ(i + 1, mesh.getEdge(i).verticesId[1]);
+ }
+}
+
+TEST_F(SegmentMeshTest, LoadMesh)
+{
+ auto mesh = std::make_shared<SegmentMeshPlain>();
+ SurgSim::Framework::ApplicationData data("config.txt");
+
+ mesh->load("SegmentMeshTest/segmentmesh.ply", data);
+
+ ASSERT_EQ(4u, mesh->getNumVertices());
+ ASSERT_EQ(3u, mesh->getNumEdges());
+
+ auto edge = mesh->getEdge(0);
+ ASSERT_EQ(0u, edge.verticesId[0]);
+ ASSERT_EQ(1u, edge.verticesId[1]);
+
+ edge = mesh->getEdge(1);
+ ASSERT_EQ(1u, edge.verticesId[0]);
+ ASSERT_EQ(2u, edge.verticesId[1]);
+
+ edge = mesh->getEdge(2);
+ ASSERT_EQ(2u, edge.verticesId[0]);
+ ASSERT_EQ(3u, edge.verticesId[1]);
+}
+
+
+
+TEST_F(SegmentMeshTest, WriteMesh)
+{
+ auto mesh = std::make_shared<SegmentMeshPlain>();
+ SurgSim::Framework::ApplicationData data("config.txt");
+
+ mesh->load("SegmentMeshTest/segmentmesh.ply", data);
+
+ EXPECT_NO_THROW(mesh->save("export.ply"));
+
+ std::vector<std::string> paths(1, ".");
+ SurgSim::Framework::ApplicationData localPath(paths);
+
+ auto loaded = std::make_shared<SegmentMeshPlain>();
+ EXPECT_NO_THROW(loaded->load("export.ply", localPath));
+
+ ASSERT_EQ(4u, mesh->getNumVertices());
+ ASSERT_EQ(3u, mesh->getNumEdges());
+
+ auto edge = mesh->getEdge(0);
+ ASSERT_EQ(0u, edge.verticesId[0]);
+ ASSERT_EQ(1u, edge.verticesId[1]);
+
+ edge = mesh->getEdge(1);
+ ASSERT_EQ(1u, edge.verticesId[0]);
+ ASSERT_EQ(2u, edge.verticesId[1]);
+
+ edge = mesh->getEdge(2);
+ ASSERT_EQ(2u, edge.verticesId[0]);
+ ASSERT_EQ(3u, edge.verticesId[1]);
+}
diff --git a/SurgSim/DataStructures/UnitTests/TetrahedronMeshTest.cpp b/SurgSim/DataStructures/UnitTests/TetrahedronMeshTest.cpp
index 34e753b..95b564c 100644
--- a/SurgSim/DataStructures/UnitTests/TetrahedronMeshTest.cpp
+++ b/SurgSim/DataStructures/UnitTests/TetrahedronMeshTest.cpp
@@ -282,7 +282,7 @@ TEST_F(TetrahedronMeshTest, CreateVerticesTest)
EXPECT_EQ(i, mesh.createVertex(testPositions[i], testNormals[i]));
EXPECT_EQ(i + 1, mesh.getNumVertices());
- const std::vector<MockTriangleMeshBase::VertexType>& vertices = mesh.getVertices();
+ const std::vector<MockTetrahedronMesh::VertexType>& vertices = mesh.getVertices();
EXPECT_EQ(i + 1, vertices.size());
/// Make sure each vertex is set properly
@@ -302,7 +302,7 @@ TEST_F(TetrahedronMeshTest, CreateVerticesTest)
EXPECT_EQ(i, mesh.createEdge(testEdgesVerticesId[i]));
EXPECT_EQ(i + 1, mesh.getNumEdges());
- const std::vector<MockTriangleMeshBase::EdgeType>& edges = mesh.getEdges();
+ const std::vector<MockTetrahedronMesh::EdgeType>& edges = mesh.getEdges();
EXPECT_EQ(i + 1, edges.size());
/// Make sure each vertex is set properly
@@ -321,7 +321,7 @@ TEST_F(TetrahedronMeshTest, CreateVerticesTest)
EXPECT_EQ(i, mesh.createTriangle(testTrianglesVerticesId[i], testTrianglesEdgesId[i]));
EXPECT_EQ(i + 1, mesh.getNumTriangles());
- const std::vector<MockTriangleMeshBase::TriangleType>& triangles = mesh.getTriangles();
+ const std::vector<MockTetrahedronMesh::TriangleType>& triangles = mesh.getTriangles();
EXPECT_EQ(i + 1, triangles.size());
/// Make sure each vertex is set properly
@@ -414,7 +414,7 @@ TEST_F(TetrahedronMeshTest, SetVertexPositionsTest)
EXPECT_EQ(1, mesh.getNumUpdates());
EXPECT_EQ(testPositions.size(), mesh.getNumVertices());
- const std::vector<MockMesh::VertexType>& vertices = mesh.getVertices();
+ const std::vector<MockTetrahedronMesh::VertexType>& vertices = mesh.getVertices();
EXPECT_EQ(testPositions.size(), vertices.size());
/// Make sure each vertex is set properly
diff --git a/SurgSim/DataStructures/UnitTests/TriangleMeshBaseTest.cpp b/SurgSim/DataStructures/UnitTests/TriangleMeshBaseTest.cpp
deleted file mode 100644
index bb34586..0000000
--- a/SurgSim/DataStructures/UnitTests/TriangleMeshBaseTest.cpp
+++ /dev/null
@@ -1,655 +0,0 @@
-// 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
index 3c1137c..6296027 100644
--- a/SurgSim/DataStructures/UnitTests/TriangleMeshTest.cpp
+++ b/SurgSim/DataStructures/UnitTests/TriangleMeshTest.cpp
@@ -13,129 +13,793 @@
// 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 <random>
+#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 "SurgSim/Framework/Runtime.h"
#include "SurgSim/Framework/ApplicationData.h"
-#include "SurgSim/Math/RigidTransform.h"
-#include "SurgSim/Math/Vector.h"
-#include <random>
-using SurgSim::Math::RigidTransform3d;
+using SurgSim::DataStructures::EmptyData;
+using SurgSim::DataStructures::TriangleMesh;
+using SurgSim::DataStructures::TriangleMeshPlain;
using SurgSim::Math::Vector3d;
-namespace SurgSim
+
+/// Triangle Mesh for testing using MockVertexData, MockEdgeData, and MockTriangleData
+class MockTriangleMesh : public SurgSim::DataStructures::TriangleMesh<MockVertexData, MockEdgeData, MockTriangleData>
+{
+public:
+ /// Vertex type for convenience
+ typedef TriangleMesh<MockVertexData, MockEdgeData, MockTriangleData>::VertexType VertexType;
+ /// Edge type for convenience
+ typedef TriangleMesh<MockVertexData, MockEdgeData, MockTriangleData>::EdgeType EdgeType;
+ /// Triangle type for convenience
+ typedef TriangleMesh<MockVertexData, MockEdgeData, MockTriangleData>::TriangleType TriangleType;
+
+ /// Constructor. Start out with no vertices and 0 updates
+ MockTriangleMesh() :
+ SurgSim::DataStructures::TriangleMesh<MockVertexData, MockEdgeData, MockTriangleData>(),
+ m_numUpdates(0)
+ {
+ }
+
+ SURGSIM_CLASSNAME(MockTriangleMesh);
+
+ /// Destructor
+ virtual ~MockTriangleMesh()
+ {
+ }
+
+ /// 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
+ bool doUpdate() override
+ {
+ ++m_numUpdates;
+ return true;
+ }
+
+ /// Number of updates performed on the mesh
+ int m_numUpdates;
+};
+
+template<>
+std::string SurgSim::DataStructures::TriangleMesh<MockVertexData, MockEdgeData, MockTriangleData>
+::m_className = "MockTriangleMesh";
+
+class TriangleMeshTest : 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(TriangleMeshTest, InitTest)
{
-namespace DataStructures
+ ASSERT_NO_THROW({MockTriangleMesh mesh;});
+ ASSERT_NO_THROW({TriangleMeshPlain mesh;});
+}
+
+TEST_F(TriangleMeshTest, CreateVerticesTest)
+{
+ MockTriangleMesh 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<MockTriangleMesh::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<MockTriangleMesh::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<MockTriangleMesh::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(TriangleMeshTest, isValidTest)
+{
+ MockTriangleMesh 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(TriangleMeshTest, SetVertexPositionsTest)
{
+ MockTriangleMesh 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);
-TEST(TriangleMeshTest, NormalTest)
+ 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_THROW(mesh.setVertexPositions(testPositions), SurgSim::Framework::AssertionFailure);
+}
+
+TEST_F(TriangleMeshTest, ClearTest)
{
- auto mesh = std::make_shared<TriangleMeshPlain>();
+ MockTriangleMesh 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());
- // 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));
+ EXPECT_EQ(0u, mesh.getNumTriangles());
+ EXPECT_EQ(0u, mesh.getTriangles().size());
- mesh->addVertex(v0);
- mesh->addVertex(v1);
- mesh->addVertex(v2);
+ /// 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());
+ }
- // Add edges
- std::array<size_t, 2> edgePoints01;
- std::array<size_t, 2> edgePoints02;
- std::array<size_t, 2> edgePoints12;
+ EXPECT_EQ(testPositions.size(), mesh.getNumVertices());
+ EXPECT_EQ(testPositions.size(), mesh.getVertices().size());
- TriangleMeshPlain::EdgeType e01(edgePoints01);
- TriangleMeshPlain::EdgeType e02(edgePoints02);
- TriangleMeshPlain::EdgeType e12(edgePoints12);
+ EXPECT_EQ(testEdgeVertices.size(), mesh.getNumEdges());
+ EXPECT_EQ(testEdgeVertices.size(), mesh.getEdges().size());
- mesh->addEdge(e01);
- mesh->addEdge(e02);
- mesh->addEdge(e12);
+ EXPECT_EQ(testTriangleVertices.size(), mesh.getNumTriangles());
+ EXPECT_EQ(testTriangleVertices.size(), mesh.getTriangles().size());
- // Add triangle
- std::array<size_t, 3> trianglePoints = {0, 1, 2};
+ /// Clear mesh
+ mesh.clear();
- TriangleMeshPlain::TriangleType t(trianglePoints);
- mesh->addTriangle(t);
+ EXPECT_EQ(0u, mesh.getNumVertices());
+ EXPECT_EQ(0u, mesh.getVertices().size());
- std::shared_ptr<TriangleMesh> meshWithNormal = std::make_shared<TriangleMesh>(*mesh);
+ EXPECT_EQ(0u, mesh.getNumEdges());
+ EXPECT_EQ(0u, mesh.getEdges().size());
+
+ EXPECT_EQ(0u, mesh.getNumTriangles());
+ EXPECT_EQ(0u, mesh.getTriangles().size());
+}
- Vector3d expectedZNormal(0.0, 0.0, 1.0);
- EXPECT_EQ(expectedZNormal, meshWithNormal->getNormal(0));
+TEST_F(TriangleMeshTest, UpdateTest)
+{
+ MockTriangleMesh mesh;
- // 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;
+ EXPECT_EQ(0, mesh.getNumUpdates());
- // Recompute normals for meshWithNormal
- meshWithNormal->calculateNormals();
- Vector3d expectedXNormal(0.0, -1.0, 0.0);
- EXPECT_EQ(expectedXNormal, meshWithNormal->getNormal(0));
+ for (int i = 0; i < 10; ++i)
+ {
+ mesh.update();
+ EXPECT_EQ(i + 1, mesh.getNumUpdates());
+ }
}
-TEST(TriangleMeshTest, CopyWithTransformTest)
+TEST_F(TriangleMeshTest, ComparisonTest)
{
- 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);
+ /// Create mesh using test data
+ MockTriangleMesh 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
+ MockTriangleMesh 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
+ MockTriangleMesh 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]));
+ }
- 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));
+ /// Create mesh with test data, but reverse each edge's vertex order to make them different
+ MockTriangleMesh 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]));
+ }
- for (auto it = expectedMesh->getVertices().begin(); it != expectedMesh->getVertices().end(); ++it)
+ /// Create mesh with test data, but only create half of the triangles to make the list different.
+ MockTriangleMesh 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)
{
- it->position = transform * it->position;
+ 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);
+}
+
- for (auto it = expectedMesh->getTriangles().begin(); it != expectedMesh->getTriangles().end(); ++it)
+TEST_F(TriangleMeshTest, CopyConstructorTest)
+{
+ MockTriangleMesh 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)
{
- it->data.normal = transform.linear() * it->data.normal;
+ 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());
}
- actualMesh->copyWithTransform(transform, *originalMesh);
+ SurgSim::DataStructures::TriangleMeshPlain mesh2(mesh);
- EXPECT_EQ(expectedMesh->getVertices(), actualMesh->getVertices());
- EXPECT_EQ(expectedMesh->getTriangles(), actualMesh->getTriangles());
+ 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(TriangleMeshTest, DoLoadTest)
+TEST_F(TriangleMeshTest, LoadTest)
{
- SurgSim::Framework::ApplicationData appData("config.txt");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("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));
+ auto mesh = std::make_shared<SurgSim::DataStructures::TriangleMeshPlain>();
+ EXPECT_THROW(mesh->load("Nonexistent file"), SurgSim::Framework::AssertionFailure);
}
{
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));
+ auto mesh = std::make_shared<SurgSim::DataStructures::TriangleMeshPlain>();
+ EXPECT_THROW(mesh->load("Geometry/InvalidMesh.ply"), SurgSim::Framework::AssertionFailure);
}
{
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));
+ auto mesh = std::make_shared<SurgSim::DataStructures::TriangleMeshPlain>();
+ EXPECT_NO_THROW(mesh->load("Geometry/staple_collision.ply"));
+ }
+
+ {
+ SCOPED_TRACE("Load Cube.ply and check mesh");
+ auto mesh = std::make_shared<SurgSim::DataStructures::TriangleMeshPlain>();
+ mesh->load("Geometry/Cube.ply");
+ ASSERT_NO_THROW(mesh->load("Geometry/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(TriangleMeshTest, GetTrianglePositions)
+{
+ MockTriangleMesh 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(TriangleMeshTest, GetEdgePositions)
+{
+ MockTriangleMesh mesh;
+
+ // Initialization
+ auto normal = testNormals.begin();
+ for (auto position = testPositions.begin(); position != testPositions.end(); ++position)
+ {
+ mesh.createVertex(*position, *normal++);
+ }
+
+ for (auto vertices = testEdgeVertices.begin(); vertices != testEdgeVertices.end(); ++vertices)
+ {
+ mesh.createEdge(*vertices);
+ }
+
+ // Testing
+ for (size_t id = 0; id < mesh.getEdges().size(); ++id)
+ {
+ auto verticesPositions = mesh.getEdgePositions(id);
+
+ auto& vertexIds = mesh.getEdge(id).verticesId;
+
+ EXPECT_TRUE(mesh.getVertex(vertexIds[0]).position.isApprox(verticesPositions[0]));
+ EXPECT_TRUE(mesh.getVertex(vertexIds[1]).position.isApprox(verticesPositions[1]));
+ }
+}
+
+TEST_F(TriangleMeshTest, TriangleDeletionTest)
+{
+ typedef TriangleMeshPlain::VertexType VertexType;
+ typedef TriangleMeshPlain::TriangleType TriangleType;
+
+ TriangleMeshPlain mesh;
+
+ mesh.addVertex(VertexType(testPositions[0]));
+ mesh.addVertex(VertexType(testPositions[0]));
+ mesh.addVertex(VertexType(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_THROW(mesh.getTriangle(1), SurgSim::Framework::AssertionFailure);
+ EXPECT_EQ(3u, mesh.getTriangles().size());
+
+ // Should be able to remove the same triangle twice
+ EXPECT_NO_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)));
+
+}
+
+TEST_F(TriangleMeshTest, Save)
+{
+ std::vector<std::string> paths;
+ paths.push_back(".");
+ SurgSim::Framework::ApplicationData data(paths);
+ /// Create mesh using test data
+ TriangleMeshPlain mesh;
+ for (size_t i = 0; i < testPositions.size(); ++i)
+ {
+ mesh.addVertex(TriangleMeshPlain::VertexType(testPositions[i]));
+ }
+ for (size_t i = 0; i < testTriangleVertices.size(); ++i)
+ {
+ mesh.addTriangle(TriangleMeshPlain::TriangleType(testTriangleVertices[i]));
+ }
+
+ ASSERT_NO_THROW(mesh.save("test.ply"));
+
+ auto loadedMesh = std::make_shared<TriangleMeshPlain>();
+
+ ASSERT_NO_THROW(loadedMesh->load("test.ply", data));
+
+ EXPECT_EQ(testPositions.size(), loadedMesh->getNumVertices());
+ EXPECT_EQ(testTriangleVertices.size(), loadedMesh->getNumTriangles());
+}
diff --git a/SurgSim/DataStructures/UnitTests/config.txt.in b/SurgSim/DataStructures/UnitTests/config.txt.in
index 507c4e7..aad44f8 100644
--- a/SurgSim/DataStructures/UnitTests/config.txt.in
+++ b/SurgSim/DataStructures/UnitTests/config.txt.in
@@ -1,2 +1,2 @@
${CMAKE_CURRENT_BINARY_DIR}/Data/
-${PROJECT_BINARY_DIR}/SurgSim/Testing/Data
\ No newline at end of file
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data/
diff --git a/SurgSim/DataStructures/Vertex.h b/SurgSim/DataStructures/Vertex.h
index 423a55e..1bc12dc 100644
--- a/SurgSim/DataStructures/Vertex.h
+++ b/SurgSim/DataStructures/Vertex.h
@@ -44,14 +44,40 @@ template <class Data>
struct Vertex
{
/// Constructor
+ 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()) :
+ explicit Vertex(const SurgSim::Math::Vector3d& position, const Data& data = Data()) :
position(position),
data(data)
{
}
+ /// 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 Vertex
+ /// \param other the Vertex to copy from
+ template <class T>
+ explicit Vertex(const Vertex<T>& other) :
+ position(other.position)
+ {
+ }
+
+ /// Assignment 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 Vertex
+ /// \param other the Vertex to copy from
+ template <class T>
+ Vertex<Data>& operator=(const Vertex<T>& other)
+ {
+ position = other.position;
+ return *this;
+ }
+
/// Position of the vertex.
SurgSim::Math::Vector3d position;
/// Extra vertex data.
diff --git a/SurgSim/DataStructures/Vertices-inl.h b/SurgSim/DataStructures/Vertices-inl.h
new file mode 100644
index 0000000..5d6baef
--- /dev/null
+++ b/SurgSim/DataStructures/Vertices-inl.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_INL_H
+#define SURGSIM_DATASTRUCTURES_VERTICES_INL_H
+
+#include <typeinfo>
+
+#include "SurgSim/Framework/Assert.h"
+
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+
+template <class VertexData>
+Vertices<VertexData>::Vertices()
+{
+}
+
+template <class VertexData>
+template <class V>
+Vertices<VertexData>::Vertices(const Vertices<V>& other)
+{
+ m_vertices.reserve(other.getVertices().size());
+ for (auto& otherVertex : other.getVertices())
+ {
+ addVertex(VertexType(otherVertex));
+ }
+}
+
+template <class VertexData>
+template <class V>
+Vertices<VertexData>& Vertices<VertexData>::operator=(const Vertices<V>& other)
+{
+ auto& otherVertices = other.getVertices();
+
+ if (otherVertices.size() < m_vertices.size())
+ {
+ m_vertices.resize(otherVertices.size());
+ }
+ else
+ {
+ m_vertices.reserve(otherVertices.size());
+ }
+
+ auto vertex = m_vertices.begin();
+ auto otherVertex = otherVertices.begin();
+ for (; vertex != m_vertices.end(); ++vertex, ++otherVertex)
+ {
+ *vertex = *otherVertex;
+ }
+ for (; otherVertex != otherVertices.end(); ++otherVertex)
+ {
+ addVertex(VertexType(*otherVertex));
+ }
+
+ return *this;
+}
+
+template <class VertexData>
+Vertices<VertexData>::~Vertices()
+{
+}
+
+template <class VertexData>
+void Vertices<VertexData>::clear()
+{
+ doClear();
+}
+
+template <class VertexData>
+bool Vertices<VertexData>::update()
+{
+ return doUpdate();
+}
+
+template <class VertexData>
+size_t Vertices<VertexData>::addVertex(const VertexType& vertex)
+{
+ m_vertices.push_back(vertex);
+ return m_vertices.size() - 1;
+}
+
+template <class VertexData>
+size_t Vertices<VertexData>::getNumVertices() const
+{
+ return m_vertices.size();
+}
+
+template <class VertexData>
+const typename Vertices<VertexData>::VertexType& Vertices<VertexData>::getVertex(size_t id) const
+{
+ return m_vertices[id];
+}
+
+template <class VertexData>
+typename Vertices<VertexData>::VertexType& Vertices<VertexData>::getVertex(size_t id)
+{
+ return m_vertices[id];
+}
+
+template <class VertexData>
+const std::vector<typename Vertices<VertexData>::VertexType>& Vertices<VertexData>::getVertices() const
+{
+ return m_vertices;
+}
+
+template <class VertexData>
+std::vector<typename Vertices<VertexData>::VertexType>& Vertices<VertexData>::getVertices()
+{
+ return m_vertices;
+}
+
+template <class VertexData>
+void Vertices<VertexData>::setVertexPosition(size_t id, const SurgSim::Math::Vector3d& position)
+{
+ m_vertices[id].position = position;
+}
+
+template <class VertexData>
+const SurgSim::Math::Vector3d& Vertices<VertexData>::getVertexPosition(size_t id) const
+{
+ return m_vertices[id].position;
+}
+
+template <class VertexData>
+void Vertices<VertexData>::setVertexPositions(const std::vector<SurgSim::Math::Vector3d>& positions, bool doUpdate)
+{
+ 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();
+ }
+}
+
+template <class VertexData>
+void Vertices<VertexData>::transform(const Math::RigidTransform3d& pose)
+{
+ for (auto& vertex : m_vertices)
+ {
+ vertex.position = pose * vertex.position;
+ }
+}
+
+template <class VertexData>
+bool Vertices<VertexData>::operator==(const Vertices& mesh) const
+{
+ return (typeid(*this) == typeid(mesh)) && isEqual(mesh);
+}
+
+template <class VertexData>
+bool Vertices<VertexData>::operator!=(const Vertices& mesh) const
+{
+ return (typeid(*this) != typeid(mesh)) || ! isEqual(mesh);
+}
+
+template <class VertexData>
+void Vertices<VertexData>::doClearVertices()
+{
+ m_vertices.clear();
+}
+
+template <class VertexData>
+bool Vertices<VertexData>::isEqual(const Vertices& mesh) const
+{
+ return m_vertices == mesh.m_vertices;
+}
+
+template <class VertexData>
+void Vertices<VertexData>::doClear()
+{
+ doClearVertices();
+}
+
+template <class VertexData>
+bool Vertices<VertexData>::doUpdate()
+{
+ return true;
+}
+
+};
+};
+
+#endif //SURGSIM_DATASTRUCTURES_VERTICES_INL_H
+
diff --git a/SurgSim/DataStructures/Vertices.h b/SurgSim/DataStructures/Vertices.h
index f52ba4a..95730ac 100644
--- a/SurgSim/DataStructures/Vertices.h
+++ b/SurgSim/DataStructures/Vertices.h
@@ -16,12 +16,13 @@
#ifndef SURGSIM_DATASTRUCTURES_VERTICES_H
#define SURGSIM_DATASTRUCTURES_VERTICES_H
+#include <vector>
+
+#include "SurgSim/DataStructures/EmptyData.h"
#include "SurgSim/DataStructures/Vertex.h"
-#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
-#include <array>
-#include <typeinfo>
-#include <vector>
namespace SurgSim
{
@@ -53,27 +54,33 @@ public:
/// Vertex type for convenience
typedef Vertex<VertexData> VertexType;
- /// Constructor. The mesh is initially empty (no vertices).
- Vertices()
- {
- }
+ /// Constructor
+ Vertices();
+
+ /// Copy constructor when the template data is a different type
+ /// In this case, no data will be copied
+ /// \tparam V type of data stored in the other Vertices
+ /// \param other the Vertices to copy from
+ template <class V>
+ explicit Vertices(const Vertices<V>& other);
+
+ /// Assignment when the template data is a different type
+ /// In this case, no data will be copied
+ /// \tparam V type of data stored in the other Vertices
+ /// \param other the Vertices to copy from
+ template <class V>
+ Vertices<VertexData>& operator=(const Vertices<V>& other);
+
/// Destructor
- virtual ~Vertices()
- {
- }
+ virtual ~Vertices();
/// Clear mesh to return to an empty state (no vertices).
- void clear()
- {
- doClear();
- }
+ void clear();
/// Performs any updates that are required when the vertices are modified.
/// Calls doUpdate() to perform the updates.
- void update()
- {
- doUpdate();
- }
+ /// \return true on success.
+ bool update();
/// Adds a vertex to the mesh.
/// Recommend that subclasses with a specific purpose (such as for use in collision detection), have a
@@ -81,125 +88,81 @@ public:
/// 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;
- }
+ size_t addVertex(const VertexType& vertex);
/// Returns the number of vertices in this mesh.
- size_t getNumVertices() const
- {
- return m_vertices.size();
- }
+ size_t getNumVertices() const;
/// Returns the specified vertex.
- const VertexType& getVertex(size_t id) const
- {
- return m_vertices[id];
- }
+ const VertexType& getVertex(size_t id) const;
+
/// Returns the specified vertex (non const version).
- VertexType& getVertex(size_t id)
- {
- return m_vertices[id];
- }
+ VertexType& getVertex(size_t id);
/// Returns a vector containing the position of each vertex.
- const std::vector<VertexType>& getVertices() const
- {
- return m_vertices;
- }
+ const std::vector<VertexType>& getVertices() const;
+
/// Returns a vector containing the position of each vertex (non const version).
- std::vector<VertexType>& getVertices()
- {
- return m_vertices;
- }
+ std::vector<VertexType>& getVertices();
/// 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;
- }
+ void setVertexPosition(size_t id, const SurgSim::Math::Vector3d& 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;
- }
+ const SurgSim::Math::Vector3d& getVertexPosition(size_t id) const;
/// 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];
- }
+ void setVertexPositions(const std::vector<SurgSim::Math::Vector3d>& positions, bool doUpdate = true);
- if (doUpdate)
- {
- update();
- }
- }
+ /// Apply a rigid transform to each vertex
+ /// \param pose the rigid transform to apply
+ void transform(const Math::RigidTransform3d& pose);
/// 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);
- }
+ bool operator==(const Vertices& mesh) const;
/// 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);
- }
+ bool operator!=(const Vertices& mesh) const;
protected:
/// Remove all vertices from the mesh.
- virtual void doClearVertices()
- {
- m_vertices.clear();
- }
+ virtual void doClearVertices();
/// 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;
- }
+ virtual bool isEqual(const Vertices& mesh) const;
private:
/// Clear mesh to return to an empty state (no vertices).
- virtual void doClear()
- {
- doClearVertices();
- }
+ virtual void doClear();
/// 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()
- {
- }
+ /// \return true on success.
+ virtual bool doUpdate();
/// Vertices
std::vector<VertexType> m_vertices;
};
-}; // namespace DataStructures
+typedef Vertices<EmptyData> VerticesPlain;
+}; // namespace DataStructures
}; // namespace SurgSim
+#include "SurgSim/DataStructures/Vertices-inl.h"
+
#endif // SURGSIM_DATASTRUCTURES_VERTICES_H
diff --git a/SurgSim/DataStructures/ply.c b/SurgSim/DataStructures/ply.c
index 273bc9c..2ea0226 100644
--- a/SurgSim/DataStructures/ply.c
+++ b/SurgSim/DataStructures/ply.c
@@ -740,6 +740,7 @@ PlyFile *ply_read(FILE *fp, int *nelems, char ***elem_names)
if (!words || !equal_strings (words[0], "ply"))
{
if (words) free(words);
+ if (plyfile) free(plyfile);
return (NULL);
}
@@ -749,7 +750,11 @@ PlyFile *ply_read(FILE *fp, int *nelems, char ***elem_names)
if (equal_strings (words[0], "format")) {
if (nwords != 3)
+ {
+ if (words) free(words);
+ if (plyfile) free(plyfile);
return (NULL);
+ }
if (equal_strings (words[1], "ascii"))
plyfile->file_type = PLY_ASCII;
else if (equal_strings (words[1], "binary_big_endian"))
@@ -758,7 +763,8 @@ PlyFile *ply_read(FILE *fp, int *nelems, char ***elem_names)
plyfile->file_type = PLY_BINARY_LE;
else
{
- free(words);
+ if (words) free(words);
+ if (plyfile) free(plyfile);
return (NULL);
}
@@ -2226,49 +2232,81 @@ void get_binary_item(
switch (type) {
case PLY_CHAR:
- fread (ptr, 1, 1, fp);
+ if (fread (ptr, 1, 1, fp) != 1)
+ {
+ fprintf (stderr, "get_binary_item: read error\n");
+ exit (-1);
+ }
*int_val = *((char *) ptr);
*uint_val = *int_val;
*double_val = *int_val;
break;
case PLY_UCHAR:
- fread (ptr, 1, 1, fp);
+ if (fread (ptr, 1, 1, fp) != 1)
+ {
+ fprintf (stderr, "get_binary_item: read error\n");
+ exit (-1);
+ }
*uint_val = *((unsigned char *) ptr);
*int_val = *uint_val;
*double_val = *uint_val;
break;
case PLY_SHORT:
- fread (ptr, 2, 1, fp);
+ if (fread (ptr, 2, 1, fp) != 1)
+ {
+ fprintf (stderr, "get_binary_item: read error\n");
+ exit (-1);
+ }
*int_val = *((short int *) ptr);
*uint_val = *int_val;
*double_val = *int_val;
break;
case PLY_USHORT:
- fread (ptr, 2, 1, fp);
+ if (fread (ptr, 2, 1, fp) != 1)
+ {
+ fprintf (stderr, "get_binary_item: read error\n");
+ exit (-1);
+ }
*uint_val = *((unsigned short int *) ptr);
*int_val = *uint_val;
*double_val = *uint_val;
break;
case PLY_INT:
- fread (ptr, 4, 1, fp);
+ if (fread (ptr, 4, 1, fp) != 1)
+ {
+ fprintf (stderr, "get_binary_item: read error\n");
+ exit (-1);
+ }
*int_val = *((int *) ptr);
*uint_val = *int_val;
*double_val = *int_val;
break;
case PLY_UINT:
- fread (ptr, 4, 1, fp);
+ if (fread (ptr, 4, 1, fp) != 1)
+ {
+ fprintf (stderr, "get_binary_item: read error\n");
+ exit (-1);
+ }
*uint_val = *((unsigned int *) ptr);
*int_val = *uint_val;
*double_val = *uint_val;
break;
case PLY_FLOAT:
- fread (ptr, 4, 1, fp);
+ if (fread (ptr, 4, 1, fp) != 1)
+ {
+ fprintf (stderr, "get_binary_item: read error\n");
+ exit (-1);
+ }
*double_val = *((float *) ptr);
*int_val = (int)(*double_val);
*uint_val = (unsigned int)(*double_val);
break;
case PLY_DOUBLE:
- fread (ptr, 8, 1, fp);
+ if (fread (ptr, 8, 1, fp) != 1)
+ {
+ fprintf (stderr, "get_binary_item: read error\n");
+ exit (-1);
+ }
*double_val = *((double *) ptr);
*int_val = (int)(*double_val);
*uint_val = (unsigned int)(*double_val);
diff --git a/SurgSim/Devices/CMakeLists.txt b/SurgSim/Devices/CMakeLists.txt
index db76bde..e4bd80c 100644
--- a/SurgSim/Devices/CMakeLists.txt
+++ b/SurgSim/Devices/CMakeLists.txt
@@ -1,5 +1,5 @@
# This file is a part of the OpenSurgSim project.
-# Copyright 2012-2013, SimQuest Solutions Inc.
+# Copyright 2015, SimQuest Solutions Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -17,28 +17,59 @@ add_subdirectory(DeviceFilters)
add_subdirectory(IdentityPoseDevice)
add_subdirectory(Keyboard)
add_subdirectory(Mouse)
+add_subdirectory(ReplayPoseDevice)
-set(DEVICES
+set(OPTIONAL_DEVICES
LabJack
+ Leap
MultiAxis
+ Nimble
Novint
+ Oculus
+ OpenNI
Phantom
Sixense
TrackIR
)
-set(SURGSIM_DEVICES_DOCUMENTATION
- devices.dox
-)
-
-foreach(DEVICE ${DEVICES})
+set(DEVICE_LIBRARIES "SurgSimDeviceFilters;IdentityPoseDevice;KeyboardDevice;MouseDevice;ReplayPoseDevice;")
+set(DEVICE_DOCUMENTATION devices.dox)
+foreach(DEVICE ${OPTIONAL_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)
+ list(APPEND DEVICE_DOCUMENTATION ${DEVICE}/${DEVICE}.dox)
+ set(DEVICE_LIBRARIES "${DEVICE_LIBRARIES};${DEVICE}Device")
endif(BUILD_DEVICE_${DEVICE_UPPER_CASE})
endforeach(DEVICE)
-add_custom_target(SurgSimDevicesDocumentation SOURCES ${SURGSIM_DEVICES_DOCUMENTATION})
+set(SURGSIM_DEVICES_SOURCES
+ DeviceUtilities.cpp
+)
+
+set(SURGSIM_DEVICES_HEADERS
+ DeviceUtilities.h
+)
+
+surgsim_add_library(
+ SurgSimDevices
+ "${SURGSIM_DEVICES_SOURCES}"
+ "${SURGSIM_DEVICES_HEADERS}"
+)
+
+target_link_libraries(SurgSimDevices
+ SurgSimFramework
+ SurgSimInput
+ ${DEVICE_LIBRARIES}
+)
+set_target_properties(SurgSimDevices PROPERTIES FOLDER "Devices")
+
+surgsim_create_library_header(Devices.h "${SURGSIM_DEVICES_HEADERS};${DEVICE_HEADERS}")
+
+add_custom_target(SurgSimDevicesDocumentation SOURCES ${DEVICE_DOCUMENTATION})
set_target_properties(SurgSimDevicesDocumentation PROPERTIES FOLDER "Devices")
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+endif()
diff --git a/SurgSim/Devices/DeviceFilters/BoolToScalar.cpp b/SurgSim/Devices/DeviceFilters/BoolToScalar.cpp
new file mode 100644
index 0000000..d4154e2
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/BoolToScalar.cpp
@@ -0,0 +1,206 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/BoolToScalar.h"
+
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/DataStructures/DataGroupCopier.h"
+#include "SurgSim/Math/Scalar.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::BoolToScalar, BoolToScalar);
+
+BoolToScalar::BoolToScalar(const std::string& name) :
+ DeviceFilter(name),
+ m_isClamping(true),
+ m_value(0.0),
+ m_scale(1.0),
+ m_range(0.0, 1.0),
+ m_increaseField(DataStructures::Names::BUTTON_2),
+ m_decreaseField(DataStructures::Names::BUTTON_1),
+ m_targetField(DataStructures::Names::TOOLDOF)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(BoolToScalar, double, Scale, getScale, setScale);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(BoolToScalar, double, Scalar, getScalar, setScalar);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(BoolToScalar, bool, Clamping, isClamping, setClamping);
+
+ {
+ typedef std::pair<double, double> ParamType;
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(BoolToScalar, ParamType, Range, getRange, setRange);
+ }
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(BoolToScalar, std::string, IncreaseField,
+ getIncreaseField, setIncreaseField);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(BoolToScalar, std::string, DecreaseField,
+ getDecreaseField, setDecreaseField);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(BoolToScalar, std::string, TargetField, getTargetField, setTargetField);
+}
+
+BoolToScalar::~BoolToScalar()
+{
+}
+
+void BoolToScalar::initializeInput(const std::string& device, const DataStructures::DataGroup& inputData)
+{
+ SURGSIM_ASSERT(inputData.booleans().getIndex(m_decreaseField) != -1)
+ << "Can't find decrease field " << m_decreaseField << " in booleans of datagroup.";
+
+ SURGSIM_ASSERT(inputData.booleans().getIndex(m_increaseField) != -1)
+ << "Can't find increase field " << m_increaseField << " in booleans of datagroup.";
+
+ if (getInputData().isEmpty())
+ {
+ if (!inputData.scalars().hasEntry(m_targetField))
+ {
+ DataStructures::DataGroupBuilder builder;
+ builder.addEntriesFrom(inputData);
+ builder.addScalar(m_targetField);
+ getInputData() = builder.createData();
+ m_copier = std::make_shared<DataStructures::DataGroupCopier>(inputData, &getInputData());
+ }
+ }
+
+ if (m_copier == nullptr)
+ {
+ getInputData() = inputData;
+ }
+ else
+ {
+ m_copier->copy(inputData, &getInputData());
+ }
+
+ m_timer.start();
+}
+
+void BoolToScalar::filterInput(const std::string& device, const DataStructures::DataGroup& dataToFilter,
+ DataStructures::DataGroup* result)
+{
+ m_timer.markFrame();
+ double dt = m_timer.getLastFramePeriod();
+
+ if (m_copier == nullptr)
+ {
+ *result = dataToFilter;
+ }
+ else
+ {
+ m_copier->copy(dataToFilter, result);
+ }
+
+ bool increase = false;
+ dataToFilter.booleans().get(m_increaseField, &increase);
+
+ bool decrease = false;
+ dataToFilter.booleans().get(m_decreaseField, &decrease);
+
+
+ if (increase != decrease)
+ {
+ m_value += (increase) ? dt * m_scale : -1.0 * dt * m_scale;
+ if (m_isClamping)
+ {
+ m_value = Math::clamp(m_value, m_range.first, m_range.second, 0.0);
+ }
+ }
+
+ result->scalars().set(m_targetField, m_value);
+}
+
+
+double BoolToScalar::getScale() const
+{
+ return m_scale;
+}
+
+void BoolToScalar::setRange(const std::pair<double, double>& val)
+{
+ m_range = val;
+ if (val.first > val.second)
+ {
+ std::swap(m_range.first, m_range.second);
+ }
+}
+
+std::pair<double, double> BoolToScalar::getRange() const
+{
+ return m_range;
+}
+
+void BoolToScalar::setClamping(bool val)
+{
+ m_isClamping = val;
+}
+
+bool BoolToScalar::isClamping()
+{
+ return m_isClamping;
+}
+
+void BoolToScalar::setScale(double val)
+{
+ m_scale = val;
+}
+
+std::string BoolToScalar::getIncreaseField() const
+{
+ return m_increaseField;
+}
+
+
+void BoolToScalar::setIncreaseField(const std::string& val)
+{
+ m_increaseField = val;
+}
+
+std::string BoolToScalar::getDecreaseField() const
+{
+ return m_decreaseField;
+}
+
+void BoolToScalar::setDecreaseField(const std::string& val)
+{
+ m_decreaseField = val;
+}
+
+double BoolToScalar::getScalar() const
+{
+ return m_value;
+}
+
+void BoolToScalar::setScalar(double val)
+{
+ m_value = val;
+ if (m_isClamping)
+ {
+ m_value = Math::clamp(m_value, m_range.first, m_range.second, 0.0);
+ }
+}
+
+
+std::string BoolToScalar::getTargetField() const
+{
+ return m_targetField;
+}
+
+void BoolToScalar::setTargetField(const std::string& val)
+{
+ m_targetField = val;
+}
+
+}; // namespace Devices
+}; // namepsace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/DeviceFilters/BoolToScalar.h b/SurgSim/Devices/DeviceFilters/BoolToScalar.h
new file mode 100644
index 0000000..911f772
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/BoolToScalar.h
@@ -0,0 +1,125 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_BOOLTOSCALAR_H
+#define SURGSIM_DEVICES_DEVICEFILTERS_BOOLTOSCALAR_H
+
+#include "SurgSim/Devices/DeviceFilters/DeviceFilter.h"
+#include "SurgSim/Framework/Timer.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+class DataGroupCopier;
+}
+
+namespace Devices
+{
+
+SURGSIM_STATIC_REGISTRATION(BoolToScalar);
+
+/// Maps the on and off state of two boolean values to the increase and decrease of a scalar field,
+/// this for example enables the driving on a scalar value through two buttons on a device, no change occurs
+/// when both booleans are true at the same time
+class BoolToScalar : public DeviceFilter
+{
+public:
+ /// Constructor
+ explicit BoolToScalar(const std::string& name);
+
+ /// Destructor
+ ~BoolToScalar();
+
+ SURGSIM_CLASSNAME(SurgSim::Devices::BoolToScalar);
+
+ void initializeInput(const std::string& device, const DataStructures::DataGroup& inputData) override;
+
+ void filterInput(const std::string& device, const DataStructures::DataGroup& dataToFilter,
+ DataStructures::DataGroup* result) override;
+
+ /// Sets the value that gets used, the actual value to be added is scale * dt per call to filterInput
+ /// \param val scale to be used
+ void setScale(double val);
+
+ /// \return the scale for addition
+ double getScale() const;
+
+ /// Set the range of values that should be produced by this
+ /// \param val (min and max for the range)
+ void setRange(const std::pair<double, double>& val);
+
+ /// \return the range of values used
+ std::pair<double, double> getRange() const;
+
+ /// Enables or disables clamping on the output value (default true)
+ /// \param val whether to enable or disable clamping
+ void setClamping(bool val);
+
+ /// \return whether the output value is being clamped
+ bool isClamping();
+
+ /// Set the field that is read to increase the scalar value, needs to be a bool
+ /// \param val name of the field to read for an increase
+ void setIncreaseField(const std::string& val);
+
+ /// \return the name of the field used for increasing the value
+ std::string getIncreaseField() const;
+
+ /// Set the field this is read to decrease the scalar value, needs to be a bool
+ /// \param val name of the field to be read for decrease
+ void setDecreaseField(const std::string& val);
+
+ /// \return the name of the field used for decreasing the value
+ std::string getDecreaseField() const;
+
+ /// Set the value of the mapped field, can also be used to set a starting value for the field
+ /// \param val new Value to be used for the scalar field
+ void setScalar(double val);
+
+ /// \return the actual value of the mapped field
+ double getScalar() const;
+
+ /// Set the name of the target field that carries the scalar value, will be created in the datagroup if it doesn't
+ /// exist
+ /// \param val name of the target field
+ void setTargetField(const std::string& val);
+
+ /// \return the name of the target field for the scalar information
+ std::string getTargetField() const;
+
+private:
+ bool m_isClamping;
+
+ double m_value;
+
+ double m_scale;
+
+ std::pair<double, double> m_range;
+
+ std::string m_increaseField;
+ std::string m_decreaseField;
+ std::string m_targetField;
+
+ Framework::Timer m_timer;
+
+ std::shared_ptr<DataStructures::DataGroupCopier> m_copier;
+
+};
+
+}; // namespace Devices
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Devices/DeviceFilters/CMakeLists.txt b/SurgSim/Devices/DeviceFilters/CMakeLists.txt
index e4e1448..c3661e6 100644
--- a/SurgSim/Devices/DeviceFilters/CMakeLists.txt
+++ b/SurgSim/Devices/DeviceFilters/CMakeLists.txt
@@ -1,5 +1,5 @@
# This file is a part of the OpenSurgSim project.
-# Copyright 2013, SimQuest Solutions Inc.
+# Copyright 2013-2016, SimQuest Solutions Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -13,30 +13,48 @@
# 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
+ SurgSimMath
)
set(DEVICE_FILTERS_SOURCES
+ BoolToScalar.cpp
+ DeviceFilter.cpp
+ FilteredDevice.cpp
ForceScale.cpp
PoseIntegrator.cpp
PoseTransform.cpp
+ RecordPose.cpp
)
set(DEVICE_FILTERS_HEADERS
+ BoolToScalar.h
+ DeviceFilter.h
+ FilteredDevice.h
ForceScale.h
PoseIntegrator.h
PoseTransform.h
+ RecordPose.h
)
+set(DEVICE_HEADERS ${DEVICE_HEADERS}
+ DeviceFilters/BoolToScalar.h #NOLINT
+ DeviceFilters/DeviceFilter.h #NOLINT
+ DeviceFilters/FilteredDevice.h #NOLINT
+ DeviceFilters/ForceScale.h #NOLINT
+ DeviceFilters/PoseIntegrator.h #NOLINT
+ DeviceFilters/PoseTransform.h #NOLINT
+ DeviceFilters/RecordPose.h #NOLINT
+ PARENT_SCOPE)
+
surgsim_add_library(
SurgSimDeviceFilters
- "${DEVICE_FILTERS_SOURCES}" "${DEVICE_FILTERS_HEADERS}"
- SurgSim/Devices/DeviceFilters
+ "${DEVICE_FILTERS_SOURCES}"
+ "${DEVICE_FILTERS_HEADERS}"
)
target_link_libraries(SurgSimDeviceFilters ${LIBS})
diff --git a/SurgSim/Devices/DeviceFilters/DeviceFilter.cpp b/SurgSim/Devices/DeviceFilters/DeviceFilter.cpp
new file mode 100644
index 0000000..7645cb1
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/DeviceFilter.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/Devices/DeviceFilters/DeviceFilter.h"
+
+#include "SurgSim/DataStructures/DataGroup.h"
+
+using SurgSim::DataStructures::DataGroup;
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+DeviceFilter::DeviceFilter(const std::string& name) : CommonDevice(name), m_initialized(false)
+{
+}
+
+bool DeviceFilter::initialize()
+{
+ SURGSIM_ASSERT(!isInitialized()) << getName() << " already initialized.";
+ m_initialized = true;
+ return true;
+}
+
+bool DeviceFilter::finalize()
+{
+ SURGSIM_ASSERT(isInitialized()) << getName() << " is not initialized, cannot finalize.";
+ m_initialized = false;
+ return true;
+}
+
+bool DeviceFilter::isInitialized() const
+{
+ return m_initialized;
+}
+
+void DeviceFilter::initializeInput(const std::string& device, const DataGroup& inputData)
+{
+ filterInput(device, inputData, &getInputData());
+}
+
+void DeviceFilter::handleInput(const std::string& device, const DataGroup& inputData)
+{
+ filterInput(device, inputData, &getInputData());
+ pushInput();
+}
+
+bool DeviceFilter::requestOutput(const std::string& device, DataGroup* outputData)
+{
+ bool state = pullOutput();
+ if (state)
+ {
+ filterOutput(device, getOutputData(), outputData);
+ }
+ return state;
+}
+
+void DeviceFilter::filterInput(const std::string& device, const DataGroup& dataToFilter, DataGroup* result)
+{
+ *result = dataToFilter;
+}
+
+void DeviceFilter::filterOutput(const std::string& device, const DataGroup& dataToFilter, DataGroup* result)
+{
+ *result = dataToFilter;
+}
+
+}; // namespace Devices
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/DeviceFilters/DeviceFilter.h b/SurgSim/Devices/DeviceFilters/DeviceFilter.h
new file mode 100644
index 0000000..0cb34fa
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/DeviceFilter.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_DEVICES_DEVICEFILTERS_DEVICEFILTER_H
+#define SURGSIM_DEVICES_DEVICEFILTERS_DEVICEFILTER_H
+
+#include <string>
+
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+class DataGroup;
+}
+
+namespace Devices
+{
+
+/// A device filter can be connected between a device and the InputConsumerInterface (e.g., InputComponent) and/or
+/// the OutputProducerInterface (e.g., OutputComponent), and can alter the data being passed from/to the device.
+class DeviceFilter :
+ public Input::CommonDevice, public Input::InputConsumerInterface, public Input::OutputProducerInterface
+{
+public:
+ /// Constructor.
+ /// \param name Name of this device filter.
+ explicit DeviceFilter(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Devices::DeviceFilter);
+
+ bool initialize() override;
+
+ bool isInitialized() const override;
+
+ void initializeInput(const std::string& device, const DataStructures::DataGroup& inputData) override;
+
+ void handleInput(const std::string& device, const DataStructures::DataGroup& inputData) override;
+
+ bool requestOutput(const std::string& device, DataStructures::DataGroup* outputData) override;
+
+protected:
+ /// Filter the input data.
+ /// \param device The name of the device pushing 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.
+ virtual void filterInput(const std::string& device, const DataStructures::DataGroup& dataToFilter,
+ DataStructures::DataGroup* result);
+
+ /// Filter the output data.
+ /// \param device The name of the device pulling the output 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.
+ virtual void filterOutput(const std::string& device, const DataStructures::DataGroup& dataToFilter,
+ DataStructures::DataGroup* result);
+
+protected:
+ bool finalize() override;
+
+ /// true if initialized and not finalized.
+ bool m_initialized;
+};
+
+}; // namespace Devices
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_DEVICEFILTERS_DEVICEFILTER_H
diff --git a/SurgSim/Devices/DeviceFilters/FilteredDevice.cpp b/SurgSim/Devices/DeviceFilters/FilteredDevice.cpp
new file mode 100644
index 0000000..203d4d4
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/FilteredDevice.cpp
@@ -0,0 +1,277 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/FilteredDevice.h"
+
+#include "SurgSim/Devices/DeviceFilters/DeviceFilter.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
+
+using SurgSim::Input::InputConsumerInterface;
+using SurgSim::Input::OutputProducerInterface;
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::FilteredDevice, FilteredDevice);
+
+FilteredDevice::FilteredDevice(const std::string& name) :
+ m_name(name),
+ m_initialized(false),
+ m_logger(Framework::Logger::getLogger("Devices/FilteredDevice"))
+{
+ m_devices.push_back(nullptr);
+}
+
+FilteredDevice::~FilteredDevice()
+{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+
+std::string FilteredDevice::getName() const
+{
+ return m_name;
+}
+
+bool FilteredDevice::initialize()
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Already initialized.";
+ boost::unique_lock<boost::shared_mutex>(m_deviceMutex);
+ SURGSIM_ASSERT(m_devices.size() > 0) << "There must be at least one device.";
+
+ bool result = true;
+ if (m_devices[0] == nullptr)
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "A raw/base device is required for " << getName();
+ result = false;
+ }
+
+ if (result)
+ {
+ for (size_t i = 0; i < m_devices.size() - 1; ++i)
+ {
+ auto deviceFilter = std::dynamic_pointer_cast<DeviceFilter>(m_devices[i + 1]);
+ SURGSIM_ASSERT(deviceFilter != nullptr) << "Expected a device filter.";
+ result = result && m_devices[i]->addInputConsumer(deviceFilter) &&
+ m_devices[i]->setOutputProducer(deviceFilter);
+ }
+
+ for (auto& device : m_devices)
+ {
+ if (!device->isInitialized())
+ {
+ result = result && device->initialize();
+ }
+ }
+
+ if (!result)
+ {
+ doFinalize();
+ }
+ }
+
+ m_initialized = result;
+ SURGSIM_LOG_IF(!result, m_logger, WARNING) << "Failed to initialize " << getName();
+ return result;
+}
+
+bool FilteredDevice::isInitialized() const
+{
+ return m_initialized;
+}
+
+bool FilteredDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized()) << getName() << " is not initialized, cannot finalize.";
+ m_initialized = false;
+ boost::shared_lock<boost::shared_mutex>(m_deviceMutex);
+ doFinalize();
+ return true;
+}
+
+void FilteredDevice::doFinalize()
+{
+ for (size_t i = 0; i < m_devices.size() - 1; ++i)
+ {
+ auto deviceFilter = std::dynamic_pointer_cast<DeviceFilter>(m_devices[i + 1]);
+ SURGSIM_ASSERT(deviceFilter != nullptr) << "Expected a device filter.";
+ m_devices[i]->removeInputConsumer(deviceFilter);
+ m_devices[i]->removeOutputProducer(deviceFilter);
+ }
+}
+
+bool FilteredDevice::addInputConsumer(std::shared_ptr<InputConsumerInterface> inputConsumer)
+{
+ boost::shared_lock<boost::shared_mutex>(m_deviceMutex);
+ SURGSIM_ASSERT(m_devices.size() > 0) << "There must be at least one device.";
+ bool result = false;
+ if (m_devices.back() == nullptr)
+ {
+ SURGSIM_LOG_WARNING(m_logger) <<
+ "Cannot addInputConsumer on " << getName() << " before setting the device and/or adding a filter.";
+ }
+ else
+ {
+ result = m_devices.back()->addInputConsumer(inputConsumer);
+ }
+ return result;
+}
+
+bool FilteredDevice::removeInputConsumer(std::shared_ptr<InputConsumerInterface> inputConsumer)
+{
+ boost::shared_lock<boost::shared_mutex>(m_deviceMutex);
+ SURGSIM_ASSERT(m_devices.size() > 0) << "There must be at least one device.";
+ bool result = false;
+ if (m_devices.back() == nullptr)
+ {
+ SURGSIM_LOG_WARNING(m_logger) <<
+ getName() << " does not have a device or filter from which to removeInputConsumer.";
+ }
+ else
+ {
+ result = m_devices.back()->removeInputConsumer(inputConsumer);
+ }
+ return result;
+}
+
+void FilteredDevice::clearInputConsumers()
+{
+ boost::shared_lock<boost::shared_mutex>(m_deviceMutex);
+ SURGSIM_ASSERT(m_devices.size() > 0) << "There must be at least one device.";
+ if (m_devices.back() != nullptr)
+ {
+ m_devices.back()->clearInputConsumers();
+ }
+}
+
+bool FilteredDevice::setOutputProducer(std::shared_ptr<OutputProducerInterface> outputProducer)
+{
+ boost::shared_lock<boost::shared_mutex>(m_deviceMutex);
+ SURGSIM_ASSERT(m_devices.size() > 0) << "There must be at least one device.";
+ bool result = false;
+ if (m_devices.back() == nullptr)
+ {
+ SURGSIM_LOG_WARNING(m_logger) <<
+ "Cannot setOutputProducer on " << getName() << " before setting the device and/or adding a filter.";
+ }
+ else
+ {
+ result = m_devices.back()->setOutputProducer(outputProducer);
+ }
+ return result;
+}
+
+bool FilteredDevice::removeOutputProducer(std::shared_ptr<OutputProducerInterface> outputProducer)
+{
+ boost::shared_lock<boost::shared_mutex>(m_deviceMutex);
+ SURGSIM_ASSERT(m_devices.size() > 0) << "There must be at least one device.";
+ bool result = false;
+ if (m_devices.back() == nullptr)
+ {
+ SURGSIM_LOG_WARNING(m_logger) <<
+ getName() << " does not have a device or filter from which to removeOutputProducer.";
+ }
+ else
+ {
+ result = m_devices.back()->removeOutputProducer(outputProducer);
+ }
+ return result;
+}
+
+bool FilteredDevice::hasOutputProducer()
+{
+ boost::shared_lock<boost::shared_mutex>(m_deviceMutex);
+ SURGSIM_ASSERT(m_devices.size() > 0) << "There must be at least one device.";
+ return ((m_devices.back() != nullptr) && m_devices.back()->hasOutputProducer());
+}
+
+void FilteredDevice::clearOutputProducer()
+{
+ boost::shared_lock<boost::shared_mutex>(m_deviceMutex);
+ SURGSIM_ASSERT(m_devices.size() > 0) << "There must be at least one device.";
+ if (m_devices.back() != nullptr)
+ {
+ m_devices.back()->clearOutputProducer();
+ }
+}
+
+void FilteredDevice::setDevice(std::shared_ptr<Input::DeviceInterface> device)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot set device after initialization";
+ SURGSIM_ASSERT(device != nullptr) << "Cannot set a nullptr device.";
+ boost::unique_lock<boost::shared_mutex>(m_deviceMutex);
+ m_devices[0] = device;
+}
+
+void FilteredDevice::addFilter(std::shared_ptr<DeviceFilter> filter)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot add filter after initialization";
+ SURGSIM_ASSERT(filter != nullptr) << "Cannot add a nullptr filter.";
+ boost::unique_lock<boost::shared_mutex>(m_deviceMutex);
+ m_devices.push_back(filter);
+}
+
+const std::vector<std::shared_ptr<Input::DeviceInterface>>& FilteredDevice::getDevices() const
+{
+ boost::shared_lock<boost::shared_mutex>(m_deviceMutex);
+ return m_devices;
+}
+
+bool FilteredDevice::setDevices(const std::vector<std::shared_ptr<Input::DeviceInterface>>& devices)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot set devices after initialization";
+ bool result = true;
+ boost::unique_lock<boost::shared_mutex>(m_deviceMutex);
+ if (devices.size() > 0)
+ {
+ for (const auto& device : devices)
+ {
+ if (device == nullptr)
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Cannot set a nullptr device on " << getName();
+ result = false;
+ }
+ else
+ {
+ if ((device != devices[0]) && (std::dynamic_pointer_cast<DeviceFilter>(device) == nullptr))
+ {
+ SURGSIM_LOG_WARNING(m_logger) << getName() << " setDevices got a " << device->getClassName() <<
+ " named " << device->getName() << " where a DeviceFilter is required.";
+ result = false;
+ }
+ }
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Cannot set 0 devices on " << getName();
+ result = false;
+ }
+
+ if (result)
+ {
+ m_devices = devices;
+ }
+ return result;
+}
+
+}; // namespace Devices
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/DeviceFilters/FilteredDevice.h b/SurgSim/Devices/DeviceFilters/FilteredDevice.h
new file mode 100644
index 0000000..b413bf9
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/FilteredDevice.h
@@ -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.
+
+#ifndef SURGSIM_DEVICES_DEVICEFILTERS_FILTEREDDEVICE_H
+#define SURGSIM_DEVICES_DEVICEFILTERS_FILTEREDDEVICE_H
+
+#include <boost/thread/shared_mutex.hpp>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "SurgSim/Input/DeviceInterface.h"
+
+namespace SurgSim
+{
+namespace Input
+{
+class InputConsumerInterface;
+class OutputProducerInterface;
+}
+
+namespace Devices
+{
+class DeviceFilter;
+
+/// A DeviceInterface connected in series with one or more DeviceFilters. Useful for serialization.
+class FilteredDevice : public Input::DeviceInterface
+{
+public:
+ /// Constructor.
+ /// \param name Name of this device.
+ explicit FilteredDevice(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Devices::FilteredDevice);
+
+ /// Destructor.
+ virtual ~FilteredDevice();
+
+ std::string getName() const override;
+
+ bool initialize() override;
+
+ bool isInitialized() const override;
+
+ bool addInputConsumer(std::shared_ptr<Input::InputConsumerInterface> inputConsumer) override;
+ bool removeInputConsumer(std::shared_ptr<Input::InputConsumerInterface> inputConsumer) override;
+ void clearInputConsumers() override;
+ bool setOutputProducer(std::shared_ptr<Input::OutputProducerInterface> outputProducer) override;
+ bool removeOutputProducer(std::shared_ptr<Input::OutputProducerInterface> outputProducer) override;
+ bool hasOutputProducer() override;
+ void clearOutputProducer() override;
+
+ /// Sets the raw/base device.
+ /// \param device The device connected to the filter(s).
+ void setDevice(std::shared_ptr<Input::DeviceInterface> device);
+
+ /// Adds a DeviceFilter. The first filter that is added will be connected to the raw/base device.
+ /// The last filter that is added will interface with InputConsumers and/or an OutputProducer.
+ /// Any filters added in-between will be connected in order.
+ /// \param filter A DeviceFilter.
+ void addFilter(std::shared_ptr<DeviceFilter> filter);
+
+ /// \return All devices.
+ const std::vector<std::shared_ptr<Input::DeviceInterface>>& getDevices() const;
+
+ /// Sets the devices.
+ /// \param devices All the devices.
+ /// \return true on success
+ bool setDevices(const std::vector<std::shared_ptr<Input::DeviceInterface>>& devices);
+
+private:
+ bool finalize() override;
+
+ /// Implements the finalize functionality.
+ void doFinalize();
+
+ /// The name of this device.
+ std::string m_name;
+
+ /// true if initialized and not finalized.
+ bool m_initialized;
+
+ /// The devices. m_devices.back() will be connected to any InputComponent or OutputComponent.
+ /// m_devices.front() is the raw/base device to be filtered. If this contains more than one element, all but
+ /// the front element are DeviceFilters.
+ std::vector<std::shared_ptr<Input::DeviceInterface>> m_devices;
+
+ /// The mutex to protect access to the devices.
+ boost::shared_mutex m_deviceMutex;
+
+ /// The logger.
+ std::shared_ptr<Framework::Logger> m_logger;
+};
+
+}; // namespace Devices
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_DEVICEFILTERS_FILTEREDDEVICE_H
diff --git a/SurgSim/Devices/DeviceFilters/ForceScale.cpp b/SurgSim/Devices/DeviceFilters/ForceScale.cpp
index 101c370..1dc7dfd 100644
--- a/SurgSim/Devices/DeviceFilters/ForceScale.cpp
+++ b/SurgSim/Devices/DeviceFilters/ForceScale.cpp
@@ -1,5 +1,5 @@
-// This filrgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -24,104 +24,54 @@ using SurgSim::Math::Vector3d;
namespace SurgSim
{
-namespace Device
+namespace Devices
{
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::ForceScale, ForceScale);
+
ForceScale::ForceScale(const std::string& name) :
- CommonDevice(name),
+ DeviceFilter(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()
+ m_torqueScale(1.0)
{
- 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)
+void ForceScale::filterOutput(const std::string& device, const DataGroup& dataToFilter, DataGroup* result)
{
boost::lock_guard<boost::mutex> lock(m_mutex);
- *result = dataToFilter; // Pass on all the data entries.
+ *result = dataToFilter;
- Vector3d force = Vector3d::Zero();
- if (dataToFilter.vectors().get(m_forceIndex, &force))
+ Vector3d force;
+ if (dataToFilter.vectors().get(DataStructures::Names::FORCE, &force))
{
force *= m_forceScale;
- result->vectors().set(m_forceIndex, force);
+ result->vectors().set(DataStructures::Names::FORCE, force);
}
- Vector3d torque = Vector3d::Zero();
- if (dataToFilter.vectors().get(m_torqueIndex, &torque))
+ Vector3d torque;
+ if (dataToFilter.vectors().get(DataStructures::Names::TORQUE, &torque))
{
torque *= m_torqueScale;
- result->vectors().set(m_torqueIndex, torque);
+ result->vectors().set(DataStructures::Names::TORQUE, 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))
+ DataGroup::DynamicMatrixType springJacobian;
+ if (dataToFilter.matrices().get(DataStructures::Names::SPRING_JACOBIAN, &springJacobian))
{
springJacobian.block<3,6>(0, 0) *= m_forceScale;
springJacobian.block<3,6>(3, 0) *= m_torqueScale;
- result->matrices().set(m_springJacobianIndex, springJacobian);
+ result->matrices().set(DataStructures::Names::SPRING_JACOBIAN, 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))
+ DataGroup::DynamicMatrixType damperJacobian;
+ if (dataToFilter.matrices().get(DataStructures::Names::DAMPER_JACOBIAN, &damperJacobian))
{
damperJacobian.block<3,6>(0, 0) *= m_forceScale;
damperJacobian.block<3,6>(3, 0) *= m_torqueScale;
- result->matrices().set(m_damperJacobianIndex, damperJacobian);
+ result->matrices().set(DataStructures::Names::DAMPER_JACOBIAN, damperJacobian);
}
}
@@ -137,5 +87,5 @@ void ForceScale::setTorqueScale(double torqueScale)
m_torqueScale = torqueScale;
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/DeviceFilters/ForceScale.h b/SurgSim/Devices/DeviceFilters/ForceScale.h
index 3492d29..4340d0e 100644
--- a/SurgSim/Devices/DeviceFilters/ForceScale.h
+++ b/SurgSim/Devices/DeviceFilters/ForceScale.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -19,61 +19,28 @@
#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"
+#include "SurgSim/Devices/DeviceFilters/DeviceFilter.h"
namespace SurgSim
{
-
-namespace Device
+namespace Devices
{
+
+SURGSIM_STATIC_REGISTRATION(ForceScale);
+
/// 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
+class ForceScale : public DeviceFilter
{
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;
+ SURGSIM_CLASSNAME(SurgSim::Devices::ForceScale);
/// Set the force scale factor so that each direction has the same scale.
/// \param forceScale The scalar scaling factor.
@@ -84,16 +51,8 @@ public:
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);
+ void filterOutput(const std::string& device, const DataStructures::DataGroup& dataToFilter,
+ DataStructures::DataGroup* result) override;
/// The mutex that protects the scaling factors.
boost::mutex m_mutex;
@@ -103,20 +62,9 @@ private:
/// 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 Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_DEVICEFILTERS_FORCESCALE_H
diff --git a/SurgSim/Devices/DeviceFilters/PoseIntegrator.cpp b/SurgSim/Devices/DeviceFilters/PoseIntegrator.cpp
index 74ea9dd..a4fc358 100644
--- a/SurgSim/Devices/DeviceFilters/PoseIntegrator.cpp
+++ b/SurgSim/Devices/DeviceFilters/PoseIntegrator.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -27,17 +27,14 @@ using SurgSim::Math::Vector3d;
namespace SurgSim
{
-namespace Device
+namespace Devices
{
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::PoseIntegrator, PoseIntegrator);
+
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)
+ DeviceFilter(name),
+ m_poseResult(PoseType::Identity())
{
}
@@ -49,31 +46,19 @@ const PoseIntegrator::PoseType& PoseIntegrator::integrate(const PoseType& pose)
return m_poseResult;
}
-bool PoseIntegrator::initialize()
-{
- return true;
-}
-
-bool PoseIntegrator::finalize()
+void PoseIntegrator::initializeInput(const std::string& device, const DataStructures::DataGroup& inputData)
{
- return true;
-}
-
-void PoseIntegrator::initializeInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData)
-{
- if (m_firstInput)
+ if (getInputData().isEmpty())
{
- m_firstInput = false;
-
- if (!inputData.vectors().hasEntry(SurgSim::DataStructures::Names::LINEAR_VELOCITY) ||
- !inputData.vectors().hasEntry(SurgSim::DataStructures::Names::ANGULAR_VELOCITY))
+ if (!inputData.vectors().hasEntry(DataStructures::Names::LINEAR_VELOCITY) ||
+ !inputData.vectors().hasEntry(DataStructures::Names::ANGULAR_VELOCITY))
{
- SurgSim::DataStructures::DataGroupBuilder builder;
+ DataStructures::DataGroupBuilder builder;
builder.addEntriesFrom(inputData);
- builder.addVector(SurgSim::DataStructures::Names::LINEAR_VELOCITY);
- builder.addVector(SurgSim::DataStructures::Names::ANGULAR_VELOCITY);
+ builder.addVector(DataStructures::Names::LINEAR_VELOCITY);
+ builder.addVector(DataStructures::Names::ANGULAR_VELOCITY);
getInputData() = builder.createData();
- m_copier = std::make_shared<SurgSim::DataStructures::DataGroupCopier>(inputData, getInputData());
+ m_copier = std::make_shared<DataStructures::DataGroupCopier>(inputData, &getInputData());
}
}
@@ -83,22 +68,17 @@ void PoseIntegrator::initializeInput(const std::string& device, const SurgSim::D
}
else
{
- m_copier->copy();
+ m_copier->copy(inputData, &getInputData());
}
- 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))
+ PoseType pose;
+ if (inputData.poses().get(DataStructures::Names::POSE, &pose))
{
m_poseResult = pose;
}
}
-void PoseIntegrator::handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData)
+void PoseIntegrator::handleInput(const std::string& device, const DataStructures::DataGroup& inputData)
{
if (m_copier == nullptr)
{
@@ -106,73 +86,54 @@ void PoseIntegrator::handleInput(const std::string& device, const SurgSim::DataS
}
else
{
- m_copier->copy();
+ m_copier->copy(inputData, &getInputData());
}
- if (m_poseIndex >= 0)
+ PoseType pose;
+ if (inputData.poses().get(DataStructures::Names::POSE, &pose))
{
- 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.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);
+ m_timer.start();
+ rate = 0.0;
+ SURGSIM_LOG_DEBUG(Framework::Logger::getLogger("Devices/Filters/PoseIntegrator")) <<
+ "The Timer used by " << getName() <<
+ " had a clock fail. The calculated velocities will be zero this update.";
}
- }
- pushInput();
-}
-bool PoseIntegrator::requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData)
-{
- bool state = pullOutput();
- if (state)
- {
- *outputData = getOutputData();
+ if (!boost::math::isnormal(rate))
+ {
+ rate = 0.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;
+ Math::computeAngleAndAxis(pose.rotation(), &angle, &rotationAxis);
+ rotationAxis = m_poseResult.rotation() * rotationAxis; // rotate the axis into global space
+ getInputData().vectors().set(DataStructures::Names::ANGULAR_VELOCITY, rotationAxis * angle * rate);
+ getInputData().poses().set(DataStructures::Names::POSE, integrate(pose));
+ getInputData().vectors().set(DataStructures::Names::LINEAR_VELOCITY, pose.translation() * rate);
}
- return state;
+ pushInput();
}
void PoseIntegrator::setReset(const std::string& name)
{
- SURGSIM_ASSERT(m_firstInput) <<
+ SURGSIM_ASSERT(getInputData().isEmpty()) <<
"PoseIntegrator::setReset cannot be called after the first call to initializeInput.";
m_resetName = name;
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/DeviceFilters/PoseIntegrator.h b/SurgSim/Devices/DeviceFilters/PoseIntegrator.h
index 5c71c8a..2894322 100644
--- a/SurgSim/Devices/DeviceFilters/PoseIntegrator.h
+++ b/SurgSim/Devices/DeviceFilters/PoseIntegrator.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -19,11 +19,8 @@
#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/Devices/DeviceFilters/DeviceFilter.h"
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Framework/Timer.h"
@@ -34,44 +31,41 @@ namespace DataStructures
class DataGroupCopier;
};
-namespace Device
+namespace Devices
{
+
+SURGSIM_STATIC_REGISTRATION(PoseIntegrator);
+
/// 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
+class PoseIntegrator : public DeviceFilter
{
public:
/// The type used for poses.
- typedef SurgSim::Math::RigidTransform3d PoseType;
+ typedef Math::RigidTransform3d PoseType;
/// Constructor.
/// \param name Name of this device filter.
explicit PoseIntegrator(const std::string& name);
+ SURGSIM_CLASSNAME(SurgSim::Devices::PoseIntegrator);
+
/// 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;
+ void initializeInput(const std::string& device, const 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;
+ void handleInput(const std::string& device, const DataStructures::DataGroup& inputData) 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.
@@ -86,28 +80,17 @@ private:
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;
+ Framework::Timer m_timer;
/// A copier into the input DataGroup, if needed.
- std::shared_ptr<SurgSim::DataStructures::DataGroupCopier> m_copier;
+ std::shared_ptr<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 Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_DEVICEFILTERS_POSEINTEGRATOR_H
diff --git a/SurgSim/Devices/DeviceFilters/PoseTransform.cpp b/SurgSim/Devices/DeviceFilters/PoseTransform.cpp
index b7035b2..82bc63a 100644
--- a/SurgSim/Devices/DeviceFilters/PoseTransform.cpp
+++ b/SurgSim/Devices/DeviceFilters/PoseTransform.cpp
@@ -17,6 +17,7 @@
#include "SurgSim/Devices/DeviceFilters/PoseTransform.h"
+#include "SurgSim/Math/MathConvert.h"
#include "SurgSim/Math/Matrix.h"
using SurgSim::DataStructures::DataGroup;
@@ -25,126 +26,55 @@ using SurgSim::Math::Vector3d;
namespace SurgSim
{
-namespace Device
+namespace Devices
{
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::PoseTransform, PoseTransform);
+
PoseTransform::PoseTransform(const std::string& name) :
- CommonDevice(name),
+ DeviceFilter(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)
+ m_translationScale(1.0)
{
- inputFilter(inputData, &getInputData());
- pushInput();
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(PoseTransform, double, TranslationScale,
+ getTranslationScale, setTranslationScale);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(PoseTransform, RigidTransform3d, Transform, getTransform, setTransform);
}
-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)
+void PoseTransform::filterInput(const std::string& device, 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(DataStructures::Names::POSE, &pose))
{
- 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);
- }
+ pose.translation() *= m_translationScale;
+ pose = m_transform * pose;
+ result->poses().set(DataStructures::Names::POSE, 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(DataStructures::Names::LINEAR_VELOCITY, &linearVelocity))
{
- // 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);
- }
+ linearVelocity *= m_translationScale;
+ linearVelocity = m_transform.linear() * linearVelocity;
+ result->vectors().set(DataStructures::Names::LINEAR_VELOCITY, linearVelocity);
}
- if (m_angularVelocityIndex >= 0)
+ Vector3d angularVelocity; // If there is an angular velocity, rotate it.
+ if (dataToFilter.vectors().get(DataStructures::Names::ANGULAR_VELOCITY, &angularVelocity))
{
- 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);
- }
+ angularVelocity = m_transform.linear() * angularVelocity;
+ result->vectors().set(DataStructures::Names::ANGULAR_VELOCITY, angularVelocity);
}
}
-void PoseTransform::outputFilter(const DataGroup& dataToFilter, DataGroup* result)
+void PoseTransform::filterOutput(const std::string& device, const DataGroup& dataToFilter, DataGroup* result)
{
boost::lock_guard<boost::mutex> lock(m_mutex); // Prevent the transform or scaling from being set simultaneously.
@@ -160,24 +90,18 @@ void PoseTransform::outputFilter(const DataGroup& dataToFilter, DataGroup* resul
// 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(DataStructures::Names::FORCE, &force))
{
- Vector3d force;
- if (dataToFilter.vectors().get(m_forceIndex, &force))
- {
- force = m_transformInverse.linear() * force;
- result->vectors().set(m_forceIndex, force);
- }
+ force = m_transformInverse.linear() * force;
+ result->vectors().set(DataStructures::Names::FORCE, force);
}
- if (m_torqueIndex >= 0)
+ Vector3d torque;
+ if (dataToFilter.vectors().get(DataStructures::Names::TORQUE, &torque))
{
- Vector3d torque;
- if (dataToFilter.vectors().get(m_torqueIndex, &torque))
- {
- torque = m_transformInverse.linear() * torque;
- result->vectors().set(m_torqueIndex, torque);
- }
+ torque = m_transformInverse.linear() * torque;
+ result->vectors().set(DataStructures::Names::TORQUE, torque);
}
// The Jacobians must be transformed into device space. The Jacobians are scaled based on the translation scaling,
@@ -191,83 +115,76 @@ void PoseTransform::outputFilter(const DataGroup& dataToFilter, DataGroup* resul
// 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)
+ DataGroup::DynamicMatrixType springJacobian;
+ if (dataToFilter.matrices().get(DataStructures::Names::SPRING_JACOBIAN, &springJacobian))
{
- 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);
- }
+ 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(DataStructures::Names::SPRING_JACOBIAN, 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);
- }
- }
+ RigidTransform3d inputPose;
+ if (dataToFilter.poses().get(DataStructures::Names::INPUT_POSE, &inputPose))
+ {
+ inputPose = m_transformInverse * inputPose;
+ inputPose.translation() /= m_translationScale;
+ result->poses().set(DataStructures::Names::INPUT_POSE, inputPose);
}
- if (m_damperJacobianIndex >= 0)
+ DataGroup::DynamicMatrixType damperJacobian;
+ if (dataToFilter.matrices().get(DataStructures::Names::DAMPER_JACOBIAN, &damperJacobian))
{
- 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);
- }
+ 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(DataStructures::Names::DAMPER_JACOBIAN, 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);
- }
- }
+ Vector3d inputLinearVelocity;
+ if (dataToFilter.vectors().get(DataStructures::Names::INPUT_LINEAR_VELOCITY, &inputLinearVelocity))
+ {
+ inputLinearVelocity = m_transformInverse.linear() * inputLinearVelocity;
+ inputLinearVelocity /= m_translationScale;
+ result->vectors().set(DataStructures::Names::INPUT_LINEAR_VELOCITY, 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);
- }
- }
+ Vector3d inputAngularVelocity;
+ if (dataToFilter.vectors().get(DataStructures::Names::INPUT_ANGULAR_VELOCITY, &inputAngularVelocity))
+ {
+ inputAngularVelocity = m_transformInverse.linear() * inputAngularVelocity;
+ result->vectors().set(DataStructures::Names::INPUT_ANGULAR_VELOCITY, inputAngularVelocity);
}
}
+double PoseTransform::getTranslationScale() const
+{
+ return m_translationScale;
+}
+
void PoseTransform::setTranslationScale(double translationScale)
{
boost::lock_guard<boost::mutex> lock(m_mutex);
m_translationScale = translationScale;
}
+const RigidTransform3d& PoseTransform::getTransform() const
+{
+ return m_transform;
+}
+
void PoseTransform::setTransform(const RigidTransform3d& transform)
{
boost::lock_guard<boost::mutex> lock(m_mutex);
@@ -275,5 +192,5 @@ void PoseTransform::setTransform(const RigidTransform3d& transform)
m_transformInverse = m_transform.inverse();
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/DeviceFilters/PoseTransform.h b/SurgSim/Devices/DeviceFilters/PoseTransform.h
index b89b386..112e3dc 100644
--- a/SurgSim/Devices/DeviceFilters/PoseTransform.h
+++ b/SurgSim/Devices/DeviceFilters/PoseTransform.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -21,17 +21,17 @@
#include <string>
#include "SurgSim/DataStructures/DataGroup.h"
-#include "SurgSim/Input/CommonDevice.h"
-#include "SurgSim/Input/InputConsumerInterface.h"
-#include "SurgSim/Input/OutputProducerInterface.h"
+#include "SurgSim/Devices/DeviceFilters/DeviceFilter.h"
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Vector.h"
namespace SurgSim
{
-
-namespace Device
+namespace Devices
{
+
+SURGSIM_STATIC_REGISTRATION(PoseTransform);
+
/// 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
@@ -50,46 +50,17 @@ namespace Device
/// \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
+class PoseTransform : public DeviceFilter
{
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;
+ SURGSIM_CLASSNAME(SurgSim::Devices::PoseTransform);
+
+ /// \return Get the translation scale factor.
+ double getTranslationScale() const;
/// Set the translation scale factor so that each direction has the same scale.
/// \param translationScale The scalar scaling factor.
@@ -97,65 +68,36 @@ public:
/// transform even if the following output data is based off input data that used the old transform.
void setTranslationScale(double translationScale);
+ /// \return The current transform.
+ const Math::RigidTransform3d& getTransform() const;
+
/// 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);
+ void setTransform(const 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);
+ void filterInput(const std::string& device, const DataStructures::DataGroup& dataToFilter,
+ DataStructures::DataGroup* result) override;
+
+ void filterOutput(const std::string& device, const DataStructures::DataGroup& dataToFilter,
+ DataStructures::DataGroup* result) override;
/// The mutex that protects the transform and scaling factor.
boost::mutex m_mutex;
/// The constant pre-transform.
- SurgSim::Math::RigidTransform3d m_transform;
+ Math::RigidTransform3d m_transform;
/// The inverse of the pre-transform.
- SurgSim::Math::RigidTransform3d m_transformInverse;
+ 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 Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_DEVICEFILTERS_POSETRANSFORM_H
diff --git a/SurgSim/Devices/DeviceFilters/RecordPose.cpp b/SurgSim/Devices/DeviceFilters/RecordPose.cpp
new file mode 100644
index 0000000..44f5c6c
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/RecordPose.cpp
@@ -0,0 +1,90 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/RecordPose.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Math::RigidTransform3d;
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::RecordPose, RecordPose);
+
+RecordPose::RecordPose(const std::string& name) :
+ DeviceFilter(name),
+ m_cumulativeTime(0),
+ m_fileName("ReplayPoseDevice.txt")
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RecordPose, std::string, FileName, getFileName, setFileName);
+ m_timer.setMaxNumberOfFrames(1);
+ m_timer.start();
+}
+
+RecordPose::~RecordPose()
+{
+ if (m_outputFile.is_open())
+ {
+ m_outputFile.close();
+ }
+}
+
+void RecordPose::setFileName(const std::string& fileName)
+{
+ m_fileName = fileName;
+}
+
+const std::string& RecordPose::getFileName() const
+{
+ return m_fileName;
+}
+
+void RecordPose::initializeInput(const std::string& device, const DataStructures::DataGroup& inputData)
+{
+ if (!m_outputFile.is_open())
+ {
+ m_outputFile.open(m_fileName, std::ios::out | std::ios::trunc);
+ if (!m_outputFile.is_open())
+ {
+ SURGSIM_LOG_IF(!m_outputFile.is_open(), Framework::Logger::getLogger("Devices/RecordPose"), WARNING) <<
+ "File " << m_fileName << " could not be open to record device pose";
+ }
+ }
+}
+
+void RecordPose::filterInput(const std::string& device, const DataGroup& dataToFilter, DataGroup* result)
+{
+ *result = dataToFilter;
+
+ if (m_outputFile.is_open())
+ {
+ RigidTransform3d pose;
+ if (dataToFilter.poses().get(DataStructures::Names::POSE, &pose))
+ {
+ m_timer.markFrame();
+ m_cumulativeTime += m_timer.getLastFramePeriod();
+ // We back up the time along with the pose to make sure we can replay the motion real-time, no matter
+ // which rate it is recorded and replayed.
+ m_outputFile << m_cumulativeTime << std::endl << pose.matrix() << std::endl;
+ }
+ }
+}
+
+}; // namespace Devices
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/DeviceFilters/RecordPose.h b/SurgSim/Devices/DeviceFilters/RecordPose.h
new file mode 100644
index 0000000..95d8e7b
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/RecordPose.h
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_RECORDPOSE_H
+#define SURGSIM_DEVICES_DEVICEFILTERS_RECORDPOSE_H
+
+#include <string>
+
+#include "SurgSim/Devices/DeviceFilters/DeviceFilter.h"
+#include "SurgSim/Framework/Timer.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+SURGSIM_STATIC_REGISTRATION(RecordPose);
+
+/// An input device filter that record the input pose along with the relative time. All entries in the DataGroup are
+/// passed through. For convenience, it is also an OutputProducerInterface that does no filtering of the ouput data.
+class RecordPose : public DeviceFilter
+{
+public:
+ /// Constructor.
+ /// \param name Name of this device filter.
+ explicit RecordPose(const std::string& name);
+
+ /// Desctructor
+ ~RecordPose();
+
+ /// \param fileName The filename to record the pose/time to (default is empty string)
+ void setFileName(const std::string& fileName);
+
+ /// \return The filename where the pose/time are recorded (default is empty string)
+ const std::string& getFileName() const;
+
+ void initializeInput(const std::string& device, const DataStructures::DataGroup& inputData) override;
+
+ SURGSIM_CLASSNAME(SurgSim::Devices::RecordPose);
+
+private:
+ void filterInput(const std::string& device, const DataStructures::DataGroup& dataToFilter,
+ DataStructures::DataGroup* result) override;
+
+ /// Timer to keep the recording real-time
+ Framework::Timer m_timer;
+
+ /// Cumulative time elapsed since the timer started (on creation of the instance, in ctor)
+ double m_cumulativeTime;
+
+ /// Filename where the poses will be recorded
+ std::string m_fileName;
+
+ /// Output stream to the file 'm_fileName', the entire content is replaced at each run
+ std::ofstream m_outputFile;
+};
+
+}; // namespace Devices
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_DEVICEFILTERS_RECORDPOSE_H
diff --git a/SurgSim/Devices/DeviceFilters/UnitTests/BoolToScalarTest.cpp b/SurgSim/Devices/DeviceFilters/UnitTests/BoolToScalarTest.cpp
new file mode 100644
index 0000000..878a759
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/UnitTests/BoolToScalarTest.cpp
@@ -0,0 +1,214 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 <gtest/gtest.h>
+#include <boost/thread.hpp>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/DeviceFilters/BoolToScalar.h"
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+namespace SurgSim
+{
+
+namespace Devices
+{
+
+using DataStructures::Names::BUTTON_0;
+using DataStructures::Names::BUTTON_1;
+using DataStructures::Names::BUTTON_2;
+using DataStructures::Names::TOOLDOF;
+
+class BoolToScalarFilterTest : public testing::Test
+{
+public:
+
+ void SetUp()
+ {
+ // values for default buttons
+ builder.addBoolean(BUTTON_1);
+ builder.addBoolean(BUTTON_2);
+ builder.addScalar(TOOLDOF);
+
+ defaultData = builder.createData();
+ }
+
+ DataStructures::DataGroupBuilder builder;
+ DataStructures::DataGroup defaultData;
+};
+
+TEST_F(BoolToScalarFilterTest, GeneralSetters)
+{
+ auto filter = std::make_shared<BoolToScalar>("Filter");
+
+ filter->setValue("Scalar", 0.5);
+ EXPECT_DOUBLE_EQ(0.5, filter->getValue<double>("Scalar"));
+
+ filter->setValue("Scale", 4.0);
+ EXPECT_DOUBLE_EQ(4.0, filter->getValue<double>("Scale"));
+
+ filter->setValue("Clamping", false);
+ EXPECT_EQ(false, filter->getValue<bool>("Clamping"));
+
+ filter->setValue("IncreaseField", std::string("IncreaseField"));
+ EXPECT_EQ("IncreaseField", filter->getValue<std::string>("IncreaseField"));
+ filter->setValue("DecreaseField", "DecreaseField");
+ EXPECT_EQ("DecreaseField", filter->getValue<std::string>("DecreaseField"));
+ filter->setValue("TargetField", "TargetField");
+ EXPECT_EQ("TargetField", filter->getValue<std::string>("TargetField"));
+}
+
+TEST_F(BoolToScalarFilterTest, SetRange)
+{
+ auto filter = std::make_shared<BoolToScalar>("Filter");
+ auto expectedRange = std::make_pair(5.0, 6.0);
+ filter->setValue("Range", expectedRange);
+ auto resultRange = filter->getValue<std::pair<double, double>>("Range");
+ EXPECT_DOUBLE_EQ(expectedRange.first, resultRange.first);
+ EXPECT_DOUBLE_EQ(expectedRange.second, resultRange.second);
+
+ // Reverse the range if it's [max, min]
+ auto inputRange = std::make_pair(10.0, 1.0);
+ expectedRange = std::make_pair(1.0, 10.0);
+ filter->setRange(inputRange);
+ resultRange = filter->getRange();
+ EXPECT_DOUBLE_EQ(expectedRange.first, resultRange.first);
+ EXPECT_DOUBLE_EQ(expectedRange.second, resultRange.second);
+}
+
+TEST_F(BoolToScalarFilterTest, DataGroupHandling)
+{
+ {
+ SCOPED_TRACE("Valid Data");
+ auto filter = std::make_shared<BoolToScalar>("Filter");
+ EXPECT_NO_THROW(filter->initializeInput("device", defaultData));
+ }
+ {
+ SCOPED_TRACE("Invalid IncreaseField");
+ auto filter = std::make_shared<BoolToScalar>("Filter");
+ filter->setIncreaseField("wrongIncrease");
+ EXPECT_ANY_THROW(filter->initializeInput("device", defaultData));
+ }
+ {
+ SCOPED_TRACE("Invalid DecreaseField");
+ auto filter = std::make_shared<BoolToScalar>("Filter");
+ filter->setDecreaseField("wrongDecrease");
+ EXPECT_ANY_THROW(filter->initializeInput("device", defaultData));
+ }
+ {
+ SCOPED_TRACE("Append Target");
+ DataStructures::DataGroupBuilder builder;
+ builder.addBoolean(BUTTON_1);
+ builder.addBoolean(BUTTON_2);
+ auto data = builder.createData();
+ auto filter = std::make_shared<BoolToScalar>("Filter");
+ EXPECT_NO_THROW(filter->initializeInput("device", data));
+ }
+}
+
+TEST_F(BoolToScalarFilterTest, UpdateTest)
+{
+ const double epsilon = 0.05;
+
+ auto filter = std::make_shared<BoolToScalar>("Filter");
+ filter->setScale(2.0);
+ auto resultData = defaultData;
+ defaultData.booleans().set(BUTTON_1, false);
+ defaultData.booleans().set(BUTTON_2, true);
+ double value;
+ filter->initializeInput("device", defaultData);
+
+ // Running .25 of a second we expect the value to go up .. by at least 0.25 * 2 (scale)
+ boost::this_thread::sleep(boost::posix_time::milliseconds(250));
+ filter->filterInput("device", defaultData, &resultData);
+ resultData.scalars().get(TOOLDOF, &value);
+ EXPECT_LT(0.5 - epsilon, value);
+
+ // Switiching the buttons, running for .125 of a second so the value should go down by at least 0.125 * 2 (scale)
+ defaultData.booleans().set(BUTTON_1, true);
+ defaultData.booleans().set(BUTTON_2, false);
+ // prevent accumulated error in the test
+ filter->setScalar(0.5);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(125));
+ filter->filterInput("device", defaultData, &resultData);
+ resultData.scalars().get(TOOLDOF, &value);
+ EXPECT_GT(0.25 + epsilon, value);
+}
+
+TEST_F(BoolToScalarFilterTest, ClampTest)
+{
+ auto filter = std::make_shared<BoolToScalar>("Filter");
+ filter->setScale(10.0);
+ auto resultData = defaultData;
+ defaultData.booleans().set(BUTTON_1, false);
+ defaultData.booleans().set(BUTTON_2, true);
+ double value;
+ filter->initializeInput("device", defaultData);
+
+ // Running .25 of a second we expect the value to go up .. by 0.25 * 10 (scale)
+ // but it's clamped to [0,1]
+ boost::this_thread::sleep(boost::posix_time::milliseconds(250));
+ filter->filterInput("device", defaultData, &resultData);
+ resultData.scalars().get(TOOLDOF, &value);
+ EXPECT_DOUBLE_EQ(1.0, value);
+
+ // Switiching the buttons, running for .25 of a second so the value should go down by 0.25 * 10 (scale)
+ // but it's clamped to [0,1]
+ defaultData.booleans().set(BUTTON_1, true);
+ defaultData.booleans().set(BUTTON_2, false);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(250));
+ filter->filterInput("device", defaultData, &resultData);
+ resultData.scalars().get(TOOLDOF, &value);
+ EXPECT_DOUBLE_EQ(0.0, value);
+}
+
+TEST_F(BoolToScalarFilterTest, UnClampedTest)
+{
+ auto filter = std::make_shared<BoolToScalar>("Filter");
+ filter->setScale(10.0);
+ filter->setClamping(false);
+ auto resultData = defaultData;
+ defaultData.booleans().set(BUTTON_1, false);
+ defaultData.booleans().set(BUTTON_2, true);
+ double value;
+ filter->initializeInput("device", defaultData);
+
+ // Running .25 of a second we expect the value to go up .. by 0.25 * 10 (scale)
+ // but it's not clamped
+ boost::this_thread::sleep(boost::posix_time::milliseconds(250));
+ filter->filterInput("device", defaultData, &resultData);
+ resultData.scalars().get(TOOLDOF, &value);
+ EXPECT_TRUE(value > 2.0);
+
+ // Switiching the buttons, running for .25 of a second so the value should go down by 0.25 * 10 (scale)
+ // but it's clamped to [0,]1
+ filter->setScalar(0.5);
+ defaultData.booleans().set(BUTTON_1, true);
+ defaultData.booleans().set(BUTTON_2, false);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(250));
+ filter->filterInput("device", defaultData, &resultData);
+ resultData.scalars().get(TOOLDOF, &value);
+ EXPECT_TRUE(value < 0.0);
+}
+
+
+}
+}
+
diff --git a/SurgSim/Devices/DeviceFilters/UnitTests/CMakeLists.txt b/SurgSim/Devices/DeviceFilters/UnitTests/CMakeLists.txt
index 2546057..70fb26a 100644
--- a/SurgSim/Devices/DeviceFilters/UnitTests/CMakeLists.txt
+++ b/SurgSim/Devices/DeviceFilters/UnitTests/CMakeLists.txt
@@ -18,16 +18,31 @@ include_directories(
)
set(UNIT_TEST_SOURCES
+ BoolToScalarTest.cpp
+ DeviceFilterTest.cpp
+ FilteredDeviceTest.cpp
ForceScaleTest.cpp
PoseIntegratorTest.cpp
PoseTransformTest.cpp
)
set(LIBS
- IdentityPoseDevice
- SurgSimDeviceFilters
+ SurgSimDataStructures
+ SurgSimDevices
+ SurgSimFramework
+ SurgSimInput
+ SurgSimMath
+ ${Boost_LIBRARIES}
)
surgsim_add_unit_tests(SurgSimDeviceFiltersTest)
set_target_properties(SurgSimDeviceFiltersTest PROPERTIES FOLDER "Devices")
+
+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"
+)
diff --git a/SurgSim/Devices/DeviceFilters/UnitTests/Data/FilteredDevice.yaml b/SurgSim/Devices/DeviceFilters/UnitTests/Data/FilteredDevice.yaml
new file mode 100644
index 0000000..ca9d822
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/UnitTests/Data/FilteredDevice.yaml
@@ -0,0 +1,11 @@
+- SurgSim::Devices::FilteredDevice:
+ Name: ConstantPose
+ Devices:
+ - SurgSim::Devices::IdentityPoseDevice:
+ Name: Identity
+ - SurgSim::Devices::PoseTransform:
+ Name: Transform
+ TranslationScale: 3.4
+ Transform:
+ Quaternion: {Angle: 12.3, Axis: [0.5, 0.5, 0.0]}
+ Translation: [7.8, 8.9, 9.0]
diff --git a/SurgSim/Devices/DeviceFilters/UnitTests/Data/PoseTransform.yaml b/SurgSim/Devices/DeviceFilters/UnitTests/Data/PoseTransform.yaml
new file mode 100644
index 0000000..c936b53
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/UnitTests/Data/PoseTransform.yaml
@@ -0,0 +1,6 @@
+- SurgSim::Devices::PoseTransform:
+ Name: Transform
+ TranslationScale: 3.4
+ Transform:
+ Quaternion: {Angle: 12.3, Axis: [0.5, 0.5, 0.0]}
+ Translation: [7.8, 8.9, 9.0]
diff --git a/SurgSim/Devices/DeviceFilters/UnitTests/DeviceFilterTest.cpp b/SurgSim/Devices/DeviceFilters/UnitTests/DeviceFilterTest.cpp
new file mode 100644
index 0000000..44dbf0c
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/UnitTests/DeviceFilterTest.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.
+
+/// \file
+/// Tests for the DeviceFilter class.
+
+#include <memory>
+#include <string>
+#include <gtest/gtest.h>
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/DeviceFilters/DeviceFilter.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::Devices::DeviceFilter;
+using SurgSim::Testing::MockInputOutput;
+
+/// Exposes protected members of CommonDevice.
+class MockDeviceFilter : public DeviceFilter
+{
+public:
+ explicit MockDeviceFilter(const std::string& name) : DeviceFilter(name)
+ {
+ }
+
+ SurgSim::DataStructures::DataGroup& doGetInputData()
+ {
+ return getInputData();
+ }
+};
+
+TEST(DeviceFilterTest, InputDataFilter)
+{
+ auto filter = std::make_shared<MockDeviceFilter>("filter");
+ ASSERT_TRUE(filter->initialize());
+
+ DataGroupBuilder builder;
+ std::string scalarName = "scalar";
+ builder.addScalar(scalarName);
+ DataGroup data = builder.createData();
+ const double initialScalar = 17.3;
+ data.scalars().set(scalarName, initialScalar);
+
+ // Normally the input device's initial input data would be set by the constructor or scaffold, then
+ // initializeInput would be called in addInputConsumer.
+ filter->initializeInput("device", data);
+
+ DataGroup actualInputData = filter->doGetInputData();
+ double actualScalar;
+ ASSERT_TRUE(actualInputData.scalars().get(scalarName, &actualScalar));
+ EXPECT_EQ(initialScalar, actualScalar);
+}
+
+TEST(DeviceFilterTest, OutputDataFilter)
+{
+ auto filter = std::make_shared<MockDeviceFilter>("filter");
+ ASSERT_TRUE(filter->initialize());
+
+ DataGroupBuilder builder;
+ std::string scalarName = "scalar";
+ builder.addScalar(scalarName);
+ DataGroup data = builder.createData();
+ const double initialScalar = 17.3;
+ data.scalars().set(scalarName, initialScalar);
+
+ // 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.
+ filter->setOutputProducer(producer);
+
+ DataGroup actualData;
+ ASSERT_TRUE(filter->requestOutput("device", &actualData));
+
+ double actualScalar;
+ ASSERT_TRUE(actualData.scalars().get(scalarName, &actualScalar));
+ EXPECT_EQ(initialScalar, actualScalar);
+}
diff --git a/SurgSim/Devices/DeviceFilters/UnitTests/FilteredDeviceTest.cpp b/SurgSim/Devices/DeviceFilters/UnitTests/FilteredDeviceTest.cpp
new file mode 100644
index 0000000..d192895
--- /dev/null
+++ b/SurgSim/Devices/DeviceFilters/UnitTests/FilteredDeviceTest.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 FilteredDevice class.
+
+#include <memory>
+#include <string>
+#include <gtest/gtest.h>
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/Devices.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::Devices::FilteredDevice;
+using SurgSim::Testing::MockInputOutput;
+
+TEST(FilteredDeviceTest, WithFilters)
+{
+ std::string name = "device";
+ auto filteredDevice = std::make_shared<FilteredDevice>(name);
+ EXPECT_EQ(name, filteredDevice->getName());
+ ASSERT_FALSE(filteredDevice->initialize());
+
+ auto inputOutput = std::make_shared<MockInputOutput>();
+ EXPECT_FALSE(filteredDevice->addInputConsumer(inputOutput));
+ EXPECT_FALSE(filteredDevice->removeInputConsumer(inputOutput));
+ EXPECT_FALSE(filteredDevice->setOutputProducer(inputOutput));
+ EXPECT_FALSE(filteredDevice->removeOutputProducer(inputOutput));
+
+ ASSERT_ANY_THROW(filteredDevice->setDevice(nullptr));
+ std::string subDeviceName = "identity";
+ ASSERT_NO_THROW(filteredDevice->setDevice(std::make_shared<SurgSim::Devices::IdentityPoseDevice>(subDeviceName)));
+
+ ASSERT_ANY_THROW(filteredDevice->addFilter(nullptr));
+ filteredDevice->addFilter(std::make_shared<SurgSim::Devices::PoseTransform>("filter1"));
+ filteredDevice->addFilter(std::make_shared<SurgSim::Devices::PoseTransform>("filter2"));
+ EXPECT_TRUE(filteredDevice->initialize());
+
+ EXPECT_ANY_THROW(filteredDevice->initialize());
+ EXPECT_ANY_THROW(filteredDevice->setDevice(std::make_shared<SurgSim::Devices::IdentityPoseDevice>("identity2")));
+ EXPECT_ANY_THROW(filteredDevice->addFilter(std::make_shared<SurgSim::Devices::PoseTransform>("filter3")));
+
+ EXPECT_TRUE(filteredDevice->addInputConsumer(inputOutput));
+ EXPECT_TRUE(filteredDevice->removeInputConsumer(inputOutput));
+
+ EXPECT_TRUE(filteredDevice->setOutputProducer(inputOutput));
+ EXPECT_TRUE(filteredDevice->hasOutputProducer());
+ EXPECT_TRUE(filteredDevice->removeOutputProducer(inputOutput));
+ EXPECT_FALSE(filteredDevice->hasOutputProducer());
+}
+
+TEST(FilteredDeviceTest, GetSetDevices)
+{
+ std::vector<std::shared_ptr<SurgSim::Input::DeviceInterface>> devices;
+ std::string subDeviceName = "identity";
+ devices.push_back(std::make_shared<SurgSim::Devices::IdentityPoseDevice>(subDeviceName));
+ devices.push_back(std::make_shared<SurgSim::Devices::PoseTransform>("filter1"));
+ devices.push_back(std::make_shared<SurgSim::Devices::PoseTransform>("filter2"));
+
+ auto filteredDevice = std::make_shared<FilteredDevice>("device");
+ ASSERT_NO_THROW(filteredDevice->setDevices(devices));
+ EXPECT_TRUE(filteredDevice->initialize());
+ ASSERT_ANY_THROW(filteredDevice->setDevices(devices));
+
+ auto actualDevices = filteredDevice->getDevices();
+ EXPECT_EQ(3, devices.size());
+ EXPECT_EQ(subDeviceName, devices[0]->getName());
+ EXPECT_EQ("SurgSim::Devices::IdentityPoseDevice", devices[0]->getClassName());
+}
+
+TEST(FilteredDeviceTest, NoFilters)
+{
+ auto filteredDevice = std::make_shared<FilteredDevice>("device");
+ ASSERT_NO_THROW(filteredDevice->setDevice(std::make_shared<SurgSim::Devices::IdentityPoseDevice>("identity")));
+ auto inputOutput = std::make_shared<MockInputOutput>();
+ EXPECT_TRUE(filteredDevice->addInputConsumer(inputOutput));
+ EXPECT_TRUE(filteredDevice->setOutputProducer(inputOutput));
+ EXPECT_TRUE(filteredDevice->initialize());
+
+ EXPECT_TRUE(filteredDevice->hasOutputProducer());
+ EXPECT_TRUE(filteredDevice->removeInputConsumer(inputOutput));
+ EXPECT_TRUE(filteredDevice->removeOutputProducer(inputOutput));
+ EXPECT_FALSE(filteredDevice->hasOutputProducer());
+}
+
+TEST(FilteredDeviceTest, Serialization)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ std::shared_ptr<SurgSim::Input::DeviceInterface> device;
+ ASSERT_NO_THROW(device = SurgSim::Devices::loadDevice("FilteredDevice.yaml"));
+ ASSERT_NE(nullptr, device);
+ auto typedDevice = std::dynamic_pointer_cast<FilteredDevice>(device);
+ ASSERT_NE(nullptr, typedDevice);
+
+ auto input = std::make_shared<MockInputOutput>();
+ ASSERT_TRUE(device->addInputConsumer(input));
+ SurgSim::Math::RigidTransform3d pose;
+ ASSERT_TRUE(input->m_lastReceivedInput.poses().get(SurgSim::DataStructures::Names::POSE, &pose));
+
+ Eigen::AngleAxisd angleAxis;
+ angleAxis.angle() = 12.3;
+ angleAxis.axis() = SurgSim::Math::Vector3d(0.5, 0.5, 0.0);
+ SurgSim::Math::Vector3d translation = SurgSim::Math::Vector3d(7.8, 8.9, 9.0);
+ SurgSim::Math::RigidTransform3d expectedTransform =
+ SurgSim::Math::makeRigidTransform(SurgSim::Math::Quaterniond(angleAxis), translation);
+ EXPECT_TRUE(pose.isApprox(expectedTransform));
+}
\ No newline at end of file
diff --git a/SurgSim/Devices/DeviceFilters/UnitTests/ForceScaleTest.cpp b/SurgSim/Devices/DeviceFilters/UnitTests/ForceScaleTest.cpp
index 229d086..410f58f 100644
--- a/SurgSim/Devices/DeviceFilters/UnitTests/ForceScaleTest.cpp
+++ b/SurgSim/Devices/DeviceFilters/UnitTests/ForceScaleTest.cpp
@@ -28,7 +28,7 @@
using SurgSim::DataStructures::DataGroup;
using SurgSim::DataStructures::DataGroupBuilder;
-using SurgSim::Device::ForceScale;
+using SurgSim::Devices::ForceScale;
using SurgSim::Input::CommonDevice;
using SurgSim::Math::Matrix66d;
using SurgSim::Math::Vector3d;
diff --git a/SurgSim/Devices/DeviceFilters/UnitTests/PoseIntegratorTest.cpp b/SurgSim/Devices/DeviceFilters/UnitTests/PoseIntegratorTest.cpp
index 62cfb90..08b05c5 100644
--- a/SurgSim/Devices/DeviceFilters/UnitTests/PoseIntegratorTest.cpp
+++ b/SurgSim/Devices/DeviceFilters/UnitTests/PoseIntegratorTest.cpp
@@ -31,7 +31,7 @@
using SurgSim::DataStructures::DataGroup;
using SurgSim::DataStructures::DataGroupBuilder;
-using SurgSim::Device::PoseIntegrator;
+using SurgSim::Devices::PoseIntegrator;
using SurgSim::Input::CommonDevice;
using SurgSim::Math::makeRigidTransform;
using SurgSim::Math::makeRotationQuaternion;
diff --git a/SurgSim/Devices/DeviceFilters/UnitTests/PoseTransformTest.cpp b/SurgSim/Devices/DeviceFilters/UnitTests/PoseTransformTest.cpp
index d63257a..11d2dd8 100644
--- a/SurgSim/Devices/DeviceFilters/UnitTests/PoseTransformTest.cpp
+++ b/SurgSim/Devices/DeviceFilters/UnitTests/PoseTransformTest.cpp
@@ -21,8 +21,10 @@
#include <gtest/gtest.h>
#include "SurgSim/DataStructures/DataGroup.h"
#include "SurgSim/DataStructures/DataGroupBuilder.h"
-#include "SurgSim/Devices/DeviceFilters/PoseTransform.h"
+#include "SurgSim/Devices/Devices.h"
+#include "SurgSim/Framework/Runtime.h"
#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Input/DeviceInterface.h"
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Quaternion.h"
@@ -30,7 +32,7 @@
using SurgSim::DataStructures::DataGroup;
using SurgSim::DataStructures::DataGroupBuilder;
-using SurgSim::Device::PoseTransform;
+using SurgSim::Devices::PoseTransform;
using SurgSim::Input::CommonDevice;
using SurgSim::Math::makeRigidTransform;
using SurgSim::Math::makeRotationQuaternion;
@@ -288,3 +290,23 @@ TEST(PoseTransformDeviceFilterTest, OutputDataFilter)
bool expectedBoolean = true;
EXPECT_EQ(expectedBoolean, actualBoolean);
}
+
+TEST(PoseTransformDeviceFilterTest, Serialization)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ std::string fileName = "PoseTransform.yaml";
+ std::shared_ptr<SurgSim::Input::DeviceInterface> device;
+ ASSERT_NO_THROW(device = SurgSim::Devices::loadDevice(fileName));
+ ASSERT_NE(nullptr, device);
+ auto typedDevice = std::dynamic_pointer_cast<PoseTransform>(device);
+ ASSERT_NE(nullptr, typedDevice);
+ EXPECT_NEAR(3.4, typedDevice->getTranslationScale(), 1e-10);
+
+ Eigen::AngleAxisd angleAxis;
+ angleAxis.angle() = 12.3;
+ angleAxis.axis() = SurgSim::Math::Vector3d(0.5, 0.5, 0.0);
+ SurgSim::Math::Vector3d translation = SurgSim::Math::Vector3d(7.8, 8.9, 9.0);
+ SurgSim::Math::RigidTransform3d expectedTransform =
+ SurgSim::Math::makeRigidTransform(SurgSim::Math::Quaterniond(angleAxis), translation);
+ EXPECT_TRUE(typedDevice->getTransform().isApprox(expectedTransform));
+}
\ No newline at end of file
diff --git a/SurgSim/Blocks/UnitTests/config.txt.in b/SurgSim/Devices/DeviceFilters/UnitTests/config.txt.in
similarity index 100%
copy from SurgSim/Blocks/UnitTests/config.txt.in
copy to SurgSim/Devices/DeviceFilters/UnitTests/config.txt.in
diff --git a/SurgSim/Devices/DeviceUtilities.cpp b/SurgSim/Devices/DeviceUtilities.cpp
new file mode 100644
index 0000000..bffe565
--- /dev/null
+++ b/SurgSim/Devices/DeviceUtilities.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/DeviceUtilities.h"
+
+#include <yaml-cpp/yaml.h>
+
+#include "SurgSim/Devices/DeviceFilters/DeviceFilter.h"
+#include "SurgSim/Devices/DeviceFilters/FilteredDevice.h"
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Runtime.h"
+
+using SurgSim::Input::DeviceInterface;
+
+namespace
+{
+const std::string NamePropertyName = "Name";
+
+/// Convert node to device
+/// \param possibleDevice The node to be converted
+/// \param fileName The original filename for error reporting
+/// \return The device if it was created and initialized, otherwise nullptr
+std::shared_ptr<DeviceInterface> tryConvertDevice(const YAML::Node& possibleDevice, const std::string& fileName)
+{
+ auto logger = SurgSim::Framework::Logger::getLogger("Devices");
+ std::shared_ptr<DeviceInterface> device;
+ if (possibleDevice.IsMap())
+ {
+ std::string className = possibleDevice.begin()->first.as<std::string>();
+ if (DeviceInterface::getFactory().isRegistered(className))
+ {
+ const YAML::Node& data = possibleDevice.begin()->second;
+ if (data.IsMap() && data[NamePropertyName].IsDefined())
+ {
+ SURGSIM_LOG_DEBUG(logger) << "Loading: " << std::endl << possibleDevice;
+ std::string name = data[NamePropertyName].as<std::string>();
+ device = DeviceInterface::getFactory().create(className, name);
+ auto filteredDevice = std::dynamic_pointer_cast<SurgSim::Devices::FilteredDevice>(device);
+ if (filteredDevice == nullptr)
+ {
+ std::vector<std::string> ignoredProperties;
+ ignoredProperties.push_back(NamePropertyName);
+ device->decode(data, ignoredProperties);
+ }
+ else
+ {
+ if (data["Devices"].IsDefined() && data["Devices"].IsSequence())
+ {
+ std::vector<std::shared_ptr<SurgSim::Input::DeviceInterface>> subDevices;
+ for (const YAML::Node& deviceNode : data["Devices"])
+ {
+ subDevices.push_back(tryConvertDevice(deviceNode, fileName));
+ }
+ SURGSIM_LOG_IF(!filteredDevice->setDevices(subDevices), logger, WARNING) <<
+ "File " << fileName << " failed to setDevices on a FilteredDevice : " <<
+ std::endl << data["Devices"];
+ }
+ std::vector<std::string> ignoredProperties;
+ ignoredProperties.push_back(NamePropertyName);
+ ignoredProperties.push_back("Devices");
+ device->decode(data, ignoredProperties);
+ }
+ if (device->initialize())
+ {
+ SURGSIM_LOG_INFO(logger) << "Successfully loaded " << className << " named " << name;
+ }
+ else
+ {
+ device = nullptr;
+ SURGSIM_LOG_INFO(logger) << "Failed to initialize " << className << " named " << name;
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(logger) << "File " << fileName << " contains an entry for class " <<
+ className << " that is not a map or does not have a Name property:" << std::endl << possibleDevice;
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_INFO(logger) << "Class " << className << " is not registered in the factory.";
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(logger) <<
+ "File " << fileName << " contains a non-map where a map is expected:" << std::endl << possibleDevice;
+ }
+ return device;
+}
+}
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+std::shared_ptr<DeviceInterface> createDevice(const std::vector<std::string>& classNames,
+ const std::string& name)
+{
+ std::shared_ptr<DeviceInterface> device;
+ auto& factory = DeviceInterface::getFactory();
+ for (const auto& className : classNames)
+ {
+ if (factory.isRegistered(className))
+ {
+ SURGSIM_LOG_INFO(Framework::Logger::getLogger("Devices")) << "Trying to create a " << className;
+ device = factory.create(className, name);
+ if (device->initialize())
+ {
+ SURGSIM_LOG_INFO(Framework::Logger::getLogger("Devices")) << "Successfully created a " << className;
+ break;
+ }
+ else
+ {
+ device = nullptr;
+ SURGSIM_LOG_INFO(Framework::Logger::getLogger("Devices")) << "Failed to initialize a " << className;
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_INFO(Framework::Logger::getLogger("Devices")) << className <<
+ " is not registered in the Devices factory.";
+ }
+ }
+ return device;
+}
+
+std::shared_ptr<Input::DeviceInterface> createDevice(const std::string& className, const std::string& name)
+{
+ std::vector<std::string> names;
+ names.push_back(className);
+ return createDevice(names, name);
+}
+
+std::shared_ptr<DeviceInterface> loadDevice(const std::string& fileName)
+{
+ SURGSIM_LOG_INFO(Framework::Logger::getLogger("Devices")) << "Adding a device from " << fileName;
+ YAML::Node node;
+ SURGSIM_ASSERT(Framework::tryLoadNode(fileName, &node)) <<
+ "Could not load a device from the YAML file: " << fileName;
+
+ std::shared_ptr<DeviceInterface> device;
+ if (node.IsSequence())
+ {
+ for (const auto& possibleDevice : node)
+ {
+ device = tryConvertDevice(possibleDevice, fileName);
+ if (device != nullptr)
+ {
+ break;
+ }
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(Framework::Logger::getLogger("Devices")) <<
+ "File " << fileName << " not a YAML sequence; cannot load device:" << std::endl << node;
+ }
+ return device;
+}
+
+}; // namespace Devices
+}; // namespace SurgSim
+
diff --git a/SurgSim/Devices/DeviceUtilities.h b/SurgSim/Devices/DeviceUtilities.h
new file mode 100644
index 0000000..9fe3628
--- /dev/null
+++ b/SurgSim/Devices/DeviceUtilities.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_DEVICES_DEVICEUTILITIES_H
+#define SURGSIM_DEVICES_DEVICEUTILITIES_H
+
+#include <memory>
+#include <string>
+#include <vector>
+
+
+namespace SurgSim
+{
+namespace Input
+{
+class DeviceInterface;
+}
+
+namespace Devices
+{
+
+/// Attempts to create and initialize a device from a list of devices to try
+/// \param classNames The fully-qualified device class names to try. The elements will be tried in order, front to back.
+/// \param name The name to be given to the created device
+/// \return An initialized device or nullptr if no device was available.
+std::shared_ptr<Input::DeviceInterface> createDevice(const std::vector<std::string>& classNames,
+ const std::string& name);
+
+/// Attempts to create and initialize a device
+/// \param className The fully-qualified device class name to try.
+/// \param name The name to be given to the created device
+/// \return An initialized device or nullptr if no device was available.
+std::shared_ptr<Input::DeviceInterface> createDevice(const std::string& className, const std::string& name);
+
+/// Loads a single device from the file, the first device that successfully initializes.
+/// The file format should be just a list of DeviceInterfaces
+/// \code
+/// - SurgSim::Devices::MultiAxisDevice:
+/// Name: element1
+/// - SurgSim::Devices::IdentityPoseDevice:
+/// Name: element1
+/// \endcode
+/// \param fileName the filename of the devices to be loaded, needs to be found
+/// \return an initialized device, or nullptr if no device could be created and initialized
+/// \throws If the file cannot be found or is an invalid file
+std::shared_ptr<Input::DeviceInterface> loadDevice(const std::string& fileName);
+
+};
+};
+
+#endif // SURGSIM_DEVICES_DEVICEUTILITIES_H
+
+
diff --git a/SurgSim/Devices/IdentityPoseDevice/CMakeLists.txt b/SurgSim/Devices/IdentityPoseDevice/CMakeLists.txt
index 4d3af9c..b32abe0 100644
--- a/SurgSim/Devices/IdentityPoseDevice/CMakeLists.txt
+++ b/SurgSim/Devices/IdentityPoseDevice/CMakeLists.txt
@@ -27,11 +27,12 @@ set(IDENTITY_POSE_DEVICE_HEADERS
IdentityPoseDevice.h
)
+set(DEVICE_HEADERS ${DEVICE_HEADERS} IdentityPoseDevice/IdentityPoseDevice.h PARENT_SCOPE)
+
surgsim_add_library(
IdentityPoseDevice
"${IDENTITY_POSE_DEVICE_SOURCES}"
"${IDENTITY_POSE_DEVICE_HEADERS}"
- "SurgSim/Devices/IdentityPoseDevice"
)
set(LIBS
diff --git a/SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.cpp b/SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.cpp
index b973683..26a4f29 100644
--- a/SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.cpp
+++ b/SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.cpp
@@ -15,56 +15,60 @@
#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;
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/RigidTransform.h"
using SurgSim::DataStructures::DataGroup;
using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::Math::RigidTransform3d;
namespace SurgSim
{
-namespace Device
+namespace Devices
{
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::IdentityPoseDevice, IdentityPoseDevice);
IdentityPoseDevice::IdentityPoseDevice(const std::string& uniqueName) :
- SurgSim::Input::CommonDevice(uniqueName, buildInputData())
+ Input::CommonDevice(uniqueName, buildInputData()),
+ m_initialized(false)
{
}
bool IdentityPoseDevice::initialize()
{
- // required by the DeviceInterface API
+ SURGSIM_ASSERT(!isInitialized()) << getName() << " already initialized.";
+ SURGSIM_LOG_INFO(Framework::Logger::getLogger("Devices/IdentityPose")) << "Device " << getName() << " initialized.";
+ m_initialized = true;
return true;
}
+bool IdentityPoseDevice::isInitialized() const
+{
+ return m_initialized;
+}
+
bool IdentityPoseDevice::finalize()
{
- // required by the DeviceInterface API
+ SURGSIM_ASSERT(isInitialized()) << getName() << " is not initialized, cannot finalize.";
+ SURGSIM_LOG_INFO(Framework::Logger::getLogger("Devices/IdentityPose")) << "Device " << getName() << " finalized.";
+ m_initialized = false;
return true;
}
DataGroup IdentityPoseDevice::buildInputData()
{
DataGroupBuilder builder;
- builder.addPose(SurgSim::DataStructures::Names::POSE);
- builder.addBoolean(SurgSim::DataStructures::Names::BUTTON_0);
+ builder.addPose(DataStructures::Names::POSE);
return builder.createData();
}
-bool IdentityPoseDevice::addInputConsumer(std::shared_ptr<SurgSim::Input::InputConsumerInterface> inputConsumer)
+bool IdentityPoseDevice::addInputConsumer(std::shared_ptr<Input::InputConsumerInterface> inputConsumer)
{
- if (! CommonDevice::addInputConsumer(std::move(inputConsumer)))
+ if (!CommonDevice::addInputConsumer(std::move(inputConsumer)))
{
return false;
}
@@ -72,13 +76,12 @@ bool IdentityPoseDevice::addInputConsumer(std::shared_ptr<SurgSim::Input::InputC
// 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);
+ getInputData().poses().set(DataStructures::Names::POSE, RigidTransform3d::Identity());
pushInput();
return true;
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h b/SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h
index c933958..762695a 100644
--- a/SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h
+++ b/SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h
@@ -23,9 +23,9 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
-
+SURGSIM_STATIC_REGISTRATION(IdentityPoseDevice);
/// A class implementing the identity pose device, which is a pretend device that doesn't move.
///
@@ -42,19 +42,27 @@ public:
/// \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);
+ SURGSIM_CLASSNAME(SurgSim::Devices::IdentityPoseDevice);
-protected:
- virtual bool initialize();
+ bool addInputConsumer(std::shared_ptr<SurgSim::Input::InputConsumerInterface> inputConsumer) override;
+
+ bool initialize() override;
- virtual bool finalize();
+ bool isInitialized() const override;
+protected:
/// Builds the data layout for the application input (i.e. device output).
static SurgSim::DataStructures::DataGroup buildInputData();
+
+private:
+ bool finalize() override;
+
+ /// true if initialized and not finalized.
+ bool m_initialized;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_IDENTITYPOSEDEVICE_IDENTITYPOSEDEVICE_H
diff --git a/SurgSim/Devices/IdentityPoseDevice/UnitTests/IdentityPoseDeviceTest.cpp b/SurgSim/Devices/IdentityPoseDevice/UnitTests/IdentityPoseDeviceTest.cpp
index 3859f50..08898db 100644
--- a/SurgSim/Devices/IdentityPoseDevice/UnitTests/IdentityPoseDeviceTest.cpp
+++ b/SurgSim/Devices/IdentityPoseDevice/UnitTests/IdentityPoseDeviceTest.cpp
@@ -25,7 +25,7 @@
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Testing/MockInputOutput.h"
-using SurgSim::Device::IdentityPoseDevice;
+using SurgSim::Devices::IdentityPoseDevice;
using SurgSim::DataStructures::DataGroup;
using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Matrix44d;
@@ -43,6 +43,14 @@ TEST(IdentityPoseDeviceTest, Name)
EXPECT_EQ("MyIdentityPoseDevice", device.getName());
}
+TEST(IdentityPoseDeviceTest, Factory)
+{
+ std::shared_ptr<SurgSim::Input::DeviceInterface> device;
+ ASSERT_NO_THROW(device = SurgSim::Input::DeviceInterface::getFactory().create(
+ "SurgSim::Devices::IdentityPoseDevice", "Device"));
+ EXPECT_NE(nullptr, device);
+}
+
TEST(IdentityPoseDeviceTest, AddInputConsumer)
{
IdentityPoseDevice device("MyIdentityPoseDevice");
@@ -51,19 +59,15 @@ TEST(IdentityPoseDeviceTest, AddInputConsumer)
EXPECT_TRUE(device.addInputConsumer(consumer));
- // IdentityPoseDevice is supposed to shove an identity pose (and a button) at every consumer when it's added.
+ // IdentityPoseDevice is supposed to shove an identity pose 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));
+ 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));
@@ -76,7 +80,7 @@ TEST(IdentityPoseDeviceTest, AddInputConsumer)
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;
+ "consumer->m_numTimesReceivedInput = " << consumer->m_numTimesReceivedInput << std::endl;
}
TEST(IdentityPoseDeviceTest, RemoveInputConsumer)
@@ -89,7 +93,7 @@ TEST(IdentityPoseDeviceTest, RemoveInputConsumer)
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.
+ // IdentityPoseDevice is supposed to shove an identity pose at every consumer when it's added.
EXPECT_EQ(1, consumer->m_numTimesReceivedInput);
EXPECT_TRUE(device.removeInputConsumer(consumer));
diff --git a/SurgSim/Devices/Keyboard/CMakeLists.txt b/SurgSim/Devices/Keyboard/CMakeLists.txt
index fc4743c..77a8932 100644
--- a/SurgSim/Devices/Keyboard/CMakeLists.txt
+++ b/SurgSim/Devices/Keyboard/CMakeLists.txt
@@ -31,15 +31,21 @@ set(KEYBOARD_DEVICE_HEADERS
OsgKeyboardHandler.h
)
+set(DEVICE_HEADERS ${DEVICE_HEADERS}
+ Keyboard/KeyboardDevice.h #NOLINT
+ Keyboard/KeyCode.h #NOLINT
+ PARENT_SCOPE)
+
surgsim_add_library(
KeyboardDevice
- "${KEYBOARD_DEVICE_SOURCES}" "${KEYBOARD_DEVICE_HEADERS}"
- SurgSim/Devices/Keyboard
+ "${KEYBOARD_DEVICE_SOURCES}"
+ "${KEYBOARD_DEVICE_HEADERS}"
)
set(LIBS
SurgSimInput
${Boost_LIBRARIES}
+ ${OPENSCENEGRAPH_LIBRARIES}
)
target_link_libraries(KeyboardDevice ${LIBS}
diff --git a/SurgSim/Devices/Keyboard/KeyCode.h b/SurgSim/Devices/Keyboard/KeyCode.h
index 30b590e..e70fdaf 100644
--- a/SurgSim/Devices/Keyboard/KeyCode.h
+++ b/SurgSim/Devices/Keyboard/KeyCode.h
@@ -18,7 +18,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
// KeyCode pulled out from osgGA/GUIEventAdapter
enum KeyCode
@@ -243,7 +243,7 @@ namespace Device
MODKEY_CAPS_ALT_L = (MODKEY_CAPS_LOCK|MODKEY_LEFT_ALT),
MODKEY_CAPS_ALT_R = (MODKEY_CAPS_LOCK|MODKEY_RIGHT_ALT)
};
-}; // namespace Device
+}; // namespace Devices
}; // 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
index eb65afc..92818da 100644
--- a/SurgSim/Devices/Keyboard/KeyboardDevice.cpp
+++ b/SurgSim/Devices/Keyboard/KeyboardDevice.cpp
@@ -21,11 +21,13 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::KeyboardDevice, KeyboardDevice);
+
KeyboardDevice::KeyboardDevice(const std::string& deviceName) :
- SurgSim::Input::CommonDevice(deviceName, KeyboardScaffold::buildDeviceInputData())
+ Input::CommonDevice(deviceName, KeyboardScaffold::buildDeviceInputData())
{
}
@@ -39,23 +41,24 @@ KeyboardDevice::~KeyboardDevice()
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;
+ SURGSIM_ASSERT(!isInitialized()) << getName() << " already initialized.";
+ auto scaffold = KeyboardScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold != nullptr);
+ bool registered = false;
+ if (scaffold->registerDevice(this))
+ {
+ m_scaffold = std::move(scaffold);
+ registered = true;
+ }
+ return registered;
}
bool KeyboardDevice::finalize()
{
- SURGSIM_ASSERT(isInitialized());
- SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Finalizing.";
+ SURGSIM_ASSERT(isInitialized()) << getName() << " is not initialized, cannot finalize.";
+ bool unregistered = m_scaffold->unregisterDevice();
m_scaffold.reset();
- return true;
+ return unregistered;
}
bool KeyboardDevice::isInitialized() const
@@ -69,5 +72,5 @@ OsgKeyboardHandler* KeyboardDevice::getKeyboardHandler() const
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/Keyboard/KeyboardDevice.h b/SurgSim/Devices/Keyboard/KeyboardDevice.h
index 4f00326..eee088f 100644
--- a/SurgSim/Devices/Keyboard/KeyboardDevice.h
+++ b/SurgSim/Devices/Keyboard/KeyboardDevice.h
@@ -23,11 +23,13 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
class KeyboardScaffold;
class OsgKeyboardHandler;
+SURGSIM_STATIC_REGISTRATION(KeyboardDevice);
+
/// A class implementing the communication with a keyboard
///
/// \par Application input provided from the device:
@@ -50,30 +52,28 @@ public:
/// Constructor
/// \param deviceName Name for keyboard device
explicit KeyboardDevice(const std::string& deviceName);
+
+ SURGSIM_CLASSNAME(SurgSim::Devices::KeyboardDevice);
+
/// 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;
+ bool initialize() 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;
+ bool isInitialized() const override;
/// Get keyboard handler
/// \return The keyboard handler associated with this device
OsgKeyboardHandler* getKeyboardHandler() const;
private:
+ bool finalize() override;
+
/// Communication with hardware is handled by scaffold.
std::shared_ptr<KeyboardScaffold> m_scaffold;
};
-}; // namespace Device
+}; // namespace Devices
}; // 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
index 0e84fa2..ad08454 100644
--- a/SurgSim/Devices/Keyboard/KeyboardScaffold.cpp
+++ b/SurgSim/Devices/Keyboard/KeyboardScaffold.cpp
@@ -24,12 +24,9 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
-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
{
@@ -53,29 +50,31 @@ private:
DeviceData& operator=(const DeviceData&) /*= delete*/;
};
-KeyboardScaffold::KeyboardScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) : m_logger(logger)
+KeyboardScaffold::KeyboardScaffold() : m_logger(Framework::Logger::getLogger("Devices/Keyboard"))
{
- 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.";
+ SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold created.";
}
KeyboardScaffold::~KeyboardScaffold()
{
- unregisterDevice();
+ if (m_device != nullptr)
+ {
+ unregisterDevice();
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold destroyed.";
}
bool KeyboardScaffold::registerDevice(KeyboardDevice* device)
{
+ SURGSIM_ASSERT(m_device == nullptr) << "Can't register two Keyboard devices.";
+
m_device.reset(new DeviceData(device));
if (m_device == nullptr)
{
- SURGSIM_LOG_CRITICAL(m_logger) << "KeyboardScaffold::registerDevice(): failed to create a DeviceData";
+ SURGSIM_LOG_CRITICAL(m_logger) << "Failed to create a DeviceData";
return false;
}
+ SURGSIM_LOG_DEBUG(m_logger) << "Registered device " << device->getName();
return true;
}
@@ -84,16 +83,17 @@ bool KeyboardScaffold::unregisterDevice()
m_device.reset();
if (m_device == nullptr)
{
- SURGSIM_LOG_DEBUG(m_logger) << "Keyboard: Shared scaffold unregistered.";
+ SURGSIM_LOG_DEBUG(m_logger) << "Unregistered device";
return true;
}
+ SURGSIM_LOG_DEBUG(m_logger) << "There is no device to unregister.";
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();
+ DataStructures::DataGroup& inputData = m_device->deviceObject->getInputData();
inputData.integers().set("key", key);
inputData.integers().set("modifierMask", modifierMask);
@@ -106,11 +106,9 @@ 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()
+DataStructures::DataGroup KeyboardScaffold::buildDeviceInputData()
{
- DataGroupBuilder builder;
+ DataStructures::DataGroupBuilder builder;
builder.addInteger("key");
builder.addInteger("modifierMask");
return builder.createData();
@@ -118,21 +116,9 @@ SurgSim::DataStructures::DataGroup KeyboardScaffold::buildDeviceInputData()
std::shared_ptr<KeyboardScaffold> KeyboardScaffold::getOrCreateSharedInstance()
{
- static SurgSim::Framework::SharedInstance<KeyboardScaffold> sharedInstance;
+ static 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 Devices
}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/Keyboard/KeyboardScaffold.h b/SurgSim/Devices/Keyboard/KeyboardScaffold.h
index 70e4700..9451c57 100644
--- a/SurgSim/Devices/Keyboard/KeyboardScaffold.h
+++ b/SurgSim/Devices/Keyboard/KeyboardScaffold.h
@@ -28,13 +28,13 @@ namespace DataStructures
class DataGroup;
}
-namespace Device
+namespace Devices
{
class KeyboardDevice;
class OsgKeyboardHandler;
/// A class that implements the behavior of KeyboardDevice objects.
-/// \sa SurgSim::Device::KeyboardDevice
+/// \sa SurgSim::Devices::KeyboardDevice
class KeyboardScaffold
{
friend class KeyboardDevice;
@@ -43,26 +43,15 @@ class KeyboardScaffold
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);
+ KeyboardScaffold();
/// 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.
+ /// The scaffold is managed using a SharedInstance 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;
@@ -72,6 +61,7 @@ private:
/// \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.
@@ -90,16 +80,13 @@ private:
/// 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 Devices
}; // namespace SurgSim
-#endif // SURGSIM_DEVICES_KEYBOARD_KEYBOARDSCAFFOLD_H
\ No newline at end of file
+#endif // SURGSIM_DEVICES_KEYBOARD_KEYBOARDSCAFFOLD_H
diff --git a/SurgSim/Devices/Keyboard/OsgKeyboardHandler.cpp b/SurgSim/Devices/Keyboard/OsgKeyboardHandler.cpp
index a66f006..a0694a2 100644
--- a/SurgSim/Devices/Keyboard/OsgKeyboardHandler.cpp
+++ b/SurgSim/Devices/Keyboard/OsgKeyboardHandler.cpp
@@ -23,7 +23,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
OsgKeyboardHandler::OsgKeyboardHandler() : m_keyboardScaffold(KeyboardScaffold::getOrCreateSharedInstance())
@@ -34,27 +34,40 @@ bool OsgKeyboardHandler::handle(const osgGA::GUIEventAdapter& ea, osgGA::GUIActi
{
bool result = false;
- switch(ea.getEventType())
+ 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::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;
}
- case(osgGA::GUIEventAdapter::KEYUP) :
+
+ // We wan to to support some of the osg viewer keys, pass these through, we will still receive the event, but osg
+ // will also react here
+ int key = ea.getUnmodifiedKey();
+ if (key == 's' || key == 't' || key == 'v' || key == 'w')
{
- m_keyboardScaffold.lock()->updateDevice(KeyCode::NONE, ModKeyMask::MODKEY_NONE);
- result = true;
- break;
- }
- default:
+ // s: Stats Display
+ // t: texturing
+ // v: vsync
+ // w: wireframe
result = false;
- break;
}
return result;
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/Keyboard/OsgKeyboardHandler.h b/SurgSim/Devices/Keyboard/OsgKeyboardHandler.h
index 3ef6541..ff32d8b 100644
--- a/SurgSim/Devices/Keyboard/OsgKeyboardHandler.h
+++ b/SurgSim/Devices/Keyboard/OsgKeyboardHandler.h
@@ -22,7 +22,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
class KeyboardScaffold;
@@ -37,14 +37,14 @@ public:
/// \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;
+ 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 Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_KEYBOARD_OSGKEYBOARDHANDLER_H
\ No newline at end of file
diff --git a/SurgSim/Devices/Keyboard/UnitTests/KeyboardDeviceTest.cpp b/SurgSim/Devices/Keyboard/UnitTests/KeyboardDeviceTest.cpp
index 3a251f6..440386e 100644
--- a/SurgSim/Devices/Keyboard/UnitTests/KeyboardDeviceTest.cpp
+++ b/SurgSim/Devices/Keyboard/UnitTests/KeyboardDeviceTest.cpp
@@ -25,10 +25,10 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
-using SurgSim::Device::KeyboardDevice;
+using SurgSim::Devices::KeyboardDevice;
using SurgSim::DataStructures::DataGroup;
using SurgSim::Testing::MockInputOutput;
@@ -42,7 +42,7 @@ public:
};
-TEST_F(KeyboardDeviceTest, CreateInitializeAndDestroyDevice)
+TEST_F(KeyboardDeviceTest, InitializeDevice)
{
std::shared_ptr<KeyboardDevice> device = std::make_shared<KeyboardDevice>("TestKeyboard");
@@ -51,15 +51,6 @@ TEST_F(KeyboardDeviceTest, CreateInitializeAndDestroyDevice)
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)
@@ -74,10 +65,16 @@ TEST_F(KeyboardDeviceTest, InputConsumer)
EXPECT_TRUE(consumer->m_lastReceivedInput.integers().hasData("key"));
EXPECT_TRUE(consumer->m_lastReceivedInput.integers().hasData("modifierMask"));
-
- device->finalize();
}
+TEST_F(KeyboardDeviceTest, NoTwoKeyboards)
+{
+ std::shared_ptr<KeyboardDevice> device1 = std::make_shared<KeyboardDevice>("TestKeyboard1");
+ std::shared_ptr<KeyboardDevice> device2 = std::make_shared<KeyboardDevice>("TestKeyboard2");
+
+ EXPECT_NO_THROW(device1->initialize());
+ EXPECT_ANY_THROW(device2->initialize());
+}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/Keyboard/UnitTests/KeyboardScaffoldTest.cpp b/SurgSim/Devices/Keyboard/UnitTests/KeyboardScaffoldTest.cpp
index b9a7a7c..c4dfa45 100644
--- a/SurgSim/Devices/Keyboard/UnitTests/KeyboardScaffoldTest.cpp
+++ b/SurgSim/Devices/Keyboard/UnitTests/KeyboardScaffoldTest.cpp
@@ -20,7 +20,7 @@
#include "SurgSim/Devices/Keyboard/KeyboardScaffold.h"
-using SurgSim::Device::KeyboardScaffold;
+using SurgSim::Devices::KeyboardScaffold;
TEST(KeyboardScaffoldTest, CreateAndDestroyScaffold)
{
diff --git a/SurgSim/Devices/Keyboard/VisualTests/CMakeLists.txt b/SurgSim/Devices/Keyboard/VisualTests/CMakeLists.txt
index ace14f4..f931806 100644
--- a/SurgSim/Devices/Keyboard/VisualTests/CMakeLists.txt
+++ b/SurgSim/Devices/Keyboard/VisualTests/CMakeLists.txt
@@ -20,8 +20,8 @@ set(UNIT_TEST_SOURCES
set(UNIT_TEST_HEADERS
)
-add_executable(KeyboardVisualTest
- ${UNIT_TEST_SOURCES} ${UNIT_TEST_HEADERS})
+surgsim_add_executable(KeyboardVisualTest
+ "${UNIT_TEST_SOURCES}" "${UNIT_TEST_HEADERS}")
set(LIBS KeyboardDevice
SurgSimDataStructures
diff --git a/SurgSim/Devices/Keyboard/VisualTests/KeyboardVisualTests.cpp b/SurgSim/Devices/Keyboard/VisualTests/KeyboardVisualTests.cpp
index f02f34a..bdabbc5 100644
--- a/SurgSim/Devices/Keyboard/VisualTests/KeyboardVisualTests.cpp
+++ b/SurgSim/Devices/Keyboard/VisualTests/KeyboardVisualTests.cpp
@@ -40,17 +40,17 @@ struct TestListener : public SurgSim::Input::InputConsumerInterface
creatKeyMap();
createModifierMap();
}
- virtual void initializeInput(const std::string& device, const DataGroup& inputData) override {}
- virtual void handleInput(const std::string& device, const DataGroup& inputData) override
+ void initializeInput(const std::string& device, const DataGroup& inputData) override {}
+ 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)
+ if (key != SurgSim::Devices::KeyCode::NONE)
{
std::cerr << "Key pressed :" << keyMap[key] << std::endl;
- if (modifierMask != SurgSim::Device::ModKeyMask::MODKEY_NONE)
+ if (modifierMask != SurgSim::Devices::ModKeyMask::MODKEY_NONE)
{
if (modifierMap[modifierMask] != "")
{
@@ -72,227 +72,229 @@ struct TestListener : public SurgSim::Input::InputConsumerInterface
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";
+ using SurgSim::Devices::KeyCode;
+ keyMap[KeyCode::KEY_SPACE] = "KEY_SPACE";
+ keyMap[KeyCode::KEY_0] = "0";
+ keyMap[KeyCode::KEY_1] = "1";
+ keyMap[KeyCode::KEY_2] = "2";
+ keyMap[KeyCode::KEY_3] = "3";
+ keyMap[KeyCode::KEY_4] = "4";
+ keyMap[KeyCode::KEY_5] = "5";
+ keyMap[KeyCode::KEY_6] = "6";
+ keyMap[KeyCode::KEY_7] = "7";
+ keyMap[KeyCode::KEY_8] = "8";
+ keyMap[KeyCode::KEY_9] = "9";
+ keyMap[KeyCode::KEY_A] = "a";
+ keyMap[KeyCode::KEY_B] = "b";
+ keyMap[KeyCode::KEY_C] = "c";
+ keyMap[KeyCode::KEY_D] = "d";
+ keyMap[KeyCode::KEY_E] = "e";
+ keyMap[KeyCode::KEY_F] = "f";
+ keyMap[KeyCode::KEY_G] = "g";
+ keyMap[KeyCode::KEY_H] = "h";
+ keyMap[KeyCode::KEY_I] = "i";
+ keyMap[KeyCode::KEY_J] = "j";
+ keyMap[KeyCode::KEY_K] = "k";
+ keyMap[KeyCode::KEY_L] = "l";
+ keyMap[KeyCode::KEY_M] = "m";
+ keyMap[KeyCode::KEY_N] = "n";
+ keyMap[KeyCode::KEY_O] = "o";
+ keyMap[KeyCode::KEY_P] = "p";
+ keyMap[KeyCode::KEY_Q] = "q";
+ keyMap[KeyCode::KEY_R] = "r";
+ keyMap[KeyCode::KEY_S] = "s";
+ keyMap[KeyCode::KEY_T] = "t";
+ keyMap[KeyCode::KEY_U] = "u";
+ keyMap[KeyCode::KEY_V] = "v";
+ keyMap[KeyCode::KEY_W] = "w";
+ keyMap[KeyCode::KEY_X] = "x";
+ keyMap[KeyCode::KEY_Y] = "y";
+ keyMap[KeyCode::KEY_Z] = "z";
+ keyMap[KeyCode::KEY_EXCLAIM] = "KEY_EXCLAIM";
+ keyMap[KeyCode::KEY_QUOTEDBL] = "KEY_QUOTEDBL";
+ keyMap[KeyCode::KEY_HASH] = "KEY_HASH";
+ keyMap[KeyCode::KEY_DOLLAR] = "KEY_DOLLAR";
+ keyMap[KeyCode::KEY_AMPERSAND] = "KEY_AMPERSAND";
+ keyMap[KeyCode::KEY_QUOTE] = "KEY_QUOTE";
+ keyMap[KeyCode::KEY_LEFTPAREN] = "KEY_LEFTPAREN";
+ keyMap[KeyCode::KEY_RIGHTPAREN]= "KEY_RIGHTPAREN";
+ keyMap[KeyCode::KEY_ASTERISK] = "KEY_ASTERISK";
+ keyMap[KeyCode::KEY_PLUS] = "KEY_PLUS";
+ keyMap[KeyCode::KEY_COMMA] = "KEY_COMMA";
+ keyMap[KeyCode::KEY_MINUS] = "KEY_MINUS";
+ keyMap[KeyCode::KEY_PERIOD] = "KEY_PERIOD";
+ keyMap[KeyCode::KEY_SLASH] = "KEY_SLASH";
+ keyMap[KeyCode::KEY_COLON] = "KEY_COLON";
+ keyMap[KeyCode::KEY_SEMICOLON] = "KEY_SEMICOLON";
+ keyMap[KeyCode::KEY_LESS] = "KEY_LESS";
+ keyMap[KeyCode::KEY_EQUALS] = "KEY_EQUALS";
+ keyMap[KeyCode::KEY_GREATER] = "KEY_GREATER";
+ keyMap[KeyCode::KEY_QUESTION] = "KEY_QUESTION";
+ keyMap[KeyCode::KEY_AT] = "KEY_AT";
+ keyMap[KeyCode::KEY_LEFTBRACKET] = "KEY_LEFTBRACKET";
+ keyMap[KeyCode::KEY_BACKSLASH] = "KEY_BACKSLASH";
+ keyMap[KeyCode::KEY_RIGHTBRACKET]= "KEY_RIGHTBRACKET";
+ keyMap[KeyCode::KEY_CARET] = "KEY_CARET";
+ keyMap[KeyCode::KEY_UNDERSCORE] = "KEY_UNDERSCORE";
+ keyMap[KeyCode::KEY_BACKQUOTE] = "KEY_BACKQUOTE";
+ keyMap[KeyCode::KEY_BACKSPACE] = "KEY_BACKSPACE";
+ keyMap[KeyCode::KEY_TAB] = "KEY_TAB";
+ keyMap[KeyCode::KEY_LINEFEED] = "KEY_LINEFEED";
+ keyMap[KeyCode::KEY_CLEAR] = "KEY_CLEAR";
+ keyMap[KeyCode::KEY_RETURN] = "KEY_RETURN";
+ keyMap[KeyCode::KEY_PAUSE] = "KEY_PAUSE";
+ keyMap[KeyCode::KEY_SCROLL_LOCK] = "KEY_SCROLL_LOCK";
+ keyMap[KeyCode::KEY_SYS_REQ] = "KEY_SYS_REQ";
+ keyMap[KeyCode::KEY_ESCAPE] = "KEY_ESCAPE";
+ keyMap[KeyCode::KEY_DELETE] = "KEY_DELETE";
+ keyMap[KeyCode::KEY_HOME] = "KEY_HOME";
+ keyMap[KeyCode::KEY_LEFT] = "KEY_LEFT";
+ keyMap[KeyCode::KEY_UP] = "KEY_UP";
+ keyMap[KeyCode::KEY_RIGHT] = "KEY_RIGHT";
+ keyMap[KeyCode::KEY_DOWN] = "KEY_DOWN";
+ keyMap[KeyCode::KEY_PRIOR] = "KEY_PRIOR";
+ keyMap[KeyCode::KEY_PAGE_UP] = "KEY_PAGE_UP";
+ keyMap[KeyCode::KEY_NEXT] = "KEY_NEXT";
+ keyMap[KeyCode::KEY_PAGE_DOWN] = "KEY_PAGE_DOWN";
+ keyMap[KeyCode::KEY_END] = "KEY_END";
+ keyMap[KeyCode::KEY_BEGIN] = "KEY_BEGIN";
+ keyMap[KeyCode::KEY_SELECT] = "KEY_SELECT";
+ keyMap[KeyCode::KEY_PRINT] = "KEY_PRINT";
+ keyMap[KeyCode::KEY_EXECUTE] = "KEY_EXECUTE";
+ keyMap[KeyCode::KEY_INSERT] = "KEY_INSERT";
+ keyMap[KeyCode::KEY_UNDO] = "KEY_UNDO";
+ keyMap[KeyCode::KEY_REDO] = "KEY_REDO";
+ keyMap[KeyCode::KEY_MENU] = "KEY_MENU";
+ keyMap[KeyCode::KEY_FIND] = "KEY_FIND";
+ keyMap[KeyCode::KEY_CANCEL] = "KEY_CANCEL";
+ keyMap[KeyCode::KEY_HELP] = "KEY_HELP";
+ keyMap[KeyCode::KEY_BREAK] = "KEY_BREAK";
+ keyMap[KeyCode::KEY_MODE_SWITCH] = "KEY_MODE_SWITCH";
+ keyMap[KeyCode::KEY_SCRIPT_SWITCH] = "KEY_SCRIPT_SWITCH";
+ keyMap[KeyCode::KEY_NUM_LOCK] = "KEY_NUM_LOCK";
+ keyMap[KeyCode::KEY_KP_SPACE] = "KEY_KP_SPACE";
+ keyMap[KeyCode::KEY_KP_TAB] = "KEY_KP_TAB";
+ keyMap[KeyCode::KEY_KP_ENTER] = "KEY_KP_ENTER";
+ keyMap[KeyCode::KEY_KP_F1] = "KEY_KP_F1";
+ keyMap[KeyCode::KEY_KP_F2] = "KEY_KP_F2";
+ keyMap[KeyCode::KEY_KP_F3] = "KEY_KP_F3";
+ keyMap[KeyCode::KEY_KP_F4] = "KEY_KP_F4";
+ keyMap[KeyCode::KEY_KP_HOME] = "KEY_KP_HOME";
+ keyMap[KeyCode::KEY_KP_LEFT] = "KEY_KP_LEFT";
+ keyMap[KeyCode::KEY_KP_UP] = "KEY_KP_UP";
+ keyMap[KeyCode::KEY_KP_RIGHT] = "KEY_KP_RIGHT";
+ keyMap[KeyCode::KEY_KP_DOWN] = "KEY_KP_DOWN";
+ keyMap[KeyCode::KEY_KP_PRIOR] = "KEY_KP_PRIOR";
+ keyMap[KeyCode::KEY_KP_PAGE_UP] = "KEY_KP_PAGE_UP";
+ keyMap[KeyCode::KEY_KP_NEXT] = "KEY_KP_NEXT";
+ keyMap[KeyCode::KEY_KP_PAGE_DOWN] = "KEY_KP_PAGE_DOWN";
+ keyMap[KeyCode::KEY_KP_END] = "KEY_KP_END";
+ keyMap[KeyCode::KEY_KP_BEGIN] = "KEY_KP_BEGIN";
+ keyMap[KeyCode::KEY_KP_INSERT] = "KEY_KP_INSERT";
+ keyMap[KeyCode::KEY_KP_DELETE] = "KEY_KP_DELETE";
+ keyMap[KeyCode::KEY_KP_EQUAL] = "KEY_KP_EQUAL";
+ keyMap[KeyCode::KEY_KP_MULTIPLY] = "KEY_KP_MULTIPLY";
+ keyMap[KeyCode::KEY_KP_ADD] = "KEY_KP_ADD";
+ keyMap[KeyCode::KEY_KP_SEPARATOR]= "KEY_KP_SEPARATOR";
+ keyMap[KeyCode::KEY_KP_SUBTRACT] = "KEY_KP_SUBTRACT";
+ keyMap[KeyCode::KEY_KP_DECIMAL] = "KEY_KP_DECIMAL";
+ keyMap[KeyCode::KEY_KP_DIVIDE] = "KEY_KP_DIVIDE";
+ keyMap[KeyCode::KEY_KP_0] = "KEY_KP_0";
+ keyMap[KeyCode::KEY_KP_1] = "KEY_KP_1";
+ keyMap[KeyCode::KEY_KP_2] = "KEY_KP_2";
+ keyMap[KeyCode::KEY_KP_3] = "KEY_KP_3";
+ keyMap[KeyCode::KEY_KP_4] = "KEY_KP_4";
+ keyMap[KeyCode::KEY_KP_5] = "KEY_KP_5";
+ keyMap[KeyCode::KEY_KP_6] = "KEY_KP_6";
+ keyMap[KeyCode::KEY_KP_7] = "KEY_KP_7";
+ keyMap[KeyCode::KEY_KP_8] = "KEY_KP_8";
+ keyMap[KeyCode::KEY_KP_9] = "KEY_KP_9";
+ keyMap[KeyCode::KEY_F1] = "KEY_F1";
+ keyMap[KeyCode::KEY_F2] = "KEY_F2";
+ keyMap[KeyCode::KEY_F3] = "KEY_F3";
+ keyMap[KeyCode::KEY_F4] = "KEY_F4";
+ keyMap[KeyCode::KEY_F5] = "KEY_F5";
+ keyMap[KeyCode::KEY_F6] = "KEY_F6";
+ keyMap[KeyCode::KEY_F7] = "KEY_F7";
+ keyMap[KeyCode::KEY_F8] = "KEY_F8";
+ keyMap[KeyCode::KEY_F9] = "KEY_F9";
+ keyMap[KeyCode::KEY_F10] = "KEY_F10";
+ keyMap[KeyCode::KEY_F11] = "KEY_F11";
+ keyMap[KeyCode::KEY_F12] = "KEY_F12";
+ keyMap[KeyCode::KEY_F13] = "KEY_F13";
+ keyMap[KeyCode::KEY_F14] = "KEY_F14";
+ keyMap[KeyCode::KEY_F15] = "KEY_F15";
+ keyMap[KeyCode::KEY_F16] = "KEY_F16";
+ keyMap[KeyCode::KEY_F17] = "KEY_F17";
+ keyMap[KeyCode::KEY_F18] = "KEY_F18";
+ keyMap[KeyCode::KEY_F19] = "KEY_F19";
+ keyMap[KeyCode::KEY_F20] = "KEY_F20";
+ keyMap[KeyCode::KEY_F21] = "KEY_F21";
+ keyMap[KeyCode::KEY_F22] = "KEY_F22";
+ keyMap[KeyCode::KEY_F23] = "KEY_F23";
+ keyMap[KeyCode::KEY_F24] = "KEY_F24";
+ keyMap[KeyCode::KEY_F25] = "KEY_F25";
+ keyMap[KeyCode::KEY_F26] = "KEY_F26";
+ keyMap[KeyCode::KEY_F27] = "KEY_F27";
+ keyMap[KeyCode::KEY_F28] = "KEY_F28";
+ keyMap[KeyCode::KEY_F29] = "KEY_F29";
+ keyMap[KeyCode::KEY_F30] = "KEY_F30";
+ keyMap[KeyCode::KEY_F31] = "KEY_F31";
+ keyMap[KeyCode::KEY_F32] = "KEY_F32";
+ keyMap[KeyCode::KEY_F33] = "KEY_F33";
+ keyMap[KeyCode::KEY_F34] = "KEY_F34";
+ keyMap[KeyCode::KEY_F35] = "KEY_F35";
+ keyMap[KeyCode::KEY_SHIFT_L] = "KEY_SHIFT_L";
+ keyMap[KeyCode::KEY_SHIFT_R] = "KEY_SHIFT_R";
+ keyMap[KeyCode::KEY_CONTROL_L] = "KEY_CONTROL_L";
+ keyMap[KeyCode::KEY_CONTROL_R] = "KEY_CONTROL_R";
+ keyMap[KeyCode::KEY_CAPS_LOCK] = "KEY_CAPS_LOCK";
+ keyMap[KeyCode::KEY_SHIFT_LOCK] = "KEY_SHIFT_LOCK";
+ keyMap[KeyCode::KEY_META_L] = "KEY_META_L";
+ keyMap[KeyCode::KEY_META_R] = "KEY_META_R";
+ keyMap[KeyCode::KEY_ALT_L] = "KEY_ALT_L";
+ keyMap[KeyCode::KEY_ALT_R] = "KEY_ALT_R";
+ keyMap[KeyCode::KEY_SUPER_L] = "KEY_SUPER_L";
+ keyMap[KeyCode::KEY_SUPER_R] = "KEY_SUPER_R";
+ keyMap[KeyCode::KEY_HYPER_L] = "KEY_HYPER_L";
+ keyMap[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";
+ using SurgSim::Devices::ModKeyMask;
+ modifierMap[ModKeyMask::MODKEY_LEFT_SHIFT] = "KEY_SHIFT_L";
+ modifierMap[ModKeyMask::MODKEY_RIGHT_SHIFT] = "KEY_SHIFT_R";
+ modifierMap[ModKeyMask::MODKEY_LEFT_CTRL] = "KEY_CONTROL_L";
+ modifierMap[ModKeyMask::MODKEY_RIGHT_CTRL] = "KEY_CONTROL_R";
+ modifierMap[ModKeyMask::MODKEY_LEFT_ALT] = "KEY_ALT_L";
+ modifierMap[ModKeyMask::MODKEY_RIGHT_ALT] = "KEY_ALT_R";
+ modifierMap[ModKeyMask::MODKEY_LEFT_META] = "KEY_META_L";
+ modifierMap[ModKeyMask::MODKEY_RIGHT_META] = "KEY_META_R";
+ modifierMap[ModKeyMask::MODKEY_LEFT_SUPER] = "KEY_SUPER_L";
+ modifierMap[ModKeyMask::MODKEY_RIGHT_SUPER] = "KEY_SUPER_R";
+ modifierMap[ModKeyMask::MODKEY_LEFT_HYPER] = "KEY_HYPER_L";
+ modifierMap[ModKeyMask::MODKEY_RIGHT_HYPER] = "KEY_HYPER_R";
+ modifierMap[ModKeyMask::MODKEY_NUM_LOCK] = "KEY_NUM_LOCK";
+ modifierMap[ModKeyMask::MODKEY_CAPS_LOCK] = "KEY_CAPS_LOCK";
+ modifierMap[ModKeyMask::MODKEY_CTRL] = "KEY_CTRL";
+ modifierMap[ModKeyMask::MODKEY_SHIFT] = "KEY_SHIFT";
+ modifierMap[ModKeyMask::MODKEY_ALT] = "KEY_ALT";
+ modifierMap[ModKeyMask::MODKEY_META] = "KEY_META";
+ modifierMap[ModKeyMask::MODKEY_SUPER] = "KEY_SUPER";
+ modifierMap[ModKeyMask::MODKEY_HYPER] = "KEY_HYPER";
+ modifierMap[ModKeyMask::MODKEY_CAPS_SHIFT_L] = "KEY_CAPS_SHIFT_L";
+ modifierMap[ModKeyMask::MODKEY_CAPS_SHIFT_R] = "KEY_CAPS_SHIFT_R";
+ modifierMap[ModKeyMask::MODKEY_CAPS_CONTROL_L] = "KEY_CAPS_CONTROL_L";
+ modifierMap[ModKeyMask::MODKEY_CAPS_CONTROL_R] = "KEY_CAPS_CONTROL_R";
+ modifierMap[ModKeyMask::MODKEY_CAPS_ALT_L] = "KEY_CAPS_ALT_L";
+ modifierMap[ModKeyMask::MODKEY_CAPS_ALT_R] = "KEY_CAPS_ALT_R";
}
};
int main(int argc, char* argv[])
{
- auto toolDevice = std::make_shared<SurgSim::Device::KeyboardDevice>("Keyboard");
+ auto toolDevice = std::make_shared<SurgSim::Devices::KeyboardDevice>("Keyboard");
toolDevice->initialize();
osg::ref_ptr<osgGA::GUIEventHandler> keyboardHandler = toolDevice->getKeyboardHandler();
diff --git a/SurgSim/Devices/LabJack/CMakeLists.txt b/SurgSim/Devices/LabJack/CMakeLists.txt
index a69e006..b734bc7 100644
--- a/SurgSim/Devices/LabJack/CMakeLists.txt
+++ b/SurgSim/Devices/LabJack/CMakeLists.txt
@@ -40,6 +40,8 @@ set(LABJACK_DEVICE_HEADERS
LabJackThread.h
)
+set(DEVICE_HEADERS ${DEVICE_HEADERS} LabJack/LabJackDevice.h PARENT_SCOPE)
+
set(LABJACK_DEVICE_LINUX_SOURCES
linux/LabJackChecksums.cpp
linux/LabJackScaffold.cpp
@@ -66,8 +68,8 @@ endif()
# TODO(advornik): the installation should NOT copy all the headers...
surgsim_add_library(
LabJackDevice
- "${LABJACK_DEVICE_SOURCES}" "${LABJACK_DEVICE_HEADERS}"
- SurgSim/Devices/LabJack
+ "${LABJACK_DEVICE_SOURCES}"
+ "${LABJACK_DEVICE_HEADERS}"
)
target_link_libraries(LabJackDevice ${LIBS})
diff --git a/SurgSim/Devices/LabJack/LabJackDevice.cpp b/SurgSim/Devices/LabJack/LabJackDevice.cpp
index ea9360d..1f11866 100644
--- a/SurgSim/Devices/LabJack/LabJackDevice.cpp
+++ b/SurgSim/Devices/LabJack/LabJackDevice.cpp
@@ -20,14 +20,17 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::LabJackDevice, LabJackDevice);
+
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_reset(false),
m_timerBase(LabJack::TIMERBASE_DEFAULT),
m_timerClockDivisor(1),
m_timerCounterPinOffset(0),
@@ -50,35 +53,30 @@ 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.";
+ SURGSIM_ASSERT(scaffold != nullptr) << "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.";
- }
+ auto logger = Framework::Logger::getLogger("Devices/LabJack");
+ SURGSIM_LOG_IF((getDigitalOutputs().size() > 0) && !hasOutputProducer(), logger, 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.";
- }
+ SURGSIM_LOG_IF((getAnalogOutputs().size() > 0) && !hasOutputProducer(), logger, WARNING) <<
+ "LabJackDevice named " << getName() <<
+ " has analog output channels but no output producer to provide the output data. Call setOutputProducer.";
- bool found = false;
+ bool registered = 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;
+ registered = true;
}
- return found;
+ return registered;
}
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;
@@ -122,6 +120,16 @@ const std::string& LabJackDevice::getAddress() const
return m_address;
}
+void LabJackDevice::setResetOnDestruct(bool reset)
+{
+ m_reset = reset;
+}
+
+bool LabJackDevice::getResetOnDestruct() const
+{
+ return m_reset;
+}
+
void LabJackDevice::enableDigitalInput(int channel)
{
SURGSIM_ASSERT(!isInitialized()) << "Digital input cannot be enabled for a LabJackDevice after it is initialized.";
@@ -194,16 +202,14 @@ int LabJackDevice::getTimerCounterPinOffset() const
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>()};
+ LabJack::TimerSettings timerModeAndOptionalInitialValue = {mode, 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)};
+ LabJack::TimerSettings timerModeAndOptionalInitialValue = {mode, DataStructures::OptionalValue<int>(initialValue)};
m_timers[index] = std::move(timerModeAndOptionalInitialValue);
}
@@ -235,7 +241,7 @@ void LabJackDevice::enableAnalogInput(int positiveChannel, 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>(negativeChannel)};
+ DataStructures::OptionalValue<int>(negativeChannel)};
m_analogInputs[positiveChannel] = std::move(rangeAndOptionalNegativeChannel);
}
@@ -243,8 +249,7 @@ 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>()};
+ LabJack::AnalogInputSettings rangeAndOptionalNegativeChannel = {range, DataStructures::OptionalValue<int>()};
m_analogInputs[channel] = std::move(rangeAndOptionalNegativeChannel);
}
@@ -302,5 +307,5 @@ int LabJackDevice::getAnalogInputSettling() const
return m_analogInputSettling;
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/LabJack/LabJackDevice.h b/SurgSim/Devices/LabJack/LabJackDevice.h
index c9bdbeb..43c23d9 100644
--- a/SurgSim/Devices/LabJack/LabJackDevice.h
+++ b/SurgSim/Devices/LabJack/LabJackDevice.h
@@ -26,10 +26,12 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
class LabJackScaffold;
+SURGSIM_STATIC_REGISTRATION(LabJackDevice);
+
namespace LabJack
{
///@{
@@ -276,18 +278,14 @@ public:
/// \param uniqueName A unique name for the device that will be used by the application.
explicit LabJackDevice(const std::string& uniqueName);
+ SURGSIM_CLASSNAME(SurgSim::Devices::LabJackDevice);
+
/// 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;
+ bool initialize() override;
- /// Check whether this device is initialized.
- bool isInitialized() const;
+ bool isInitialized() const override;
/// Set the model, e.g., U6.
/// \param model The model.
@@ -314,6 +312,21 @@ public:
/// \return The address of the LabJack, e.g., "1" or "192.168.7.23".
const std::string& getAddress() const;
+ /// Reset LabJack during destruct.
+ /// \param reset true if the hardware should reset & re-enumerate when this object destructs.
+ /// \note The LabJack hardware has the feature that it continues operation and remembers all settings even after
+ /// the associated LabJackDevice object destructs and communication ends.
+ /// This function causes the hardware to reset when the LabJackDevice object destructs, thereby returning to its
+ /// default (boot-up) settings and forcing USB re-enumeration.
+ /// \note After a reset, it will take a few seconds before the hardware can communicate.
+ /// \warning If the LabJackDevice object does not cleanly destruct (e.g., the executable halts due to an exception),
+ /// then the hardware will not reset.
+ void setResetOnDestruct(bool reset);
+
+ /// Get whether or not the hardware should reset when the LabJackDevice object destructs.
+ /// \return true if should reset on destruct.
+ bool getResetOnDestruct() const;
+
/// Enable digital input line.
/// \param channel The channel number.
/// \exception Asserts if already initialized.
@@ -462,13 +475,10 @@ public:
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;
+ bool finalize() override;
+
/// The single scaffold object that handles communications with all instances of LabJackDevice.
std::shared_ptr<LabJackScaffold> m_scaffold;
@@ -481,6 +491,9 @@ private:
/// The address, or a zero-length string to indicate the first-found device of this type on this connection.
std::string m_address;
+ /// Whether or not the hardware should reset when this object destructs.
+ bool m_reset;
+
/// The line numbers for the digital inputs.
std::unordered_set<int> m_digitalInputChannels;
@@ -517,7 +530,7 @@ private:
int m_analogInputSettling;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_LABJACK_LABJACKDEVICE_H
diff --git a/SurgSim/Devices/LabJack/LabJackScaffold.h b/SurgSim/Devices/LabJack/LabJackScaffold.h
index 4aeff8e..f3fc5b6 100644
--- a/SurgSim/Devices/LabJack/LabJackScaffold.h
+++ b/SurgSim/Devices/LabJack/LabJackScaffold.h
@@ -30,14 +30,14 @@ namespace Framework
class Logger;
}
-namespace Device
+namespace Devices
{
class LabJackDevice;
class LabJackThread;
/// A class that implements the behavior of LabJackDevice objects.
-/// \sa SurgSim::Device::LabJackDevice
+/// \sa SurgSim::Devices::LabJackDevice
class LabJackScaffold
{
public:
@@ -50,20 +50,15 @@ public:
/// 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.
+ /// The scaffold is managed using a SharedInstance 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);
+ void configureDevice(DeviceData* device);
private:
/// Internal shared state data type.
@@ -135,16 +130,16 @@ private:
bool configureAnalog(DeviceData* deviceData);
/// Builds the data layout for the application input (i.e. device output).
- static SurgSim::DataStructures::DataGroup buildDeviceInputData();
+ static DataStructures::DataGroup buildDeviceInputData();
/// Logger used by the scaffold and all devices.
- std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+ std::shared_ptr<Framework::Logger> m_logger;
/// Internal scaffold state.
std::unique_ptr<StateData> m_state;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_LABJACK_LABJACKSCAFFOLD_H
diff --git a/SurgSim/Devices/LabJack/LabJackThread.cpp b/SurgSim/Devices/LabJack/LabJackThread.cpp
index 8532672..30be55c 100644
--- a/SurgSim/Devices/LabJack/LabJackThread.cpp
+++ b/SurgSim/Devices/LabJack/LabJackThread.cpp
@@ -17,13 +17,14 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
LabJackThread::LabJackThread(LabJackScaffold* scaffold, LabJackScaffold::DeviceData* deviceData) :
BasicThread("LabJack thread"),
m_scaffold(scaffold),
m_deviceData(deviceData)
{
+ m_scaffold->configureDevice(m_deviceData);
}
LabJackThread::~LabJackThread()
@@ -32,7 +33,7 @@ LabJackThread::~LabJackThread()
bool LabJackThread::doInitialize()
{
- return m_scaffold->configureDevice(m_deviceData);
+ return true;
}
bool LabJackThread::doStartUp()
@@ -45,5 +46,5 @@ bool LabJackThread::doUpdate(double dt)
return m_scaffold->runInputFrame(m_deviceData);
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/LabJack/LabJackThread.h b/SurgSim/Devices/LabJack/LabJackThread.h
index d997bc1..3f82faf 100644
--- a/SurgSim/Devices/LabJack/LabJackThread.h
+++ b/SurgSim/Devices/LabJack/LabJackThread.h
@@ -23,11 +23,11 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
/// A class implementing the thread context for communicating with LabJack devices.
-/// \sa SurgSim::Device::LabJackScaffold
+/// \sa SurgSim::Devices::LabJackScaffold
class LabJackThread : public SurgSim::Framework::BasicThread
{
public:
@@ -36,16 +36,16 @@ public:
virtual ~LabJackThread();
protected:
- virtual bool doInitialize() override;
- virtual bool doStartUp() override;
- virtual bool doUpdate(double dt) override;
+ bool doInitialize() override;
+ bool doStartUp() override;
+ bool doUpdate(double dt) override;
private:
LabJackScaffold* m_scaffold;
LabJackScaffold::DeviceData* m_deviceData;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_LABJACK_LABJACKTHREAD_H
diff --git a/SurgSim/Devices/LabJack/UnitTests/LabJackChecksumsTest.cpp b/SurgSim/Devices/LabJack/UnitTests/LabJackChecksumsTest.cpp
index 3c532f0..41c4f54 100644
--- a/SurgSim/Devices/LabJack/UnitTests/LabJackChecksumsTest.cpp
+++ b/SurgSim/Devices/LabJack/UnitTests/LabJackChecksumsTest.cpp
@@ -21,16 +21,22 @@
#include "SurgSim/Devices/LabJack/linux/LabJackConstants.h"
#include "SurgSim/Devices/LabJack/linux/LabJackChecksums.h"
+using SurgSim::Devices::LabJack::extendedChecksum;
+using SurgSim::Devices::LabJack::extendedChecksum8;
+using SurgSim::Devices::LabJack::extendedChecksum16;
+using SurgSim::Devices::LabJack::normalChecksum;
+using SurgSim::Devices::LabJack::normalChecksum8;
+
TEST(LabJackChecksumsTest, NormalChecksum)
{
// Sum less than 256
- std::array<unsigned char, SurgSim::Device::LabJack::MAXIMUM_BUFFER> bytes;
+ std::array<unsigned char, SurgSim::Devices::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, normalChecksum8(bytes, count));
+ normalChecksum(&bytes, count);
EXPECT_EQ(expectedValue, bytes[0]);
// Sum greater than 256, quotient + remainder < 256
@@ -41,8 +47,8 @@ TEST(LabJackChecksumsTest, NormalChecksum)
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, normalChecksum8(bytes, count));
+ normalChecksum(&bytes, count);
EXPECT_EQ(expectedValue, bytes[0]);
// Sum greater than 256, quotient + remainder > 256
@@ -53,8 +59,8 @@ TEST(LabJackChecksumsTest, NormalChecksum)
// 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, normalChecksum8(bytes, count));
+ normalChecksum(&bytes, count);
EXPECT_EQ(expectedValue, bytes[0]);
}
@@ -62,16 +68,16 @@ TEST(LabJackChecksumsTest, NormalChecksum)
TEST(LabJackChecksumsTest, ExtendedChecksum)
{
// Sums less than 256
- std::array<unsigned char, SurgSim::Device::LabJack::MAXIMUM_BUFFER> bytes;
+ std::array<unsigned char, SurgSim::Devices::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));
+ EXPECT_EQ(expectedValue16, extendedChecksum16(bytes, count));
int expectedValue8 = (6 - 1) * fillValue; // 5 * 2 = 10
- EXPECT_EQ(expectedValue8, SurgSim::Device::LabJack::extendedChecksum8(bytes));
+ EXPECT_EQ(expectedValue8, extendedChecksum8(bytes));
- SurgSim::Device::LabJack::extendedChecksum(&bytes, count);
+ 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.
@@ -82,11 +88,11 @@ TEST(LabJackChecksumsTest, ExtendedChecksum)
bytes.fill(fillValue);
count = 20;
expectedValue16 = (count - 6) * fillValue; // 14 * 100 = 1400
- EXPECT_EQ(expectedValue16, SurgSim::Device::LabJack::extendedChecksum16(bytes, count));
+ EXPECT_EQ(expectedValue16, extendedChecksum16(bytes, count));
expectedValue8 = 245; // sum = 5 * 100 = 500, quotient = 1, remainder = 244, quotient + remainder = 245
- EXPECT_EQ(expectedValue8, SurgSim::Device::LabJack::extendedChecksum8(bytes));
+ EXPECT_EQ(expectedValue8, extendedChecksum8(bytes));
- SurgSim::Device::LabJack::extendedChecksum(&bytes, count);
+ 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.
diff --git a/SurgSim/Devices/LabJack/UnitTests/LabJackDeviceTest.cpp b/SurgSim/Devices/LabJack/UnitTests/LabJackDeviceTest.cpp
index feac585..02db055 100644
--- a/SurgSim/Devices/LabJack/UnitTests/LabJackDeviceTest.cpp
+++ b/SurgSim/Devices/LabJack/UnitTests/LabJackDeviceTest.cpp
@@ -27,8 +27,8 @@
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Testing/MockInputOutput.h"
-using SurgSim::Device::LabJackDevice;
-using SurgSim::Device::LabJackScaffold;
+using SurgSim::Devices::LabJackDevice;
+using SurgSim::Devices::LabJackScaffold;
using SurgSim::DataStructures::DataGroup;
using SurgSim::Input::InputConsumerInterface;
using SurgSim::Input::OutputProducerInterface;
@@ -129,6 +129,7 @@ TEST(LabJackDeviceTest, InputConsumer)
EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
EXPECT_TRUE(device->addInputConsumer(consumer));
+ EXPECT_TRUE(device->setOutputProducer(consumer));
// Adding the same input consumer again should fail.
EXPECT_FALSE(device->addInputConsumer(consumer));
@@ -194,11 +195,11 @@ 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;
+ const SurgSim::Devices::LabJack::Model model = SurgSim::Devices::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;
+ const SurgSim::Devices::LabJack::Connection connection = SurgSim::Devices::LabJack::CONNECTION_USB;
EXPECT_NO_THROW(device->setConnection(connection));
EXPECT_EQ(connection, device->getConnection());
@@ -206,11 +207,18 @@ TEST(LabJackDeviceTest, GettersAndSetters)
EXPECT_NO_THROW(device->setAddress(address));
EXPECT_EQ(address, device->getAddress());
+ bool reset = true;
+ device->setResetOnDestruct(reset);
+ EXPECT_EQ(reset, device->getResetOnDestruct());
+ reset = false;
+ device->setResetOnDestruct(reset);
+ EXPECT_EQ(reset, device->getResetOnDestruct());
+
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_NO_THROW(device->enableDigitalInput(SurgSim::Devices::LabJack::FIO2));
+ EXPECT_NO_THROW(device->enableDigitalInput(SurgSim::Devices::LabJack::EIO3));
EXPECT_EQ(digitalInputChannels, device->getDigitalInputs());
digitalInputChannels.insert(14);
@@ -220,15 +228,15 @@ TEST(LabJackDeviceTest, GettersAndSetters)
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_NO_THROW(device->enableDigitalOutput(SurgSim::Devices::LabJack::FIO3));
+ EXPECT_NO_THROW(device->enableDigitalOutput(SurgSim::Devices::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;
+ const SurgSim::Devices::LabJack::TimerBase timerBase = SurgSim::Devices::LabJack::TIMERBASE_DEFAULT;
EXPECT_NO_THROW(device->setTimerBase(timerBase));
EXPECT_EQ(timerBase, device->getTimerBase());
@@ -240,21 +248,21 @@ TEST(LabJackDeviceTest, GettersAndSetters)
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>()};
+ std::unordered_map<int, SurgSim::Devices::LabJack::TimerSettings> timers;
+ SurgSim::Devices::LabJack::TimerSettings quadrature =
+ {SurgSim::Devices::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)};
+ SurgSim::Devices::LabJack::TimerSettings frequencyOutput =
+ {SurgSim::Devices::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_NO_THROW(device->enableTimer(SurgSim::Devices::LabJack::TIMER0,
+ SurgSim::Devices::LabJack::TIMERMODE_QUADRATURE));
+ EXPECT_NO_THROW(device->enableTimer(SurgSim::Devices::LabJack::TIMER3,
+ SurgSim::Devices::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>()};
+ SurgSim::Devices::LabJack::TimerSettings dutyCycle =
+ {SurgSim::Devices::LabJack::TIMERMODE_DUTY_CYCLE, SurgSim::DataStructures::OptionalValue<int>()};
timers[4] = dutyCycle;
EXPECT_NO_THROW(device->setTimers(timers));
EXPECT_EQ(timers, device->getTimers());
@@ -263,23 +271,23 @@ TEST(LabJackDeviceTest, GettersAndSetters)
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,
+ std::unordered_map<int, SurgSim::Devices::LabJack::AnalogInputSettings> analogInputs;
+ const SurgSim::Devices::LabJack::AnalogInputSettings differentialRangeAndChannel =
+ {SurgSim::Devices::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,
+ const SurgSim::Devices::LabJack::AnalogInputSettings singleEndedRange =
+ {SurgSim::Devices::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_NO_THROW(device->enableAnalogInput(SurgSim::Devices::LabJack::AIN2,
+ SurgSim::Devices::LabJack::Range::RANGE_10, 1));
+ EXPECT_NO_THROW(device->enableAnalogInput(SurgSim::Devices::LabJack::AIN5,
+ SurgSim::Devices::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,
+ const SurgSim::Devices::LabJack::AnalogInputSettings anotherRange =
+ {SurgSim::Devices::LabJack::Range::RANGE_0_POINT_01,
SurgSim::DataStructures::OptionalValue<int>()};
analogInputs[6] = anotherRange;
EXPECT_NO_THROW(device->setAnalogInputs(analogInputs));
@@ -295,7 +303,7 @@ TEST(LabJackDeviceTest, GettersAndSetters)
std::unordered_set<int> analogOutputChannels;
analogOutputChannels.insert(1);
- EXPECT_NO_THROW(device->enableAnalogOutput(SurgSim::Device::LabJack::DAC1));
+ EXPECT_NO_THROW(device->enableAnalogOutput(SurgSim::Devices::LabJack::DAC1));
EXPECT_EQ(analogOutputChannels, device->getAnalogOutputs());
analogOutputChannels.insert(0);
@@ -311,49 +319,51 @@ TEST(LabJackDeviceTest, NoSettingAfterInitialization)
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->setModel(SurgSim::Devices::LabJack::MODEL_U6), SurgSim::Framework::AssertionFailure);
- EXPECT_THROW(device->setConnection(SurgSim::Device::LabJack::CONNECTION_USB), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(device->setConnection(SurgSim::Devices::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->enableDigitalInput(SurgSim::Devices::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->enableDigitalOutput(SurgSim::Devices::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),
+ EXPECT_THROW(device->setTimerBase(SurgSim::Devices::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),
+ EXPECT_THROW(device->enableTimer(SurgSim::Devices::LabJack::TIMER0,
+ SurgSim::Devices::LabJack::TIMERMODE_QUADRATURE),
SurgSim::Framework::AssertionFailure);
EXPECT_THROW(device->setTimers(std::unordered_map<int,
- SurgSim::Device::LabJack::TimerSettings>()),
+ SurgSim::Devices::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),
+ EXPECT_THROW(device->enableAnalogInput(SurgSim::Devices::LabJack::AIN2,
+ SurgSim::Devices::LabJack::Range::RANGE_10, 1),
SurgSim::Framework::AssertionFailure);
EXPECT_THROW(device->setAnalogInputs(std::unordered_map<int,
- SurgSim::Device::LabJack::AnalogInputSettings>()),
+ SurgSim::Devices::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->enableAnalogOutput(SurgSim::Devices::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
index 31ac6f5..ab22160 100644
--- a/SurgSim/Devices/LabJack/UnitTests/LabJackScaffoldTest.cpp
+++ b/SurgSim/Devices/LabJack/UnitTests/LabJackScaffoldTest.cpp
@@ -27,8 +27,8 @@
#include "SurgSim/Input/InputConsumerInterface.h"
#include "SurgSim/Input/OutputProducerInterface.h"
-using SurgSim::Device::LabJackDevice;
-using SurgSim::Device::LabJackScaffold;
+using SurgSim::Devices::LabJackDevice;
+using SurgSim::Devices::LabJackScaffold;
TEST(LabJackScaffoldTest, CreateAndDestroyScaffold)
{
diff --git a/SurgSim/Devices/LabJack/UnitTests/LabJackTypeConvertersTest.cpp b/SurgSim/Devices/LabJack/UnitTests/LabJackTypeConvertersTest.cpp
index f6b4bdd..db82f45 100644
--- a/SurgSim/Devices/LabJack/UnitTests/LabJackTypeConvertersTest.cpp
+++ b/SurgSim/Devices/LabJack/UnitTests/LabJackTypeConvertersTest.cpp
@@ -28,52 +28,52 @@ namespace
TEST(LabJackTypeConvertersTest, DoubleFromChars)
{
- std::array<unsigned char, SurgSim::Device::LabJack::MAXIMUM_BUFFER> bytes;
+ std::array<unsigned char, SurgSim::Devices::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);
+ EXPECT_NEAR(0.0, SurgSim::Devices::LabJack::doubleFromChars(bytes, 0), EPSILON);
bytes[12] = 2;
- EXPECT_NEAR(2.0, SurgSim::Device::LabJack::doubleFromChars(bytes, 8), EPSILON);
+ EXPECT_NEAR(2.0, SurgSim::Devices::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);
+ EXPECT_NEAR(-1.0, SurgSim::Devices::LabJack::doubleFromChars(bytes, 16), EPSILON);
}
TEST(LabJackTypeConvertersTest, Uint32FromChars)
{
- std::array<unsigned char, SurgSim::Device::LabJack::MAXIMUM_BUFFER> bytes;
+ std::array<unsigned char, SurgSim::Devices::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));
+ EXPECT_ANY_THROW(SurgSim::Devices::LabJack::uint32FromChars(bytes, 0, 5));
+ EXPECT_EQ(0, SurgSim::Devices::LabJack::uint32FromChars(bytes, 0, 4));
+ EXPECT_EQ(1 << 24, SurgSim::Devices::LabJack::uint32FromChars(bytes, 1, 4));
+ EXPECT_EQ(1 << 16, SurgSim::Devices::LabJack::uint32FromChars(bytes, 2, 3));
+ EXPECT_EQ(1, SurgSim::Devices::LabJack::uint32FromChars(bytes, 4, 4));
}
TEST(LabJackTypeConvertersTest, Uint16FromChars)
{
- std::array<unsigned char, SurgSim::Device::LabJack::MAXIMUM_BUFFER> bytes;
+ std::array<unsigned char, SurgSim::Devices::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
+ EXPECT_ANY_THROW(SurgSim::Devices::LabJack::uint16FromChars(bytes, 0, 3));
+ EXPECT_EQ(0, SurgSim::Devices::LabJack::uint16FromChars(bytes, 0, 2));
+ EXPECT_EQ(1 << 8, SurgSim::Devices::LabJack::uint16FromChars(bytes, 1, 2));
+ EXPECT_EQ(1, SurgSim::Devices::LabJack::uint16FromChars(bytes, 2, 1));
+ EXPECT_EQ(1, SurgSim::Devices::LabJack::uint16FromChars(bytes, 2, 2));
+}
diff --git a/SurgSim/Devices/LabJack/VisualTest/CMakeLists.txt b/SurgSim/Devices/LabJack/VisualTest/CMakeLists.txt
index f8758b1..28c84b5 100644
--- a/SurgSim/Devices/LabJack/VisualTest/CMakeLists.txt
+++ b/SurgSim/Devices/LabJack/VisualTest/CMakeLists.txt
@@ -14,14 +14,6 @@
# limitations under the License.
-link_directories(
- ${Boost_LIBRARY_DIRS}
-)
-
-include_directories(
- "${CMAKE_CURRENT_SOURCE_DIR}"
-)
-
set(EXAMPLE_SOURCES
main.cpp
)
@@ -29,20 +21,17 @@ set(EXAMPLE_SOURCES
set(EXAMPLE_HEADERS
)
-add_executable(LabJackVisualTest
- ${EXAMPLE_SOURCES} ${EXAMPLE_HEADERS})
-surgsim_show_ide_folders(
+surgsim_add_executable(LabJackVisualTest
"${EXAMPLE_SOURCES}" "${EXAMPLE_HEADERS}")
set(LIBS
- LabJackDevice
IdentityPoseDevice
- VisualTestCommon
- SurgSimInput
- SurgSimFramework
+ LabJackDevice
SurgSimDataStructures
- ${Boost_LIBRARIES}
- ${OPENGL_LIBRARIES}
+ SurgSimDeviceFilters
+ SurgSimInput
+ SurgSimMath
+ VisualTestCommon
)
target_link_libraries(LabJackVisualTest ${LIBS})
diff --git a/SurgSim/Devices/LabJack/VisualTest/main.cpp b/SurgSim/Devices/LabJack/VisualTest/main.cpp
index 916390b..742ad63 100644
--- a/SurgSim/Devices/LabJack/VisualTest/main.cpp
+++ b/SurgSim/Devices/LabJack/VisualTest/main.cpp
@@ -17,6 +17,7 @@
#include "SurgSim/DataStructures/DataGroup.h"
#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/DeviceFilters/DeviceFilter.h"
#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
#include "SurgSim/Devices/LabJack/LabJackDevice.h"
#include "SurgSim/Input/CommonDevice.h"
@@ -29,25 +30,26 @@
using SurgSim::DataStructures::DataGroup;
using SurgSim::DataStructures::DataGroupBuilder;
-using SurgSim::Device::IdentityPoseDevice;
-using SurgSim::Device::LabJackDevice;
+using SurgSim::Devices::IdentityPoseDevice;
+using SurgSim::Devices::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
+class LabJackToPoseFilter : public SurgSim::Devices::DeviceFilter
{
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),
+ LabJackToPoseFilter(const std::string& name, int firstTimerForQuadrature, int resetQuadrature, int plusX,
+ int minusX, double translationPerUpdate, int positiveAnalogDifferential, int analogSingleEnded, int xOut,
+ int loopbackOut) :
+ SurgSim::Devices::DeviceFilter(name),
m_pose(RigidTransform3d::Identity()),
m_translationPerUpdate(translationPerUpdate),
m_lineForPlusX(plusX),
m_lineForMinusX(minusX),
m_firstTimerForQuadrature(firstTimerForQuadrature),
+ m_lineForResetQuadrature(resetQuadrature),
m_analogInputDifferentialPositive(positiveAnalogDifferential),
m_analogInputSingleEnded(analogSingleEnded),
m_cachedOutputIndices(false),
@@ -71,27 +73,14 @@ public:
const std::string digitalOutputName =
SurgSim::DataStructures::Names::DIGITAL_OUTPUT_PREFIX + std::to_string(loopbackOut);
outputBuilder.addBoolean(digitalOutputName);
+ outputBuilder.addScalar(SurgSim::DataStructures::Names::TIMER_OUTPUT_PREFIX +
+ std::to_string(m_firstTimerForQuadrature));
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)
+ void initializeInput(const std::string& device, const DataGroup& inputData) override
{
m_digitalInputPlusXIndex = inputData.booleans().getIndex(SurgSim::DataStructures::Names::DIGITAL_INPUT_PREFIX +
std::to_string(m_lineForPlusX));
@@ -106,28 +95,12 @@ public:
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;
+ filterInput(device, inputData, &getInputData());
}
- void inputFilter(const DataGroup& dataToFilter, DataGroup* result)
+ void filterInput(const std::string& device, const DataGroup& dataToFilter, DataGroup* result) override
{
+ m_lastInputData = dataToFilter;
// Turn LabJack inputs into a pose so it can control the sphere.
if (m_digitalInputPlusXIndex >= 0)
{
@@ -187,9 +160,9 @@ public:
result->poses().set(m_poseIndex, m_pose);
}
- void outputFilter(const DataGroup& dataToFilter, DataGroup* result)
+ void filterOutput(const std::string& device, const DataGroup& dataToFilter, DataGroup* result) override
{
- *result = dataToFilter;
+ *result = m_outputData;
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);
@@ -203,6 +176,20 @@ public:
{
result->booleans().reset(m_digitalOutputIndex);
}
+
+ bool resetQuadrature = false;
+ if ((m_lastInputData.booleans().get(SurgSim::DataStructures::Names::DIGITAL_INPUT_PREFIX +
+ std::to_string(m_lineForResetQuadrature), &resetQuadrature)) &&
+ resetQuadrature)
+ {
+ result->scalars().set(SurgSim::DataStructures::Names::TIMER_OUTPUT_PREFIX +
+ std::to_string(m_firstTimerForQuadrature), 0.0);
+ }
+ else
+ {
+ result->scalars().reset(SurgSim::DataStructures::Names::TIMER_OUTPUT_PREFIX +
+ std::to_string(m_firstTimerForQuadrature));
+ }
}
private:
@@ -215,6 +202,7 @@ private:
int m_lineForPlusX;
int m_lineForMinusX;
int m_firstTimerForQuadrature;
+ int m_lineForResetQuadrature;
int m_analogInputDifferentialPositive;
int m_analogInputDifferentialNegative;
int m_analogInputSingleEnded;
@@ -236,43 +224,44 @@ 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;
+ const int plusX = SurgSim::Devices::LabJack::FIO0;
toolDevice->enableDigitalInput(plusX);
- const int minusX = SurgSim::Device::LabJack::FIO1;
+ const int minusX = SurgSim::Devices::LabJack::FIO1;
toolDevice->enableDigitalInput(minusX);
- const int loopbackOut = SurgSim::Device::LabJack::FIO2;
+ const int loopbackOut = SurgSim::Devices::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 firstTimerForQuadrature = SurgSim::Devices::LabJack::TIMER0;
+ toolDevice->enableTimer(firstTimerForQuadrature, SurgSim::Devices::LabJack::TIMERMODE_QUADRATURE);
+ toolDevice->enableTimer(firstTimerForQuadrature + 1, SurgSim::Devices::LabJack::TIMERMODE_QUADRATURE);
+
+ const int resetQuadrature = SurgSim::Devices::LabJack::FIO3;
+ toolDevice->enableDigitalInput(resetQuadrature);
- const int singleEndedAnalog = SurgSim::Device::LabJack::AIN1;
- toolDevice->enableAnalogInput(singleEndedAnalog, SurgSim::Device::LabJack::Range::RANGE_10);
+ const int singleEndedAnalog = SurgSim::Devices::LabJack::AIN1;
+ toolDevice->enableAnalogInput(singleEndedAnalog, SurgSim::Devices::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,
+ const int positiveAnalogDifferential = SurgSim::Devices::LabJack::AIN2;
+ const int negativeAnalogDifferential = SurgSim::Devices::LabJack::AIN3;
+ toolDevice->enableAnalogInput(positiveAnalogDifferential, SurgSim::Devices::LabJack::Range::RANGE_10,
negativeAnalogDifferential);
- const int xOut = SurgSim::Device::LabJack::DAC1;
+ const int xOut = SurgSim::Devices::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,
+ resetQuadrature, 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");
@@ -288,7 +277,8 @@ int main(int argc, char** argv)
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 += " to move the sphere +/- y-direction. Set FIO" + std::to_string(resetQuadrature);
+ text += " high to reset the quadrature reading. ";
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. ";
diff --git a/SurgSim/Devices/LabJack/linux/LabJackChecksums.cpp b/SurgSim/Devices/LabJack/linux/LabJackChecksums.cpp
index db470b4..f11274f 100644
--- a/SurgSim/Devices/LabJack/linux/LabJackChecksums.cpp
+++ b/SurgSim/Devices/LabJack/linux/LabJackChecksums.cpp
@@ -17,7 +17,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
namespace LabJack
{
@@ -73,5 +73,5 @@ void extendedChecksum(std::array<unsigned char, MAXIMUM_BUFFER>* bytes, int coun
}
}; // namespace LabJack
-}; // namespace Device
-}; // namespace SurgSim
\ No newline at end of file
+}; // namespace Devices
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/LabJack/linux/LabJackChecksums.h b/SurgSim/Devices/LabJack/linux/LabJackChecksums.h
index bff2b81..84196c4 100644
--- a/SurgSim/Devices/LabJack/linux/LabJackChecksums.h
+++ b/SurgSim/Devices/LabJack/linux/LabJackChecksums.h
@@ -22,7 +22,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
/// A collection of checksum functions specifically tailored for the labjackusb driver. These functions are based off
@@ -64,7 +64,7 @@ void normalChecksum(std::array<unsigned char, MAXIMUM_BUFFER>* bytes, int count)
void extendedChecksum(std::array<unsigned char, MAXIMUM_BUFFER>* bytes, int count);
}; // namespace LabJack
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
-#endif // SURGSIM_DEVICES_LABJACK_LINUX_LABJACKCHECKSUMS_H
\ No newline at end of file
+#endif // SURGSIM_DEVICES_LABJACK_LINUX_LABJACKCHECKSUMS_H
diff --git a/SurgSim/Devices/LabJack/linux/LabJackConstants.h b/SurgSim/Devices/LabJack/linux/LabJackConstants.h
index cd37f77..4023efc 100644
--- a/SurgSim/Devices/LabJack/linux/LabJackConstants.h
+++ b/SurgSim/Devices/LabJack/linux/LabJackConstants.h
@@ -20,7 +20,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
namespace LabJack
{
@@ -29,7 +29,7 @@ namespace LabJack
static const int MAXIMUM_BUFFER = 64;
}; // namespace LabJack
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
-#endif // SURGSIM_DEVICES_LABJACK_LINUX_LABJACKCONSTANTS_H
\ No newline at end of file
+#endif // SURGSIM_DEVICES_LABJACK_LINUX_LABJACKCONSTANTS_H
diff --git a/SurgSim/Devices/LabJack/linux/LabJackScaffold.cpp b/SurgSim/Devices/LabJack/linux/LabJackScaffold.cpp
index 2b28e3c..b196a17 100644
--- a/SurgSim/Devices/LabJack/linux/LabJackScaffold.cpp
+++ b/SurgSim/Devices/LabJack/linux/LabJackScaffold.cpp
@@ -17,8 +17,8 @@
#include <algorithm>
#include <array>
-#include <boost/thread/mutex.hpp>
-#include <boost/thread/locks.hpp>
+#include <boost/chrono.hpp>
+#include <boost/thread.hpp>
#include <errno.h>
#include <labjackusb.h> // the low-level LabJack library (aka exodriver)
#include <list>
@@ -40,7 +40,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
namespace
@@ -187,40 +187,19 @@ public:
/// \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,
+ Handle(SurgSim::Devices::LabJack::Model model, SurgSim::Devices::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())
+ m_logger(Framework::Logger::getLogger("Devices/LabJack"))
{
- 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. " <<
+ SURGSIM_LOG_SEVERE(m_logger) << "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 <<
@@ -231,7 +210,7 @@ public:
if (m_connection != LabJack::CONNECTION_USB)
{
- SURGSIM_LOG_SEVERE(m_scaffold->getLogger()) << "Failed to open a device. " <<
+ SURGSIM_LOG_SEVERE(m_logger) << "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: '" <<
@@ -248,7 +227,7 @@ public:
}
catch (int e)
{
- SURGSIM_LOG_SEVERE(m_scaffold->getLogger()) << "Failed to open a device. " <<
+ SURGSIM_LOG_SEVERE(m_logger) << "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 <<
@@ -261,11 +240,19 @@ public:
if (result)
{
const unsigned int dwReserved = 0; // Not used, set to 0.
- m_deviceHandle = LJUSB_OpenDevice(deviceNumber, dwReserved, m_model);
+ int tries = 3;
+ m_deviceHandle = LABJACK_INVALID_HANDLE;
+ while ((m_deviceHandle == LABJACK_INVALID_HANDLE) && (--tries >= 0))
+ {
+ m_deviceHandle = LJUSB_OpenDevice(deviceNumber, dwReserved, m_model);
+ if ((m_deviceHandle == LABJACK_INVALID_HANDLE) && (tries >= 0))
+ {
+ boost::this_thread::sleep_for(boost::chrono::milliseconds(1000));
+ }
+ }
if (m_deviceHandle == LABJACK_INVALID_HANDLE)
{
- SURGSIM_LOG_SEVERE(m_scaffold->getLogger()) << "Failed to open a device." <<
- std::endl <<
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to open a device." << std::endl <<
" Model: '" << m_model << "'. Connection: '" << m_connection << "'. Address: '" <<
m_address << "'." << std::endl <<
" labjackusb error code: " << errno << "." << std::endl;
@@ -274,16 +261,85 @@ public:
}
}
- bool destroy()
+ /// Destructor.
+ ~Handle()
+ {
+ destroy();
+ }
+
+ /// \return Whether or not the wrapped handle is valid.
+ bool isValid() const
+ {
+ return (m_deviceHandle != LABJACK_INVALID_HANDLE);
+ }
+
+ /// Close communication with the hardware.
+ /// \param reset true to cause a hardware reset & USB re-enumeration. Otherwise the hardware's settings will be
+ /// unchanged (i.e., it will continue timing, counting, and outputting).
+ void destroy(bool reset = false)
{
- bool result = false;
if (isValid())
{
+ if (reset)
+ {
+ std::array<BYTE, LabJack::MAXIMUM_BUFFER> sendBytes;
+ sendBytes[1] = 0x99; //Command byte, Reset
+ sendBytes[2] = 2; // Bit 1: Hard Reset. Bit 0: Soft Reset.
+ sendBytes[3] = 0x00;
+
+ int sendBytesSize = 4;
+ int readBytesSize = 4;
+
+ sendBytes[0] = LabJack::normalChecksum8(sendBytes, sendBytesSize);
+
+ const int sent = LJUSB_Write(m_deviceHandle, &(sendBytes[0]), sendBytesSize);
+
+ bool sendResult = true;
+ if (sent < sendBytesSize)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) <<
+ "Failed to write reset command to a device." <<
+ " Model: '" << m_model << "'. Connection: '" << m_connection << "'. Address: '" <<
+ m_address << "'." << std::endl << sendBytesSize << " bytes should have been sent, but only " <<
+ sent << " were actually sent." << std::endl <<
+ " labjackusb error code: " << errno << "." << std::endl;
+ sendResult = false;
+ }
+
+ if (sendResult)
+ {
+ std::array<BYTE, LabJack::MAXIMUM_BUFFER> readBytes;
+ const int read = LJUSB_Read(m_deviceHandle, &(readBytes[0]), readBytesSize);
+ if (read < readBytesSize)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to read response of reset command." <<
+ " Model: '" << m_model << "'. Connection: '" << m_connection << "'. Address: '" <<
+ m_address << "'." << std::endl <<
+ readBytesSize << " bytes were expected, but only " << read << " were received." <<
+ std::endl << " labjackusb error code: " << errno << "." << std::endl;
+ }
+ else if (LabJack::normalChecksum8(readBytes, readBytesSize) != readBytes[0])
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to read response of reset command." <<
+ " Model: '" << m_model << "'. Connection: '" << m_connection << "'. Address: '" <<
+ m_address << "'." << std::endl <<
+ "The checksums are bad." << std::endl << " labjackusb error code: " << errno << "." <<
+ std::endl;
+ }
+ else if (readBytes[3] != 0)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to read response of reset command." <<
+ " Model: '" << m_model << "'. Connection: '" << m_connection << "'. Address: '" <<
+ m_address << "'." << std::endl <<
+ "The device library returned an error code: " << static_cast<int>(readBytes[3]) << "." <<
+ std::endl << " labjackusb error code: " << errno << "." << std::endl;
+ }
+ }
+ }
+
LJUSB_CloseDevice(m_deviceHandle);
m_deviceHandle = LABJACK_INVALID_HANDLE;
- result = true;
}
- return result;
}
/// \return The LabJack SDK's handle wrapped by this Handle.
@@ -302,11 +358,11 @@ private:
/// 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;
+ SurgSim::Devices::LabJack::Model m_model;
/// The connection to the device.
- SurgSim::Device::LabJack::Connection m_connection;
- /// The scaffold.
- std::shared_ptr<LabJackScaffold> m_scaffold;
+ SurgSim::Devices::LabJack::Connection m_connection;
+ /// The logger.
+ std::shared_ptr<Framework::Logger> m_logger;
};
/// The per-device data.
@@ -324,13 +380,20 @@ public:
timerOutputChannels(getTimerOutputChannels(device->getTimers())),
analogInputs(device->getAnalogInputs()),
analogOutputChannels(device->getAnalogOutputs()),
- cachedOutputIndices(false)
+ cachedOutputIndices(false),
+ configured(false)
{
}
~DeviceData()
{
+ if (thread != nullptr)
+ {
+ thread->stop();
+ thread = nullptr;
+ }
+ deviceHandle->destroy(deviceObject->getResetOnDestruct());
}
/// The corresponding device object.
@@ -365,8 +428,10 @@ public:
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).
+ /// Calibration constants. The meaning of each entry is specific to the model (i.e., LabJack::Model).
double calibration[40];
+ /// True if the device has been successfully configured.
+ bool configured;
private:
/// Given all the timers, return just the ones that provide inputs.
@@ -397,18 +462,18 @@ private:
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))
+ 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);
}
@@ -444,9 +509,9 @@ private:
};
LabJackScaffold::LabJackScaffold() :
+ m_logger(Framework::Logger::getLogger("Devices/LabJack")),
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() << ".";
}
@@ -454,17 +519,9 @@ LabJackScaffold::LabJackScaffold() :
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.";
@@ -499,8 +556,8 @@ bool LabJackScaffold::registerDevice(LabJackDevice* device)
if (result && (address.length() > 0))
{
- const SurgSim::Device::LabJack::Model model = device->getModel();
- const SurgSim::Device::LabJack::Connection connection = device->getConnection();
+ const SurgSim::Devices::LabJack::Model model = device->getModel();
+ const SurgSim::Devices::LabJack::Connection connection = device->getConnection();
auto const sameInitialization = std::find_if(m_state->activeDeviceList.cbegin(),
m_state->activeDeviceList.cend(),
@@ -562,7 +619,6 @@ bool LabJackScaffold::registerDevice(LabJackDevice* device)
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
@@ -615,14 +671,18 @@ bool LabJackScaffold::registerDevice(LabJackDevice* device)
}
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));
+ result = info->configured;
+ if (result)
+ {
+ thread->setRate(device->getMaximumUpdateRate());
+ thread->start();
+ info.get()->thread = std::move(thread);
+ m_state->activeDeviceList.emplace_back(std::move(info));
+ }
}
}
+ SURGSIM_LOG_IF(result, m_logger, INFO) << "Device " << device->getName() << " registered.";
return result;
}
@@ -635,22 +695,15 @@ bool LabJackScaffold::unregisterDevice(const LabJackDevice* const device)
[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;
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << " unregistered.";
}
}
- if (!found)
- {
- SURGSIM_LOG_CRITICAL(m_logger) << "Attempted to release a non-registered device named '" <<
- device->getName() << ".";
- }
+ SURGSIM_LOG_IF(!found, m_logger, CRITICAL) << "Attempted to release a non-registered device named '" <<
+ device->getName() << ".";
return found;
}
@@ -687,14 +740,12 @@ bool LabJackScaffold::runInputFrame(LabJackScaffold::DeviceData* info)
info->cachedOutputIndices = true;
}
- if (!info->deviceObject->hasOutputProducer() || info->cachedOutputIndices)
+ if (!updateDevice(info))
{
- if (!updateDevice(info))
- {
- return false;
- }
- info->deviceObject->pushInput();
+ return false;
}
+ info->deviceObject->pushInput();
+
return true;
}
@@ -1007,17 +1058,6 @@ bool LabJackScaffold::updateDevice(LabJackScaffold::DeviceData* info)
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;
@@ -1049,9 +1089,10 @@ std::shared_ptr<LabJackScaffold> LabJackScaffold::getOrCreateSharedInstance()
return sharedInstance.get();
}
-bool LabJackScaffold::configureDevice(DeviceData* deviceData)
+void LabJackScaffold::configureDevice(DeviceData* deviceData)
{
- return configureClockAndTimers(deviceData) && configureDigital(deviceData) && configureAnalog(deviceData);
+ deviceData->configured =
+ configureClockAndTimers(deviceData) && configureDigital(deviceData) && configureAnalog(deviceData);
}
bool LabJackScaffold::configureClockAndTimers(DeviceData* deviceData)
@@ -1503,12 +1544,5 @@ bool LabJackScaffold::configureDigital(DeviceData* deviceData)
return result;
}
-std::shared_ptr<SurgSim::Framework::Logger> LabJackScaffold::getLogger() const
-{
- return m_logger;
-}
-
-
-
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/LabJack/linux/LabJackTypeConverters.cpp b/SurgSim/Devices/LabJack/linux/LabJackTypeConverters.cpp
index cab7bb4..e617ee8 100644
--- a/SurgSim/Devices/LabJack/linux/LabJackTypeConverters.cpp
+++ b/SurgSim/Devices/LabJack/linux/LabJackTypeConverters.cpp
@@ -18,7 +18,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
namespace LabJack
{
@@ -67,5 +67,5 @@ uint16_t uint16FromChars(const std::array<unsigned char, LabJack::MAXIMUM_BUFFER
}
}; // namespace LabJack
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/LabJack/linux/LabJackTypeConverters.h b/SurgSim/Devices/LabJack/linux/LabJackTypeConverters.h
index 567ca08..8bafaca 100644
--- a/SurgSim/Devices/LabJack/linux/LabJackTypeConverters.h
+++ b/SurgSim/Devices/LabJack/linux/LabJackTypeConverters.h
@@ -22,7 +22,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
namespace LabJack
{
@@ -54,7 +54,7 @@ uint32_t uint32FromChars(const std::array<unsigned char, LabJack::MAXIMUM_BUFFER
uint16_t uint16FromChars(const std::array<unsigned char, LabJack::MAXIMUM_BUFFER> &bytes, int startIndex, int count);
}; // namespace LabJack
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
-#endif // SURGSIM_DEVICES_LABJACK_LINUX_LABJACKTYPECONVERTERS_H
\ No newline at end of file
+#endif // SURGSIM_DEVICES_LABJACK_LINUX_LABJACKTYPECONVERTERS_H
diff --git a/SurgSim/Devices/LabJack/win32/LabJackScaffold.cpp b/SurgSim/Devices/LabJack/win32/LabJackScaffold.cpp
index 6ed2908..eb49090 100644
--- a/SurgSim/Devices/LabJack/win32/LabJackScaffold.cpp
+++ b/SurgSim/Devices/LabJack/win32/LabJackScaffold.cpp
@@ -16,8 +16,8 @@
#include "SurgSim/Devices/LabJack/LabJackScaffold.h"
#include <algorithm>
-#include <boost/thread/mutex.hpp>
-#include <boost/thread/locks.hpp>
+#include <boost/chrono.hpp>
+#include <boost/thread.hpp>
#include <iostream>
#include <LabJackUD.h> // the high-level LabJack library.
#include <list>
@@ -35,7 +35,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
namespace
@@ -89,15 +89,33 @@ public:
m_model(model),
m_connection(connection),
m_deviceHandle(LABJACK_INVALID_HANDLE),
- m_scaffold(LabJackScaffold::getOrCreateSharedInstance())
+ m_logger(Framework::Logger::getLogger("Devices/LabJack"))
{
- create();
+ 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.
+ }
+
+ int tries = 3;
+ LJ_ERROR error = LJE_MIN_USER_ERROR;
+ while (!isOk(error) && (--tries >= 0))
+ {
+ error = OpenLabJack(m_model, m_connection, m_address.c_str(), firstFound, &m_deviceHandle);
+ if (!isOk(error) && (tries >= 0))
+ {
+ boost::this_thread::sleep_for(boost::chrono::milliseconds(1000));
+ }
+ }
+ SURGSIM_LOG_IF(!isOk(error), m_logger, SEVERE) <<
+ "Failed to initialize a device. Model: " << m_model << ". Connection: " << m_connection << ". Address: '" <<
+ m_address << "'." << std::endl << formatErrorMessage(error);
}
/// Destructor.
~Handle()
{
- SURGSIM_ASSERT(!isValid()) << "Expected destroy() to be called before Handle object destruction.";
+ destroy();
}
/// \return Whether or not the wrapped handle is valid.
@@ -106,41 +124,23 @@ public:
return (m_deviceHandle != LABJACK_INVALID_HANDLE);
}
- /// Helper function called by the constructor to open the LabJack device for communications.
- void create()
+ /// Close communication with the hardware.
+ /// \param reset true to cause a hardware reset & USB re-enumeration. Otherwise the hardware's settings will be
+ /// unchanged (i.e., it will continue timing, counting, and outputting).
+ void destroy(bool reset = false)
{
- 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)
+ if (reset)
{
- m_deviceHandle = LABJACK_INVALID_HANDLE;
+ const LJ_ERROR error = ResetLabJack(m_deviceHandle);
+ SURGSIM_LOG_IF(!isOk(error), m_logger, SEVERE) <<
+ "Failed to reset the LabJack device. Model: " << m_model << ". Connection: " <<
+ m_connection << ". Address: '" << m_address << "'." << std::endl << formatErrorMessage(error);
}
+
+ m_deviceHandle = LABJACK_INVALID_HANDLE;
}
- return result;
}
/// \return The LabJackUD's handle wrapped by this Handle.
@@ -162,8 +162,8 @@ private:
LabJack::Model m_model;
/// The connection to the device.
LabJack::Connection m_connection;
- /// The scaffold.
- std::shared_ptr<LabJackScaffold> m_scaffold;
+ /// The logger.
+ std::shared_ptr<Framework::Logger> m_logger;
};
/// The per-device data.
@@ -173,21 +173,28 @@ public:
/// Initialize the data, creating a thread.
DeviceData(LabJackDevice* device, std::unique_ptr<Handle>&& handle) :
deviceObject(device),
- deviceHandle(std::move(handle)),
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)
+ cachedOutputIndices(false),
+ configured(false)
{
}
~DeviceData()
{
+ if (thread != nullptr)
+ {
+ thread->stop();
+ thread = nullptr;
+ }
+ deviceHandle->destroy(deviceObject->getResetOnDestruct());
}
/// The corresponding device object.
@@ -222,6 +229,8 @@ public:
std::unordered_map<int, int> analogInputIndices;
/// True if the output indices have been cached.
bool cachedOutputIndices;
+ /// True if the device has been successfully configured.
+ bool configured;
private:
/// Given all the timers, return just the ones that provide inputs.
@@ -252,18 +261,18 @@ private:
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))
+ 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);
}
@@ -299,27 +308,18 @@ private:
};
LabJackScaffold::LabJackScaffold() :
+ m_logger(Framework::Logger::getLogger("Devices/LabJack")),
m_state(new StateData)
{
- m_logger = SurgSim::Framework::Logger::getLogger("LabJack device");
- SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold created. LabJackUD driver version: " <<
- GetDriverVersion() << ".";
+ 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.";
@@ -424,7 +424,6 @@ bool LabJackScaffold::registerDevice(LabJackDevice* device)
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
@@ -479,14 +478,18 @@ bool LabJackScaffold::registerDevice(LabJackDevice* device)
}
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));
+ result = info->configured;
+ if (result)
+ {
+ thread->setRate(device->getMaximumUpdateRate());
+ thread->start();
+ info.get()->thread = std::move(thread);
+ m_state->activeDeviceList.emplace_back(std::move(info));
+ }
}
}
+ SURGSIM_LOG_IF(result, m_logger, INFO) << "Device " << device->getName() << " registered.";
return result;
}
@@ -499,22 +502,15 @@ bool LabJackScaffold::unregisterDevice(const LabJackDevice* const device)
[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;
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << " unregistered.";
}
}
- if (!found)
- {
- SURGSIM_LOG_CRITICAL(m_logger) << "Attempted to release a non-registered device named '" <<
- device->getName() << ".";
- }
+ SURGSIM_LOG_IF(!found, m_logger, CRITICAL) << "Attempted to release a non-registered device named '" <<
+ device->getName() << ".";
return found;
}
@@ -551,14 +547,12 @@ bool LabJackScaffold::runInputFrame(LabJackScaffold::DeviceData* info)
info->cachedOutputIndices = true;
}
- if (!info->deviceObject->hasOutputProducer() || info->cachedOutputIndices)
+ if (!updateDevice(info))
{
- if (!updateDevice(info))
- {
- return false;
- }
- info->deviceObject->pushInput();
+ return false;
}
+ info->deviceObject->pushInput();
+
return true;
}
@@ -772,17 +766,6 @@ bool LabJackScaffold::updateDevice(LabJackScaffold::DeviceData* info)
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;
@@ -814,7 +797,7 @@ std::shared_ptr<LabJackScaffold> LabJackScaffold::getOrCreateSharedInstance()
return sharedInstance.get();
}
-bool LabJackScaffold::configureDevice(DeviceData* deviceData)
+void LabJackScaffold::configureDevice(DeviceData* deviceData)
{
LJ_HANDLE rawHandle = deviceData->deviceHandle->get();
@@ -825,7 +808,8 @@ bool LabJackScaffold::configureDevice(DeviceData* deviceData)
"Failed to reset configuration for a device named '" << deviceData->deviceObject->getName() << "." <<
std::endl << formatErrorMessage(error);
- return result && configureClockAndTimers(deviceData) && configureDigital(deviceData) && configureAnalog(deviceData);
+ deviceData->configured = result &&
+ configureClockAndTimers(deviceData) && configureDigital(deviceData) && configureAnalog(deviceData);
}
bool LabJackScaffold::configureClockAndTimers(DeviceData* deviceData)
@@ -1022,10 +1006,5 @@ bool LabJackScaffold::configureDigital(DeviceData* deviceData)
return true;
}
-std::shared_ptr<SurgSim::Framework::Logger> LabJackScaffold::getLogger() const
-{
- return m_logger;
-}
-
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/Leap/CMakeLists.txt b/SurgSim/Devices/Leap/CMakeLists.txt
new file mode 100644
index 0000000..d842861
--- /dev/null
+++ b/SurgSim/Devices/Leap/CMakeLists.txt
@@ -0,0 +1,59 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013-2015, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT 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(LeapSdk REQUIRED)
+
+include_directories(SYSTEM
+ ${LEAPSDK_INCLUDE_DIR}
+)
+
+set(LIBS
+ SurgSimDataStructures
+ SurgSimFramework
+ SurgSimInput
+ ${LEAPSDK_LIBRARY}
+)
+
+set(LEAP_DEVICE_SOURCES
+ LeapDevice.cpp
+ LeapScaffold.cpp
+ LeapUtilities.cpp
+)
+
+set(LEAP_DEVICE_HEADERS
+ LeapDevice.h
+ LeapScaffold.h
+ LeapUtilities.h
+)
+
+set(DEVICE_HEADERS ${DEVICE_HEADERS} Leap/LeapDevice.h Leap/LeapUtilities.h PARENT_SCOPE)
+
+surgsim_add_library(
+ LeapDevice
+ "${LEAP_DEVICE_SOURCES}"
+ "${LEAP_DEVICE_HEADERS}"
+)
+target_link_libraries(LeapDevice ${LIBS})
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+
+ if(GLUT_FOUND)
+ add_subdirectory(VisualTest)
+ endif(GLUT_FOUND)
+endif()
+
+set_target_properties(LeapDevice PROPERTIES FOLDER "Devices")
+
diff --git a/SurgSim/Devices/Leap/Leap.dox b/SurgSim/Devices/Leap/Leap.dox
new file mode 100644
index 0000000..d5f2bb5
--- /dev/null
+++ b/SurgSim/Devices/Leap/Leap.dox
@@ -0,0 +1,16 @@
+/*!
+
+\page Leap Leap: Leap Motion Hand Tracking Cameras
+
+This adds support for <A HREF=https://www.leapmotion.com/>Leap Motion</A> hand tracking cameras.
+
+Dependencies
+------------
+Leap Motion SDK: The SDK is available for Windows and GNU/Linux on Leap
+Motion's Developer <A HREF="https://developer.leapmotion.com/">website</A>.
+To build OpenSurgSim with Leap Motion support, create a LEAPSDK_DIR environment
+variable that points to the location of the LeapSDK directory. This directory
+should include the 'include' and 'lib' directories. Also, in CMake enable
+BUILD_DEVICE_LEAP.
+
+*/
diff --git a/SurgSim/Devices/Leap/LeapDevice.cpp b/SurgSim/Devices/Leap/LeapDevice.cpp
new file mode 100644
index 0000000..ad40461
--- /dev/null
+++ b/SurgSim/Devices/Leap/LeapDevice.cpp
@@ -0,0 +1,128 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Leap/LeapDevice.h"
+
+#include <algorithm>
+
+#include "SurgSim/Devices/Leap/LeapScaffold.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::LeapDevice, LeapDevice);
+
+LeapDevice::LeapDevice(const std::string& name) :
+ Input::CommonDevice(name, LeapScaffold::buildDeviceInputData()),
+ m_handType(HANDTYPE_RIGHT),
+ m_isProvidingImages(false)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(LeapDevice, bool, UseHmdTrackingMode, isUsingHmdTrackingMode,
+ setUseHmdTrackingMode);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(LeapDevice, bool, ProvideImages, isProvidingImages, setProvideImages);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(LeapDevice, SurgSim::Devices::HandType, HandType, getHandType, setHandType);
+}
+
+
+LeapDevice::~LeapDevice()
+{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+
+void LeapDevice::setHandType(HandType type)
+{
+ m_handType = type;
+}
+
+HandType LeapDevice::getHandType() const
+{
+ return m_handType;
+}
+
+void LeapDevice::setUseHmdTrackingMode(bool useHmdTrackingMode)
+{
+ if (isInitialized())
+ {
+ m_scaffold->setUseHmdTrackingMode(useHmdTrackingMode);
+ }
+ m_requestedHmdTrackingMode = useHmdTrackingMode;
+}
+
+bool LeapDevice::isUsingHmdTrackingMode() const
+{
+ if (isInitialized())
+ {
+ return m_scaffold->isUsingHmdTrackingMode();
+ }
+ else
+ {
+ return m_requestedHmdTrackingMode.getValue();
+ }
+}
+
+void LeapDevice::setProvideImages(bool produceImages)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot call setProvideImages after LeapDevice is initialized";
+ m_isProvidingImages = produceImages;
+}
+
+bool LeapDevice::isProvidingImages() const
+{
+ return m_isProvidingImages;
+}
+
+bool LeapDevice::initialize()
+{
+ SURGSIM_ASSERT(!isInitialized()) << getName() << "is already initialized, cannot initialize again.";
+ auto scaffold = LeapScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold != nullptr) << getName() << " initialization failed, cannot get scaffold.";
+
+ bool registered = false;
+ if (scaffold->registerDevice(this))
+ {
+ if (m_requestedHmdTrackingMode.hasValue())
+ {
+ scaffold->setUseHmdTrackingMode(m_requestedHmdTrackingMode.getValue());
+ }
+ m_scaffold = std::move(scaffold);
+ registered = true;
+ }
+
+ return registered;
+}
+
+bool LeapDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized()) << getName() << " is not initialized, cannot finalize.";
+ bool success = m_scaffold->unregisterDevice(this);
+ m_scaffold.reset();
+ return success;
+}
+
+bool LeapDevice::isInitialized() const
+{
+ return (m_scaffold != nullptr);
+}
+
+
+}; // namespace Devices
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Leap/LeapDevice.h b/SurgSim/Devices/Leap/LeapDevice.h
new file mode 100644
index 0000000..62fdfe9
--- /dev/null
+++ b/SurgSim/Devices/Leap/LeapDevice.h
@@ -0,0 +1,130 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_LEAP_LEAPDEVICE_H
+#define SURGSIM_DEVICES_LEAP_LEAPDEVICE_H
+
+#include <memory>
+
+#include "SurgSim/DataStructures/OptionalValue.h"
+#include "SurgSim/Input/CommonDevice.h"
+
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+class LeapScaffold;
+
+SURGSIM_STATIC_REGISTRATION(LeapDevice);
+
+enum HandType : SURGSIM_ENUM_TYPE;
+
+/// A class implementing the communication with one hand tracked by Leap Motion camera
+///
+/// \par Application input provided by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | image | "left" | Left infrared image, each pixel value is between 0 and 1. |
+/// | image | "right" | Right infrared image, each pixel value is between 0 and 1. |
+/// | image | "left_distortion" | Left distortion calibration map |
+/// | image | "right_distortion" | Right distortion calibration map |
+/// | pose | "pose" | %Hand pose |
+/// | pose | "ThumbProximal" | %Pose of thumb proximal joint |
+/// | pose | "ThumbIntermediate" | %Pose of thumb intermediate joint |
+/// | pose | "ThumbDistal" | %Pose of thumb distal joint |
+/// | pose | "IndexFingerProximal" | %Pose of index finger proximal joint |
+/// | pose | "IndexFingerIntermediate" | %Pose of index finger intermediate joint |
+/// | pose | "IndexFingerDistal" | %Pose of index finger distal joint |
+/// | pose | "MiddleFingerProximal" | %Pose of middle finger proximal joint |
+/// | pose | "MiddleFingerIntermediate" | %Pose of middle finger intermediate joint |
+/// | pose | "MiddleFingerDistal" | %Pose of middle finger distal joint |
+/// | pose | "RingFingerProximal" | %Pose of ring finger proximal joint |
+/// | pose | "RingFingerIntermediate" | %Pose of ring finger intermediate joint |
+/// | pose | "RingFingerDistal" | %Pose of ring finger distal joint |
+/// | pose | "SmallFingerProximal" | %Pose of small finger proximal joint |
+/// | pose | "SmallFingerIntermediate" | %Pose of small finger intermediate joint |
+/// | pose | "SmallFingerDistal" | %Pose of small finger distal joint |
+///
+/// \par Application output used by the device: none.
+///
+/// \sa SurgSim::Devices::LeapScaffold
+class LeapDevice : public SurgSim::Input::CommonDevice
+{
+public:
+ /// Constructor.
+ /// \param name A unique name for the device that will be used by the application.
+ explicit LeapDevice(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Devices::LeapDevice);
+
+ /// Destructor.
+ virtual ~LeapDevice();
+
+ /// Set the type of hand
+ /// \param type The hand type, either HANDTYPE_LEFT or HANDTYPE_RIGHT
+ void setHandType(HandType type);
+
+ /// Get the type of hand
+ /// \return The hand type, either HANDTYPE_LEFT or HANDTYPE_RIGHT
+ HandType getHandType() const;
+
+ /// Set the hand tracking mode to HMD
+ /// This is a global setting that optimizes hand tracking based on the Leap camera
+ /// placement. The default is desktop mode, where the camera is placed
+ /// face up on a desktop. Use Hmd mode when the camera is attached to
+ /// the front of a head mounted display.
+ /// \param useHmdTrackingMode True if Hmd tracking mode is to be used
+ void setUseHmdTrackingMode(bool useHmdTrackingMode);
+
+ /// Is Using HMD Tracking Mode
+ /// \return True is HMD Tracking Mode is on
+ bool isUsingHmdTrackingMode() const;
+
+ /// Set if the device should provide the stereo infrared images
+ /// \param provideImages True if providing images
+ void setProvideImages(bool provideImages);
+
+ /// Get if the device should provide the stereo infrared images
+ /// \return True if providing images
+ bool isProvidingImages() const;
+
+ bool initialize() override;
+
+ bool isInitialized() const override;
+
+private:
+ friend class LeapScaffold;
+
+ bool finalize() override;
+
+ std::shared_ptr<LeapScaffold> m_scaffold;
+
+ HandType m_handType;
+
+ /// Tracking mode
+ DataStructures::OptionalValue<bool> m_requestedHmdTrackingMode;
+
+ /// Request Camera Images mode
+ bool m_isProvidingImages;
+};
+
+};
+};
+
+SURGSIM_SERIALIZABLE_ENUM(SurgSim::Devices::HandType, (HANDTYPE_LEFT)(HANDTYPE_RIGHT));
+
+#endif //SURGSIM_DEVICES_LEAP_LEAPDEVICE_H
diff --git a/SurgSim/Devices/Leap/LeapScaffold.cpp b/SurgSim/Devices/Leap/LeapScaffold.cpp
new file mode 100644
index 0000000..26465b6
--- /dev/null
+++ b/SurgSim/Devices/Leap/LeapScaffold.cpp
@@ -0,0 +1,467 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Leap/LeapScaffold.h"
+
+#include <boost/thread/mutex.hpp>
+#include <Leap.h>
+#include <list>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.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::Math::RigidTransform3d;
+
+
+namespace
+{
+
+RigidTransform3d makeRigidTransform(const Leap::Matrix& rotation, const Leap::Vector& translation,
+ bool isRightHanded = true)
+{
+ Leap::Matrix matrix = rotation;
+ if (!isRightHanded)
+ {
+ matrix.zBasis *= -1.0;
+ }
+
+ // Convert milimeters to meters
+ matrix.origin = translation * 0.001f;
+
+ SurgSim::Math::Matrix44d transform;
+ matrix.toArray4x4(transform.data());
+ return RigidTransform3d(transform.transpose());
+}
+
+void updateDataGroup(const Leap::Hand& hand, SurgSim::DataStructures::DataGroup* inputData)
+{
+ static const std::array<std::string, 5> fingerNames = {"Thumb", "IndexFinger", "MiddleFinger", "RingFinger",
+ "SmallFinger"};
+ static const std::array<std::string, 4> boneNames = {"Metacarpal", "Proximal", "Intermediate", "Distal"};
+
+ inputData->poses().set("pose", makeRigidTransform(hand.basis(), hand.palmPosition(), hand.isRight()));
+
+ std::string name;
+ const Leap::FingerList fingers = hand.fingers();
+ for (const Leap::Finger& finger : fingers)
+ {
+ for (int boneType = Leap::Bone::TYPE_PROXIMAL; boneType <= Leap::Bone::TYPE_DISTAL; ++boneType)
+ {
+ Leap::Bone bone = finger.bone(static_cast<Leap::Bone::Type>(boneType));
+ name = fingerNames[finger.type()] + boneNames[boneType];
+ inputData->poses().set(name, makeRigidTransform(bone.basis(), bone.prevJoint(), hand.isRight()));
+ inputData->scalars().set(name + "Length", bone.length() / 1000.0);
+ inputData->scalars().set(name + "Width", bone.width() / 1000.0);
+ }
+ }
+}
+
+};
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+class LeapScaffold::Listener : public Leap::Listener
+{
+public:
+ Listener() :
+ m_scaffold(LeapScaffold::getOrCreateSharedInstance()),
+ m_logger(Framework::Logger::getLogger("Devices/Leap"))
+ {
+ }
+
+ void onConnect(const Leap::Controller&) override
+ {
+ SURGSIM_LOG_INFO(m_logger) << "Connected to Leap Motion camera";
+ auto scaffold = m_scaffold.lock();
+ if (scaffold != nullptr)
+ {
+ scaffold->handleConnect();
+ }
+ }
+
+ void onDisconnect(const Leap::Controller&) override
+ {
+ SURGSIM_LOG_INFO(m_logger) << "Diconnected from Leap Motion camera";
+ }
+
+ void onFrame(const Leap::Controller&) override
+ {
+ auto scaffold = m_scaffold.lock();
+ if (scaffold != nullptr)
+ {
+ scaffold->handleFrame();
+ }
+ }
+
+private:
+ std::weak_ptr<Devices::LeapScaffold> m_scaffold;
+ std::shared_ptr<Framework::Logger> m_logger;
+};
+
+struct LeapScaffold::DeviceData
+{
+ explicit DeviceData(LeapDevice* device) :
+ deviceObject(device),
+ handId(std::numeric_limits<int32_t>::quiet_NaN())
+ {
+ }
+
+ /// The corresponding device object.
+ LeapDevice* const deviceObject;
+
+ /// A unique id for the hand, assigned by the Leap SDK
+ int32_t handId;
+};
+
+struct LeapScaffold::StateData
+{
+ // The SDK's interface to a single Leap Motion Camera
+ Leap::Controller controller;
+
+ /// A listener that receives updates from the Leap SDK
+ std::unique_ptr<Listener> listener;
+
+ /// The list of known devices.
+ std::list<std::unique_ptr<DeviceData>> activeDevices;
+
+ /// The mutex that protects the list of known devices.
+ boost::mutex mutex;
+};
+
+LeapScaffold::LeapScaffold() :
+ m_state(new StateData),
+ m_logger(Framework::Logger::getLogger("Devices/Leap"))
+{
+}
+
+LeapScaffold::~LeapScaffold()
+{
+ if (m_state->listener != nullptr)
+ {
+ m_state->controller.removeListener(*m_state->listener);
+ }
+}
+
+bool LeapScaffold::registerDevice(LeapDevice* device)
+{
+ bool success = true;
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ const std::string deviceName = device->getName();
+ auto sameName = [&deviceName](const std::unique_ptr<DeviceData>& info)
+ {
+ return info->deviceObject->getName() == deviceName;
+ };
+ auto found = std::find_if(m_state->activeDevices.cbegin(), m_state->activeDevices.cend(), sameName);
+
+ if (found == m_state->activeDevices.end())
+ {
+ std::unique_ptr<DeviceData> info(new DeviceData(device));
+ success = doRegisterDevice(info.get());
+ if (success)
+ {
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << ": Registered";
+ m_state->activeDevices.emplace_back(std::move(info));
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Device " << device->getName() << ": Not registered";
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Tried to register a device when the same name, '" <<
+ device->getName() << "', is already present!";
+ success = false;
+ }
+
+ return success;
+}
+
+bool LeapScaffold::doRegisterDevice(DeviceData* info)
+{
+ if (m_state->listener == nullptr)
+ {
+ m_state->listener = std::unique_ptr<Listener>(new Listener);
+ m_state->controller.addListener(*m_state->listener);
+ }
+
+ if (info->deviceObject->isProvidingImages())
+ {
+ m_state->controller.setPolicy(Leap::Controller::PolicyFlag::POLICY_IMAGES);
+ }
+
+ return true;
+}
+
+bool LeapScaffold::unregisterDevice(const LeapDevice* device)
+{
+ bool success = true;
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ auto& devices = m_state->activeDevices;
+ if (device->isProvidingImages())
+ {
+ auto providingImages = [](const std::unique_ptr<DeviceData>& info)
+ {
+ return info->deviceObject->isProvidingImages();
+ };
+ if (std::find_if(devices.begin(), devices.end(), providingImages) == devices.end())
+ {
+ m_state->controller.clearPolicy(Leap::Controller::PolicyFlag::POLICY_IMAGES);
+ }
+ }
+
+ auto sameDevice = [device](const std::unique_ptr<DeviceData>& info)
+ {
+ return info->deviceObject == device;
+ };
+ auto info = std::find_if(devices.begin(), devices.end(), sameDevice);
+ if (info != devices.end())
+ {
+ devices.erase(info);
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << ": Unregistered";
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Attempted to release a non-registered device named '" <<
+ device->getName() << ".";
+ success = false;
+ }
+
+ return success;
+}
+
+void LeapScaffold::handleConnect()
+{
+ m_state->controller.setPolicy(Leap::Controller::PolicyFlag::POLICY_BACKGROUND_FRAMES);
+
+ for (auto& device : m_state->activeDevices)
+ {
+ if (device->deviceObject->isProvidingImages())
+ {
+ m_state->controller.setPolicy(Leap::Controller::PolicyFlag::POLICY_IMAGES);
+ break;
+ }
+ }
+}
+
+void LeapScaffold::handleFrame()
+{
+ updateHandData();
+ updateImageData();
+
+ for (auto& device : m_state->activeDevices)
+ {
+ device->deviceObject->pushInput();
+ }
+}
+
+void LeapScaffold::updateHandData()
+{
+ std::list<DeviceData*> unassignedDevices;
+ for (auto& device : m_state->activeDevices)
+ {
+ unassignedDevices.push_back(device.get());
+ }
+
+ std::list<Leap::HandList::const_iterator> newHands;
+ Leap::HandList hands = m_state->controller.frame().hands();
+ for (auto hand = hands.begin(); hand != hands.end(); ++hand)
+ {
+ auto sameHandId = [hand](const std::unique_ptr<DeviceData>& info)
+ {
+ return info->handId == (*hand).id();
+ };
+ auto assignedDevice = std::find_if(m_state->activeDevices.begin(), m_state->activeDevices.end(), sameHandId);
+
+ if (assignedDevice != m_state->activeDevices.end())
+ {
+ updateDataGroup(*hand, &(*assignedDevice)->deviceObject->getInputData());
+ unassignedDevices.remove(assignedDevice->get());
+ }
+ else
+ {
+ newHands.push_back(hand);
+ }
+ }
+
+ static auto higherConfidence = [](const Leap::HandList::const_iterator &a, const Leap::HandList::const_iterator &b)
+ {
+ return (*a).confidence() > (*b).confidence();
+ };
+ newHands.sort(higherConfidence);
+
+ for (auto& newHand : newHands)
+ {
+ auto sameHandType = [newHand](DeviceData* info)
+ {
+ return (info->deviceObject->getHandType() == HANDTYPE_LEFT) == (*newHand).isLeft();
+ };
+ auto unassignedDevice = std::find_if(unassignedDevices.begin(), unassignedDevices.end(), sameHandType);
+
+ if (unassignedDevice != unassignedDevices.end())
+ {
+ (*unassignedDevice)->handId = (*newHand).id();
+ updateDataGroup(*newHand, &(*unassignedDevice)->deviceObject->getInputData());
+ unassignedDevices.remove(*unassignedDevice);
+ }
+ }
+
+ for(auto& unassignedDevice : unassignedDevices)
+ {
+ unassignedDevice->deviceObject->getInputData().poses().resetAll();
+ }
+}
+
+void LeapScaffold::updateImageData()
+{
+ Leap::ImageList images = m_state->controller.frame().images();
+ if (!images.isEmpty())
+ {
+ typedef DataStructures::DataGroup::ImageType ImageType;
+ ImageType leftImage(images[0].width(), images[0].height(), 1, images[0].data());
+ ImageType rightImage(images[1].width(), images[1].height(), 1, images[1].data());
+ ImageType leftDistortion(images[0].distortionWidth() / 2, images[0].distortionHeight(), 2,
+ images[0].distortion());
+ ImageType rightDistortion(images[1].distortionWidth() / 2, images[1].distortionHeight(), 2,
+ images[1].distortion());
+
+ // scale values to 0..1
+ leftImage.getAsVector() *= (1.0f / 255.0f);
+ rightImage.getAsVector() *= (1.0f / 255.0f);
+
+ for (auto& device : m_state->activeDevices)
+ {
+ if (device->deviceObject->isProvidingImages())
+ {
+ device->deviceObject->getInputData().images().set("left", leftImage);
+ device->deviceObject->getInputData().images().set("right", rightImage);
+ device->deviceObject->getInputData().images().set("left_distortion", leftDistortion);
+ device->deviceObject->getInputData().images().set("right_distortion", rightDistortion);
+ }
+ else
+ {
+ device->deviceObject->getInputData().images().resetAll();
+ }
+ }
+ }
+ else
+ {
+ for (auto& device : m_state->activeDevices)
+ {
+ device->deviceObject->getInputData().images().resetAll();
+ }
+ }
+}
+
+std::shared_ptr<LeapScaffold> LeapScaffold::getOrCreateSharedInstance()
+{
+ static auto creator = []()
+ {
+ return std::shared_ptr<LeapScaffold>(new LeapScaffold);
+ };
+ static Framework::SharedInstance<LeapScaffold> sharedInstance(creator);
+ return sharedInstance.get();
+}
+
+DataStructures::DataGroup LeapScaffold::buildDeviceInputData()
+{
+ DataStructures::DataGroupBuilder builder;
+
+ builder.addImage("left");
+ builder.addImage("right");
+ builder.addImage("left_distortion");
+ builder.addImage("right_distortion");
+
+ builder.addPose("pose");
+ builder.addPose("ThumbProximal");
+ builder.addPose("ThumbIntermediate");
+ builder.addPose("ThumbDistal");
+ builder.addPose("IndexFingerProximal");
+ builder.addPose("IndexFingerIntermediate");
+ builder.addPose("IndexFingerDistal");
+ builder.addPose("MiddleFingerProximal");
+ builder.addPose("MiddleFingerIntermediate");
+ builder.addPose("MiddleFingerDistal");
+ builder.addPose("RingFingerProximal");
+ builder.addPose("RingFingerIntermediate");
+ builder.addPose("RingFingerDistal");
+ builder.addPose("SmallFingerProximal");
+ builder.addPose("SmallFingerIntermediate");
+ builder.addPose("SmallFingerDistal");
+
+ builder.addScalar("ThumbProximalWidth");
+ builder.addScalar("ThumbIntermediateWidth");
+ builder.addScalar("ThumbDistalWidth");
+ builder.addScalar("IndexFingerProximalWidth");
+ builder.addScalar("IndexFingerIntermediateWidth");
+ builder.addScalar("IndexFingerDistalWidth");
+ builder.addScalar("MiddleFingerProximalWidth");
+ builder.addScalar("MiddleFingerIntermediateWidth");
+ builder.addScalar("MiddleFingerDistalWidth");
+ builder.addScalar("RingFingerProximalWidth");
+ builder.addScalar("RingFingerIntermediateWidth");
+ builder.addScalar("RingFingerDistalWidth");
+ builder.addScalar("SmallFingerProximalWidth");
+ builder.addScalar("SmallFingerIntermediateWidth");
+ builder.addScalar("SmallFingerDistalWidth");
+
+ builder.addScalar("ThumbProximalLength");
+ builder.addScalar("ThumbIntermediateLength");
+ builder.addScalar("ThumbDistalLength");
+ builder.addScalar("IndexFingerProximalLength");
+ builder.addScalar("IndexFingerIntermediateLength");
+ builder.addScalar("IndexFingerDistalLength");
+ builder.addScalar("MiddleFingerProximalLength");
+ builder.addScalar("MiddleFingerIntermediateLength");
+ builder.addScalar("MiddleFingerDistalLength");
+ builder.addScalar("RingFingerProximalLength");
+ builder.addScalar("RingFingerIntermediateLength");
+ builder.addScalar("RingFingerDistalLength");
+ builder.addScalar("SmallFingerProximalLength");
+ builder.addScalar("SmallFingerIntermediateLength");
+ builder.addScalar("SmallFingerDistalLength");
+
+ return builder.createData();
+}
+
+void LeapScaffold::setUseHmdTrackingMode(bool useHmdTrackingMode)
+{
+ if (useHmdTrackingMode)
+ {
+ m_state->controller.setPolicy(Leap::Controller::PolicyFlag::POLICY_OPTIMIZE_HMD);
+ }
+ else
+ {
+ m_state->controller.clearPolicy(Leap::Controller::PolicyFlag::POLICY_OPTIMIZE_HMD);
+ }
+}
+
+bool LeapScaffold::isUsingHmdTrackingMode() const
+{
+ return m_state->controller.isPolicySet(Leap::Controller::PolicyFlag::POLICY_OPTIMIZE_HMD);
+}
+
+}; // namespace Devices
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Leap/LeapScaffold.h b/SurgSim/Devices/Leap/LeapScaffold.h
new file mode 100644
index 0000000..80b0796
--- /dev/null
+++ b/SurgSim/Devices/Leap/LeapScaffold.h
@@ -0,0 +1,115 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_LEAP_LEAPSCAFFOLD_H
+#define SURGSIM_DEVICES_LEAP_LEAPSCAFFOLD_H
+
+#include <memory>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Devices/Leap/LeapDevice.h"
+
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+class Logger;
+};
+
+namespace Devices
+{
+
+class LeapDevice;
+
+/// A class that manages Leap devices
+///
+/// \sa SurgSim::Devices::LeapDevice
+class LeapScaffold
+{
+public:
+ /// Destructor.
+ virtual ~LeapScaffold();
+
+private:
+ /// Internal shared state data type.
+ struct StateData;
+
+ /// Internal per-device information.
+ struct DeviceData;
+
+ /// Class for receiving data from the Leap SDK
+ class Listener;
+
+ friend class LeapDevice;
+
+ /// Constructor.
+ LeapScaffold();
+
+ /// Gets or creates the scaffold shared by all LeapDevice instances.
+ /// The scaffold is managed using a SharedInstance object, so it will be destroyed when all devices are released.
+ /// \return the scaffold object.
+ static std::shared_ptr<LeapScaffold> getOrCreateSharedInstance();
+
+ /// Registers the specified device object.
+ /// \param device The device object to be used
+ /// \return True if the initialization succeeds, false if it fails.
+ bool registerDevice(LeapDevice* device);
+
+ /// Unregisters the specified device object.
+ /// \param device The device object.
+ /// \return true on success, false on failure.
+ bool unregisterDevice(const LeapDevice* device);
+
+ /// Do the Leap specific registration
+ /// \param info The device data
+ /// \return true on success, false on failure.
+ bool doRegisterDevice(DeviceData* info);
+
+ /// Handle a new frame of data
+ void handleFrame();
+
+ /// Handle initial connection to SDK
+ void handleConnect();
+
+ /// Update the hand data
+ void updateHandData();
+
+ /// Update the image data
+ void updateImageData();
+
+ /// Builds the data layout for the application input (i.e. device output).
+ static DataStructures::DataGroup buildDeviceInputData();
+
+ /// Internal scaffold state.
+ std::unique_ptr<StateData> m_state;
+
+ /// Set using HMD Tracking Mode
+ /// \param useHmdTrackingMode True if using HMD Tracking Mode
+ void setUseHmdTrackingMode(bool useHmdTrackingMode);
+
+ /// Is Using HMD Tracking Mode
+ /// \return True if using HMD Tracking Mode
+ bool isUsingHmdTrackingMode() const;
+
+ /// Logger used by the scaffold
+ std::shared_ptr<Framework::Logger> m_logger;
+};
+
+}; // namespace Devices
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_LEAP_LEAPSCAFFOLD_H
diff --git a/SurgSim/Devices/Leap/LeapUtilities.cpp b/SurgSim/Devices/Leap/LeapUtilities.cpp
new file mode 100644
index 0000000..b17ea89
--- /dev/null
+++ b/SurgSim/Devices/Leap/LeapUtilities.cpp
@@ -0,0 +1,70 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Leap/LeapUtilities.h"
+
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+DataStructures::DataGroup::ImageType undistortLeapImage(const DataStructures::DataGroup::ImageType& image,
+ const DataStructures::DataGroup::ImageType& distortion)
+{
+ DataStructures::DataGroup::ImageType result(image.getWidth(), image.getHeight(), image.getNumChannels());
+ Eigen::Array2f distortionPosition;
+ Eigen::Array2f weights;
+ Eigen::Array2f size(result.getWidth(), result.getHeight());
+ Eigen::Array2i corner;
+ Eigen::RowVector2f preMultiply;
+ Eigen::Vector2f postMultiply;
+ Eigen::Array2f position;
+
+ // Undistort the images using the Leap distortion map.
+ // See Image::distortion() documentation for reference implementation:
+ // https://developer.leapmotion.com/documentation/cpp/api/Leap.Image.html
+ for (size_t i = 0; i < result.getWidth(); i++)
+ {
+ for (size_t j = 0; j < result.getHeight(); j++)
+ {
+ distortionPosition << 63 * i / size[0], 62 * (1 - j / size[1]);
+ corner = distortionPosition.template cast<int>();
+
+ // Bilinear interpolation
+ weights = distortionPosition - distortionPosition.unaryExpr(std::ptr_fun<float, float>(std::floor));
+ preMultiply << 1 - weights[0], weights[0];
+ postMultiply << 1 - weights[1], weights[1];
+ position << preMultiply * distortion.getChannel(0).block<2, 2>(corner[0], corner[1]) * postMultiply,
+ preMultiply * distortion.getChannel(1).block<2, 2>(corner[0], corner[1]) * postMultiply;
+
+ if ((position >= 0).all() && (position <= 1.0).all())
+ {
+ position *= size;
+ result(i, j) = image(static_cast<size_t>(position[0]), static_cast<size_t>(position[1]));
+ }
+ else
+ {
+ result(i, j).setConstant(0);
+ }
+ }
+ }
+ return result;
+}
+
+};
+};
+
+
diff --git a/SurgSim/Devices/Leap/LeapUtilities.h b/SurgSim/Devices/Leap/LeapUtilities.h
new file mode 100644
index 0000000..283ec99
--- /dev/null
+++ b/SurgSim/Devices/Leap/LeapUtilities.h
@@ -0,0 +1,38 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_LEAP_LEAPUTILITIES_H
+#define SURGSIM_DEVICES_LEAP_LEAPUTILITIES_H
+
+#include "SurgSim/DataStructures/DataGroup.h"
+
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+/// Correct an infrared image from the LeapDevice, given its distortion callibration map
+/// \param image The distorted infrared image
+/// \param distortion The distortion callibration map
+/// \return The corrected infrared image
+DataStructures::DataGroup::ImageType undistortLeapImage(const DataStructures::DataGroup::ImageType& image,
+ const DataStructures::DataGroup::ImageType& distortion);
+
+};
+};
+
+#endif //SURGSIM_DEVICES_LEAP_LEAPUTILITIES_H
+
diff --git a/SurgSim/Devices/Leap/UnitTests/CMakeLists.txt b/SurgSim/Devices/Leap/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..70e2113
--- /dev/null
+++ b/SurgSim/Devices/Leap/UnitTests/CMakeLists.txt
@@ -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_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ LeapDeviceTest.cpp
+)
+
+set(LIBS
+ LeapDevice
+ SurgSimDataStructures
+ SurgSimInput
+)
+
+surgsim_add_unit_tests(LeapDeviceTest)
+
+set_target_properties(LeapDeviceTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Leap/UnitTests/LeapDeviceTest.cpp b/SurgSim/Devices/Leap/UnitTests/LeapDeviceTest.cpp
new file mode 100644
index 0000000..8290b65
--- /dev/null
+++ b/SurgSim/Devices/Leap/UnitTests/LeapDeviceTest.cpp
@@ -0,0 +1,232 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 LeapDevice class.
+
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include <memory>
+#include <string>
+
+#include "SurgSim/Devices/Leap/LeapDevice.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::Devices::LeapDevice;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Testing::MockInputOutput;
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+TEST(LeapDeviceTest, CreateUninitializedDevice)
+{
+ std::shared_ptr<LeapDevice> device;
+ ASSERT_NO_THROW({ device = std::make_shared<LeapDevice>("TestLeap"); });
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+}
+
+TEST(LeapDeviceTest, CreateAndInitializeDevice)
+{
+ std::shared_ptr<LeapDevice> device = std::make_shared<LeapDevice>("TestLeap");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Leap device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+}
+
+TEST(LeapDeviceTest, Name)
+{
+ std::shared_ptr<LeapDevice> device = std::make_shared<LeapDevice>("TestLeap");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_EQ("TestLeap", device->getName());
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Leap device plugged in?";
+ EXPECT_EQ("TestLeap", device->getName());
+}
+
+TEST(LeapDeviceTest, HandType)
+{
+ std::shared_ptr<LeapDevice> device = std::make_shared<LeapDevice>("TestLeap");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+
+ EXPECT_EQ(HANDTYPE_RIGHT, device->getHandType());
+
+ device->setHandType(HANDTYPE_LEFT);
+ EXPECT_EQ(HANDTYPE_LEFT, device->getHandType());
+
+ device->setHandType(HANDTYPE_RIGHT);
+ EXPECT_EQ(HANDTYPE_RIGHT, device->getHandType());
+}
+
+TEST(LeapDeviceTest, TrackingMode)
+{
+ {
+ std::shared_ptr<LeapDevice> device = std::make_shared<LeapDevice>("TestLeap");
+ EXPECT_THROW(device->isUsingHmdTrackingMode(), Framework::AssertionFailure)
+ << "TrackingMode not previously set, nor device initialized, should not be able to determine tracking mode";
+ }
+ {
+ std::shared_ptr<LeapDevice> device = std::make_shared<LeapDevice>("TestLeap");
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Leap device plugged in?";
+
+ EXPECT_FALSE(device->isUsingHmdTrackingMode()) << "HMD Tracking should be off by default.";
+ }
+ {
+ std::shared_ptr<LeapDevice> device = std::make_shared<LeapDevice>("TestLeap");
+ device->setUseHmdTrackingMode(true);
+ EXPECT_TRUE(device->isUsingHmdTrackingMode());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Leap device plugged in?";
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(100));
+ EXPECT_TRUE(device->isUsingHmdTrackingMode())
+ << "HMD Tracking Mode not set. This could be do to user settings in the LeapControlPanel." << std::endl
+ << "Disable 'Auto-orient Tracking' in Settings>>Tracking.";
+ }
+}
+
+TEST(LeapDeviceTest, ProvidingImages)
+{
+ std::shared_ptr<LeapDevice> device = std::make_shared<LeapDevice>("TestLeap");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+
+ EXPECT_FALSE(device->isProvidingImages());
+
+ device->setProvideImages(true);
+ EXPECT_TRUE(device->isProvidingImages());
+
+ EXPECT_FALSE(device->isInitialized());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Leap device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+
+ EXPECT_THROW(device->setProvideImages(true), Framework::AssertionFailure);
+}
+
+TEST(LeapDeviceTest, CreateDevicesWithSameName)
+{
+ std::shared_ptr<LeapDevice> device1 = std::make_shared<LeapDevice>("TestLeap");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a Leap device plugged in?";
+
+ std::shared_ptr<LeapDevice> device2 = std::make_shared<LeapDevice>("TestLeap");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate name.";
+}
+
+TEST(LeapDeviceTest, InputConsumer)
+{
+ std::shared_ptr<LeapDevice> device = std::make_shared<LeapDevice>("TestLeap");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Leap 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.
+ 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, 5);
+ EXPECT_LE(consumer->m_numTimesReceivedInput, 120);
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.images().hasEntry("left"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.images().hasEntry("left_distortion"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.images().hasEntry("right"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.images().hasEntry("right_distortion"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("pose"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("ThumbProximal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("ThumbIntermediate"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("ThumbDistal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("IndexFingerProximal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("IndexFingerIntermediate"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("IndexFingerDistal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("MiddleFingerProximal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("MiddleFingerIntermediate"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("MiddleFingerDistal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("RingFingerProximal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("RingFingerIntermediate"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("RingFingerDistal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("SmallFingerProximal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("SmallFingerIntermediate"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasEntry("SmallFingerDistal"));
+}
+
+TEST(LeapDeviceTest, OutputProducer)
+{
+ std::shared_ptr<LeapDevice> device = std::make_shared<LeapDevice>("TestLeap");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Leap 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.
+ 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);
+}
+
+TEST(LeapDeviceTest, FactoryCreation)
+{
+ std::shared_ptr<Input::DeviceInterface> device;
+ ASSERT_NO_THROW(device = Input::DeviceInterface::getFactory().create("SurgSim::Devices::LeapDevice", "TestLeap"));
+ EXPECT_EQ("SurgSim::Devices::LeapDevice", device->getClassName());
+}
+
+TEST(LeapDeviceTest, AccessibleTest)
+{
+ std::shared_ptr<LeapDevice> device = std::make_shared<LeapDevice>("TestLeap");
+
+ EXPECT_EQ(HANDTYPE_RIGHT, device->getValue<HandType>("HandType"));
+
+ device->setValue("HandType", HANDTYPE_LEFT);
+ EXPECT_EQ(HANDTYPE_LEFT, device->getValue<HandType>("HandType"));
+ EXPECT_EQ(HANDTYPE_LEFT, device->getHandType());
+
+ EXPECT_NO_THROW(device->setValue("HandType", HANDTYPE_RIGHT));
+ EXPECT_EQ(HANDTYPE_RIGHT, device->getValue<HandType>("HandType"));
+ EXPECT_EQ(HANDTYPE_RIGHT, device->getHandType());
+}
+
+};
+};
diff --git a/SurgSim/Devices/Leap/VisualTest/CMakeLists.txt b/SurgSim/Devices/Leap/VisualTest/CMakeLists.txt
new file mode 100644
index 0000000..13dea58
--- /dev/null
+++ b/SurgSim/Devices/Leap/VisualTest/CMakeLists.txt
@@ -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_directories(
+ "${GLUT_INCLUDE_DIR}"
+)
+
+set(EXAMPLE_SOURCES
+ main.cpp
+)
+
+set(EXAMPLE_HEADERS
+)
+
+surgsim_add_executable(LeapVisualTest
+ "${EXAMPLE_SOURCES}" "${EXAMPLE_HEADERS}")
+
+set(LIBS
+ LeapDevice
+ SurgSimInput
+ VisualTestCommon
+ ${GLUT_LIBRARIES}
+)
+
+target_link_libraries(LeapVisualTest ${LIBS})
+
+set_target_properties(LeapVisualTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Leap/VisualTest/main.cpp b/SurgSim/Devices/Leap/VisualTest/main.cpp
new file mode 100644
index 0000000..31f21a4
--- /dev/null
+++ b/SurgSim/Devices/Leap/VisualTest/main.cpp
@@ -0,0 +1,132 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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.hpp>
+#include <memory>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Devices/Leap/LeapDevice.h"
+#include "SurgSim/Devices/Leap/LeapUtilities.h"
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Testing/VisualTestCommon/GlutRenderer.h"
+
+using SurgSim::Input::DeviceInterface;
+using SurgSim::Devices::LeapDevice;
+using SurgSim::Math::Vector2d;
+using SurgSim::Math::Vector3d;
+
+
+class GlutWindow : public SurgSim::Input::InputConsumerInterface
+{
+public:
+ GlutWindow()
+ : m_numHands(0)
+ {
+ m_camera = std::make_shared<GlutCamera>(Vector3d(0.0, -1.0, 0.0), Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 0.0, -1.0), 45.0, 0.001, 2.0);
+ GlutRenderer::setCamera(m_camera);
+
+ m_leftView = std::make_shared<GlutImage>(
+ Eigen::AlignedBox<double, 2>(Vector2d(-0.7, -0.9), Vector2d(-0.3, -0.5)));
+ m_rightView = std::make_shared<GlutImage>(
+ Eigen::AlignedBox<double, 2>(Vector2d(0.3, -0.9), Vector2d(0.7, -0.5)));
+ GlutRenderer::addObject(m_leftView);
+ GlutRenderer::addObject(m_rightView);
+
+ m_renderThread = boost::thread(boost::ref(GlutRenderer::run));
+ }
+
+ ~GlutWindow()
+ {
+ if (m_renderThread.joinable())
+ {
+ m_renderThread.join();
+ }
+ }
+
+ void initializeInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override
+ {
+ m_numHands++;
+ for (size_t i = 0; i < inputData.poses().size(); i++)
+ {
+ auto sphere = std::make_shared<GlutSphere>(0.010, Vector3d::Unit(m_numHands % 3));
+ GlutRenderer::addObject(sphere);
+ m_spheres.insert(std::make_pair(device + inputData.poses().getName(i), sphere));
+ }
+ }
+
+ void handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override
+ {
+ SurgSim::Math::RigidTransform3d pose;
+ for (size_t i = 0; i < inputData.poses().size(); i++)
+ {
+ inputData.poses().get(i, &pose);
+ m_spheres[device + inputData.poses().getName(i)]->pose = pose;
+ }
+
+ SurgSim::DataStructures::DataGroup::ImageType image;
+ SurgSim::DataStructures::DataGroup::ImageType distortion;
+ if (inputData.images().get("left", &image) && inputData.images().get("left_distortion", &distortion))
+ {
+ image = SurgSim::Devices::undistortLeapImage(image, distortion);
+ m_leftView->image.set(std::move(image));
+ }
+ if (inputData.images().get("right", &image) && inputData.images().get("right_distortion", &distortion))
+ {
+ image = SurgSim::Devices::undistortLeapImage(image, distortion);
+ m_rightView->image.set(std::move(image));
+ }
+ }
+
+private:
+ boost::thread m_renderThread;
+ std::shared_ptr<GlutCamera> m_camera;
+ size_t m_numHands;
+ std::unordered_map<std::string, std::shared_ptr<GlutSphere>> m_spheres;
+ std::shared_ptr<GlutImage> m_leftView;
+ std::shared_ptr<GlutImage> m_rightView;
+};
+
+int main(int argc, char** argv)
+{
+ auto leftHand = std::make_shared<LeapDevice>("Left Hand");
+ leftHand->setHandType(SurgSim::Devices::HANDTYPE_LEFT);
+ leftHand->setProvideImages(true);
+ leftHand->initialize();
+
+ auto rightHand = std::make_shared<LeapDevice>("Right Hand");
+ rightHand->setHandType(SurgSim::Devices::HANDTYPE_RIGHT);
+ rightHand->initialize();
+
+ std::shared_ptr<GlutWindow> window = std::make_shared<GlutWindow>();
+ leftHand->addInputConsumer(window);
+ rightHand->addInputConsumer(window);
+
+ std::cout << std::endl
+ << "**********************************************************************" << std::endl
+ << "Leap Visual Test" << std::endl << std::endl
+ << "When done, press Enter to quit the application." << std::endl
+ << "**********************************************************************" << std::endl;
+ getc(stdin);
+ std::cout << "Exiting..." << std::endl;
+
+ leftHand->removeInputConsumer(window);
+ rightHand->removeInputConsumer(window);
+
+ exit(0);
+}
diff --git a/SurgSim/Devices/Mouse/CMakeLists.txt b/SurgSim/Devices/Mouse/CMakeLists.txt
index 8a0bcf3..d797af9 100644
--- a/SurgSim/Devices/Mouse/CMakeLists.txt
+++ b/SurgSim/Devices/Mouse/CMakeLists.txt
@@ -18,27 +18,29 @@ include_directories(
"${OPENSCENEGRAPH_INCLUDE_DIR}"
)
-set(Mouse_DEVICE_SOURCES
+set(MOUSE_DEVICE_SOURCES
MouseDevice.cpp
MouseScaffold.cpp
OsgMouseHandler.cpp
)
-set(Mouse_DEVICE_HEADERS
+set(MOUSE_DEVICE_HEADERS
MouseDevice.h
MouseScaffold.h
OsgMouseHandler.h
)
+set(DEVICE_HEADERS ${DEVICE_HEADERS} Mouse/MouseDevice.h PARENT_SCOPE)
+
surgsim_add_library(
MouseDevice
- "${Mouse_DEVICE_SOURCES}"
- "${Mouse_DEVICE_HEADERS}"
- SurgSim/Devices/Mouse
+ "${MOUSE_DEVICE_SOURCES}"
+ "${MOUSE_DEVICE_HEADERS}"
)
set(LIBS
SurgSimInput
+ ${OPENSCENEGRAPH_LIBRARIES}
)
target_link_libraries(MouseDevice ${LIBS}
diff --git a/SurgSim/Devices/Mouse/MouseDevice.cpp b/SurgSim/Devices/Mouse/MouseDevice.cpp
index 340c7b0..7a29308 100644
--- a/SurgSim/Devices/Mouse/MouseDevice.cpp
+++ b/SurgSim/Devices/Mouse/MouseDevice.cpp
@@ -20,9 +20,11 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::MouseDevice, MouseDevice);
+
MouseDevice::MouseDevice(const std::string& deviceName) :
SurgSim::Input::CommonDevice(deviceName, MouseScaffold::buildDeviceInputData())
{
@@ -38,23 +40,24 @@ MouseDevice::~MouseDevice()
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;
+ SURGSIM_ASSERT(!isInitialized()) << getName() << " already initialized.";
+ auto scaffold = MouseScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold != nullptr);
+ bool registered = false;
+ if (scaffold->registerDevice(this))
+ {
+ m_scaffold = std::move(scaffold);
+ registered = true;
+ }
+ return registered;
}
bool MouseDevice::finalize()
{
- SURGSIM_ASSERT(isInitialized());
- SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Finalizing.";
+ SURGSIM_ASSERT(isInitialized()) << getName() << " is not initialized, cannot finalize.";
+ bool unregistered = m_scaffold->unregisterDevice();
m_scaffold.reset();
- return true;
+ return unregistered;
}
bool MouseDevice::isInitialized() const
@@ -68,5 +71,5 @@ OsgMouseHandler* MouseDevice::getMouseHandler() const
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/Mouse/MouseDevice.h b/SurgSim/Devices/Mouse/MouseDevice.h
index 96666c9..63ecca8 100644
--- a/SurgSim/Devices/Mouse/MouseDevice.h
+++ b/SurgSim/Devices/Mouse/MouseDevice.h
@@ -23,12 +23,13 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
-
class MouseScaffold;
class OsgMouseHandler;
+SURGSIM_STATIC_REGISTRATION(MouseDevice);
+
/// A class implementing the communication with a mouse
///
/// \par Application input provided from the device:
@@ -57,30 +58,28 @@ public:
/// Constructor
/// \param deviceName Name for mouse device
explicit MouseDevice(const std::string& deviceName);
+
+ SURGSIM_CLASSNAME(SurgSim::Devices::MouseDevice);
+
/// 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;
+ bool initialize() 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;
+ bool isInitialized() const override;
/// Get mouse handler
/// \return The mouse handler associated with this device
OsgMouseHandler* getMouseHandler() const;
private:
+ bool finalize() override;
+
/// Communication with hardware is handled by scaffold.
std::shared_ptr<MouseScaffold> m_scaffold;
};
-}; // namespace Device
+}; // namespace Devices
}; // 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
index 01b3795..fec03f3 100644
--- a/SurgSim/Devices/Mouse/MouseScaffold.cpp
+++ b/SurgSim/Devices/Mouse/MouseScaffold.cpp
@@ -24,7 +24,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
using SurgSim::DataStructures::DataGroup;
@@ -53,41 +53,39 @@ private:
DeviceData& operator=(const DeviceData&) /*= delete*/;
};
-MouseScaffold::MouseScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) : m_logger(logger)
+MouseScaffold::MouseScaffold() : m_logger(Framework::Logger::getLogger("Devices/Mouse"))
{
- 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.";
+ SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold created.";
}
MouseScaffold::~MouseScaffold()
{
- unregisterDevice();
+ if (m_device != nullptr)
+ {
+ unregisterDevice();
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold destroyed.";
}
bool MouseScaffold::registerDevice(MouseDevice* device)
{
+ SURGSIM_ASSERT(m_device == nullptr) << "Can't register two Mouse devices.";
+
m_device.reset(new DeviceData(device));
if (nullptr == m_device)
{
- SURGSIM_LOG_CRITICAL(m_logger) << "MouseScaffold::registerDevice(): failed to create a DeviceData";
+ SURGSIM_LOG_CRITICAL(m_logger) << "Failed to create a DeviceData for " << device->getName();
return false;
}
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << " registered.";
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;
+ SURGSIM_LOG_DEBUG(m_logger) << "Unregistered device.";
+ return true;
}
bool MouseScaffold::updateDevice(int buttonMask, float x, float y, int scrollDeltaX, int scrollDeltaY)
@@ -114,7 +112,6 @@ 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()
{
@@ -136,18 +133,5 @@ std::shared_ptr<MouseScaffold> MouseScaffold::getOrCreateSharedInstance()
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 Devices
}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/Mouse/MouseScaffold.h b/SurgSim/Devices/Mouse/MouseScaffold.h
index a6115ee..d0be387 100644
--- a/SurgSim/Devices/Mouse/MouseScaffold.h
+++ b/SurgSim/Devices/Mouse/MouseScaffold.h
@@ -28,13 +28,13 @@ namespace DataStructures
class DataGroup;
}
-namespace Device
+namespace Devices
{
class MouseDevice;
class OsgMouseHandler;
/// A class that implements the behavior of MouseDevice objects.
-/// \sa SurgSim::Device::MouseDevice
+/// \sa SurgSim::Devices::MouseDevice
class MouseScaffold
{
friend class MouseDevice;
@@ -43,26 +43,16 @@ class MouseScaffold
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);
+ MouseScaffold();
+
/// 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.
+ /// The scaffold is managed using a SharedInstance 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;
@@ -96,13 +86,11 @@ private:
/// 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 Devices
}; // namespace SurgSim
-#endif // SURGSIM_DEVICES_MOUSE_MOUSESCAFFOLD_H
\ No newline at end of file
+#endif // SURGSIM_DEVICES_MOUSE_MOUSESCAFFOLD_H
diff --git a/SurgSim/Devices/Mouse/OsgMouseHandler.cpp b/SurgSim/Devices/Mouse/OsgMouseHandler.cpp
index 862c040..d9a141c 100644
--- a/SurgSim/Devices/Mouse/OsgMouseHandler.cpp
+++ b/SurgSim/Devices/Mouse/OsgMouseHandler.cpp
@@ -18,7 +18,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
OsgMouseHandler::OsgMouseHandler() : m_mouseScaffold(MouseScaffold::getOrCreateSharedInstance()),
@@ -75,5 +75,5 @@ bool OsgMouseHandler::handle(const osgGA::GUIEventAdapter& eventHandler, osgGA::
return true;
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/Mouse/OsgMouseHandler.h b/SurgSim/Devices/Mouse/OsgMouseHandler.h
index 6f3333e..3bd1d39 100644
--- a/SurgSim/Devices/Mouse/OsgMouseHandler.h
+++ b/SurgSim/Devices/Mouse/OsgMouseHandler.h
@@ -22,7 +22,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
class MouseScaffold;
@@ -37,7 +37,7 @@ public:
/// \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;
+ bool handle(const osgGA::GUIEventAdapter& eventHandler, osgGA::GUIActionAdapter& actionAdapter) override;
private:
/// A back pointer to the scaffold which owns this handle
@@ -53,7 +53,7 @@ private:
int m_lastScrollX, m_lastScrollY;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_MOUSE_OSGMOUSEHANDLER_H
\ No newline at end of file
diff --git a/SurgSim/Devices/Mouse/UnitTests/MouseDeviceTest.cpp b/SurgSim/Devices/Mouse/UnitTests/MouseDeviceTest.cpp
index 1f1f138..123b1d3 100644
--- a/SurgSim/Devices/Mouse/UnitTests/MouseDeviceTest.cpp
+++ b/SurgSim/Devices/Mouse/UnitTests/MouseDeviceTest.cpp
@@ -25,10 +25,10 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
-using SurgSim::Device::MouseDevice;
+using SurgSim::Devices::MouseDevice;
using SurgSim::DataStructures::DataGroup;
using SurgSim::Testing::MockInputOutput;
@@ -42,7 +42,7 @@ public:
};
-TEST_F(MouseDeviceTest, CreateInitializeAndDestroyDevice)
+TEST_F(MouseDeviceTest, InitializeDevice)
{
std::shared_ptr<MouseDevice> device = std::make_shared<MouseDevice>("TestMouse");
@@ -51,15 +51,6 @@ TEST_F(MouseDeviceTest, CreateInitializeAndDestroyDevice)
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)
@@ -79,10 +70,17 @@ TEST_F(MouseDeviceTest, InputConsumer)
EXPECT_TRUE(consumer->m_lastReceivedInput.scalars().hasData("y"));
EXPECT_TRUE(consumer->m_lastReceivedInput.integers().hasData("scrollDeltaX"));
EXPECT_TRUE(consumer->m_lastReceivedInput.integers().hasData("scrollDeltaY"));
+}
+
+TEST_F(MouseDeviceTest, NoTwoMice)
+{
+ std::shared_ptr<MouseDevice> device1 = std::make_shared<MouseDevice>("TestMouse1");
+ std::shared_ptr<MouseDevice> device2 = std::make_shared<MouseDevice>("TestMouse2");
- device->finalize();
+ EXPECT_NO_THROW(device1->initialize());
+ EXPECT_ANY_THROW(device2->initialize());
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/Mouse/UnitTests/MouseScaffoldTest.cpp b/SurgSim/Devices/Mouse/UnitTests/MouseScaffoldTest.cpp
index dac638b..3cab814 100644
--- a/SurgSim/Devices/Mouse/UnitTests/MouseScaffoldTest.cpp
+++ b/SurgSim/Devices/Mouse/UnitTests/MouseScaffoldTest.cpp
@@ -20,7 +20,7 @@
#include "SurgSim/Devices/Mouse/MouseScaffold.h"
-using SurgSim::Device::MouseScaffold;
+using SurgSim::Devices::MouseScaffold;
TEST(MouseScaffoldTest, CreateAndDestroyScaffold)
{
diff --git a/SurgSim/Devices/Mouse/VisualTests/CMakeLists.txt b/SurgSim/Devices/Mouse/VisualTests/CMakeLists.txt
index 54ed59a..aa3d1f7 100644
--- a/SurgSim/Devices/Mouse/VisualTests/CMakeLists.txt
+++ b/SurgSim/Devices/Mouse/VisualTests/CMakeLists.txt
@@ -20,8 +20,8 @@ set(UNIT_TEST_SOURCES
set(UNIT_TEST_HEADERS
)
-add_executable(MouseVisualTest
- ${UNIT_TEST_SOURCES} ${UNIT_TEST_HEADERS})
+surgsim_add_executable(MouseVisualTest
+ "${UNIT_TEST_SOURCES}" "${UNIT_TEST_HEADERS}")
set(LIBS MouseDevice
SurgSimDataStructures
diff --git a/SurgSim/Devices/Mouse/VisualTests/MouseVisualTests.cpp b/SurgSim/Devices/Mouse/VisualTests/MouseVisualTests.cpp
index a749f92..b5ae978 100644
--- a/SurgSim/Devices/Mouse/VisualTests/MouseVisualTests.cpp
+++ b/SurgSim/Devices/Mouse/VisualTests/MouseVisualTests.cpp
@@ -34,11 +34,11 @@ using SurgSim::DataStructures::DataGroup;
struct TestListener : public SurgSim::Input::InputConsumerInterface
{
- virtual void initializeInput(const std::string& device, const DataGroup& inputData) override
+ void initializeInput(const std::string& device, const DataGroup& inputData) override
{
}
- virtual void handleInput(const std::string& device, const DataGroup& inputData) override
+ void handleInput(const std::string& device, const DataGroup& inputData) override
{
bool button1, button2, button3;
double x, y;
@@ -79,7 +79,7 @@ struct TestListener : public SurgSim::Input::InputConsumerInterface
int main(int argc, char* argv[])
{
- auto toolDevice = std::make_shared<SurgSim::Device::MouseDevice>("Mouse");
+ auto toolDevice = std::make_shared<SurgSim::Devices::MouseDevice>("Mouse");
toolDevice->initialize();
osg::ref_ptr<osgGA::GUIEventHandler> mouseHandler = toolDevice->getMouseHandler();
diff --git a/SurgSim/Devices/MultiAxis/BitSetBuffer.h b/SurgSim/Devices/MultiAxis/BitSetBuffer.h
index 2fadf81..0c79aa6 100644
--- a/SurgSim/Devices/MultiAxis/BitSetBuffer.h
+++ b/SurgSim/Devices/MultiAxis/BitSetBuffer.h
@@ -22,7 +22,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
/// A bit set corresponding to a contiguous memory buffer.
@@ -127,7 +127,7 @@ private:
std::array<value_type, NUM_BYTES> m_bytes;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_MULTIAXIS_BITSETBUFFER_H
diff --git a/SurgSim/Devices/MultiAxis/CMakeLists.txt b/SurgSim/Devices/MultiAxis/CMakeLists.txt
index d0832e0..ca98963 100644
--- a/SurgSim/Devices/MultiAxis/CMakeLists.txt
+++ b/SurgSim/Devices/MultiAxis/CMakeLists.txt
@@ -58,6 +58,11 @@ set(MULTIAXIS_DEVICE_HEADERS
SystemInputDeviceHandle.h
)
+set(DEVICE_HEADERS ${DEVICE_HEADERS}
+ MultiAxis/MultiAxisDevice.h #NOLINT
+ MultiAxis/RawMultiAxisDevice.h #NOLINT
+ PARENT_SCOPE)
+
set(MULTIAXIS_DEVICE_LINUX_SOURCES
linux/CreateInputDeviceHandle.cpp
linux/FileDescriptor.cpp
@@ -94,8 +99,8 @@ endif()
# TODO(advornik): the installation should NOT copy all the headers...
surgsim_add_library(
MultiAxisDevice
- "${MULTIAXIS_DEVICE_SOURCES}" "${MULTIAXIS_DEVICE_HEADERS}"
- SurgSim/Devices/MultiAxis
+ "${MULTIAXIS_DEVICE_SOURCES}"
+ "${MULTIAXIS_DEVICE_HEADERS}"
)
set(LIBS
diff --git a/SurgSim/Devices/MultiAxis/CreateInputDeviceHandle.h b/SurgSim/Devices/MultiAxis/CreateInputDeviceHandle.h
index bb78b0a..9cf8632 100644
--- a/SurgSim/Devices/MultiAxis/CreateInputDeviceHandle.h
+++ b/SurgSim/Devices/MultiAxis/CreateInputDeviceHandle.h
@@ -27,7 +27,7 @@ namespace Framework
class Logger;
}; // namespace Framework
-namespace Device
+namespace Devices
{
class SystemInputDeviceHandle;
@@ -45,7 +45,7 @@ std::unique_ptr<SystemInputDeviceHandle> createInputDeviceHandle(const std::stri
std::vector<std::string> enumerateInputDevicePaths(SurgSim::Framework::Logger* logger);
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_MULTIAXIS_CREATEINPUTDEVICEHANDLE_H
diff --git a/SurgSim/Devices/MultiAxis/GetSystemError.h b/SurgSim/Devices/MultiAxis/GetSystemError.h
index 22fc030..b92a9df 100644
--- a/SurgSim/Devices/MultiAxis/GetSystemError.h
+++ b/SurgSim/Devices/MultiAxis/GetSystemError.h
@@ -21,7 +21,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
namespace Internal
{
@@ -37,7 +37,7 @@ int64_t getSystemErrorCode();
std::string getSystemErrorText(int64_t errorCode = getSystemErrorCode());
}; // namespace Internal
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_MULTIAXIS_GETSYSTEMERROR_H
diff --git a/SurgSim/Devices/MultiAxis/MultiAxisDevice.cpp b/SurgSim/Devices/MultiAxis/MultiAxisDevice.cpp
index 9be17d0..7438365 100644
--- a/SurgSim/Devices/MultiAxis/MultiAxisDevice.cpp
+++ b/SurgSim/Devices/MultiAxis/MultiAxisDevice.cpp
@@ -23,81 +23,30 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::MultiAxisDevice, MultiAxisDevice);
MultiAxisDevice::MultiAxisDevice(const std::string& uniqueName) :
- m_name(uniqueName),
- m_rawDevice(new RawMultiAxisDevice(uniqueName + "_RawBase"))
+ FilteredDevice(uniqueName),
+ m_rawDevice(std::make_shared<RawMultiAxisDevice>(uniqueName + "_RawBase")),
+ m_filter(std::make_shared<PoseIntegrator>(uniqueName + "_Integrator"))
{
- 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;
-}
-
+ setDevice(m_rawDevice);
-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);
-}
+ m_filter->setNameForCallback(uniqueName); // the filter should make callbacks as the entire device
+ addFilter(m_filter);
-bool MultiAxisDevice::hasOutputProducer()
-{
- return m_filter->hasOutputProducer();
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(MultiAxisDevice, double, PositionScale,
+ getPositionScale, setPositionScale);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(MultiAxisDevice, double, OrientationScale,
+ getOrientationScale, setOrientationScale);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(MultiAxisDevice, bool, AxisDominance,
+ isUsingAxisDominance, setAxisDominance);
}
void MultiAxisDevice::setPositionScale(double scale)
@@ -105,19 +54,16 @@ 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();
@@ -128,7 +74,6 @@ void MultiAxisDevice::setAxisDominance(bool onOff)
m_rawDevice->setAxisDominance(onOff);
}
-
bool MultiAxisDevice::isUsingAxisDominance() const
{
return m_rawDevice->isUsingAxisDominance();
@@ -149,5 +94,5 @@ void MultiAxisDevice::setReset(const std::string& name)
m_filter->setReset(name);
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/MultiAxisDevice.h b/SurgSim/Devices/MultiAxis/MultiAxisDevice.h
index a95ff52..5249f6a 100644
--- a/SurgSim/Devices/MultiAxis/MultiAxisDevice.h
+++ b/SurgSim/Devices/MultiAxis/MultiAxisDevice.h
@@ -19,16 +19,18 @@
#include <memory>
#include <string>
+#include "SurgSim/Devices/DeviceFilters/FilteredDevice.h"
#include "SurgSim/Input/CommonDevice.h"
namespace SurgSim
{
-namespace Device
+namespace Devices
{
class RawMultiAxisDevice;
class PoseIntegrator;
+SURGSIM_STATIC_REGISTRATION(MultiAxisDevice);
/// A class implementing the communication with a multi-axis controller input device, for example a 3DConnexion
/// SpaceNavigator.
@@ -51,58 +53,15 @@ class PoseIntegrator;
/// | ---- | ---- | --- |
/// | 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
+/// \sa RawMultiAxisDevice
+class MultiAxisDevice : public FilteredDevice
{
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;
+ SURGSIM_CLASSNAME(SurgSim::Devices::MultiAxisDevice);
/// Sets the position scale for this device.
/// The position scale controls how much the pose changes for a given device translation.
@@ -148,9 +107,6 @@ private:
/// \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;
@@ -158,7 +114,7 @@ private:
std::shared_ptr<PoseIntegrator> m_filter;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_MULTIAXIS_MULTIAXISDEVICE_H
diff --git a/SurgSim/Devices/MultiAxis/RawMultiAxisDevice.cpp b/SurgSim/Devices/MultiAxis/RawMultiAxisDevice.cpp
index 6eda566..21a5ecf 100644
--- a/SurgSim/Devices/MultiAxis/RawMultiAxisDevice.cpp
+++ b/SurgSim/Devices/MultiAxis/RawMultiAxisDevice.cpp
@@ -17,21 +17,27 @@
#include "SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h"
#include "SurgSim/Framework/Log.h"
-#include "SurgSim/Framework/Assert.h"
namespace SurgSim
{
-namespace Device
+namespace Devices
{
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::RawMultiAxisDevice, RawMultiAxisDevice);
RawMultiAxisDevice::RawMultiAxisDevice(const std::string& uniqueName) :
- SurgSim::Input::CommonDevice(uniqueName, RawMultiAxisScaffold::buildDeviceInputData()),
+ Input::CommonDevice(uniqueName, RawMultiAxisScaffold::buildDeviceInputData()),
m_positionScale(defaultPositionScale()),
m_orientationScale(defaultOrientationScale()),
m_useAxisDominance(false)
{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RawMultiAxisDevice, double, PositionScale,
+ getPositionScale, setPositionScale);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RawMultiAxisDevice, double, OrientationScale,
+ getOrientationScale, setOrientationScale);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RawMultiAxisDevice, bool, AxisDominance,
+ isUsingAxisDominance, setAxisDominance);
}
@@ -46,11 +52,11 @@ RawMultiAxisDevice::~RawMultiAxisDevice()
bool RawMultiAxisDevice::initialize()
{
- SURGSIM_ASSERT(! isInitialized());
+ SURGSIM_ASSERT(!isInitialized()) << getName() << " already initialized.";
std::shared_ptr<RawMultiAxisScaffold> scaffold = RawMultiAxisScaffold::getOrCreateSharedInstance();
SURGSIM_ASSERT(scaffold);
- if (! scaffold->registerDevice(this))
+ if (!scaffold->registerDevice(this))
{
return false;
}
@@ -59,15 +65,13 @@ bool RawMultiAxisDevice::initialize()
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.";
+ SURGSIM_ASSERT(isInitialized()) << getName() << " is not initialized, cannot finalize.";
bool ok = m_scaffold->unregisterDevice(this);
m_scaffold.reset();
return ok;
@@ -128,5 +132,5 @@ bool RawMultiAxisDevice::isUsingAxisDominance() const
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h b/SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h
index e1c57ee..c63020e 100644
--- a/SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h
+++ b/SurgSim/Devices/MultiAxis/RawMultiAxisDevice.h
@@ -24,10 +24,11 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
class RawMultiAxisScaffold;
+SURGSIM_STATIC_REGISTRATION(RawMultiAxisDevice);
/// A class implementing the communication with a multi-axis controller input device, for example a 3DConnexion
/// SpaceNavigator.
@@ -60,15 +61,14 @@ public:
/// \param uniqueName A unique name for the device that will be used by the application.
explicit RawMultiAxisDevice(const std::string& uniqueName);
+ SURGSIM_CLASSNAME(SurgSim::Devices::RawMultiAxisDevice);
+
/// Destructor.
virtual ~RawMultiAxisDevice();
- virtual bool initialize() override;
-
- virtual bool finalize() override;
+ bool initialize() override;
- /// Check whether this device is initialized.
- bool isInitialized() const;
+ bool isInitialized() const override;
/// Sets the position scale for this device.
/// The position scale controls how much the pose changes for a given device translation.
@@ -93,6 +93,8 @@ public:
bool isUsingAxisDominance() const;
private:
+ bool finalize() override;
+
// Returns the default position scale, in meters per tick.
static double defaultPositionScale()
{
@@ -108,7 +110,7 @@ private:
}
- friend SurgSim::Device::MultiAxisDevice::MultiAxisDevice(const std::string& uniqueName);
+ friend SurgSim::Devices::MultiAxisDevice::MultiAxisDevice(const std::string& uniqueName);
friend class RawMultiAxisScaffold;
std::shared_ptr<RawMultiAxisScaffold> m_scaffold;
@@ -121,7 +123,7 @@ private:
bool m_useAxisDominance;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_MULTIAXIS_RAWMULTIAXISDEVICE_H
diff --git a/SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.cpp b/SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.cpp
index 62465f4..69d2303 100644
--- a/SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.cpp
+++ b/SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.cpp
@@ -46,13 +46,13 @@ using SurgSim::Math::Matrix44d;
using SurgSim::Math::Matrix33d;
using SurgSim::Math::RigidTransform3d;
-using SurgSim::Device::Internal::getSystemErrorCode;
-using SurgSim::Device::Internal::getSystemErrorText;
+using SurgSim::Devices::Internal::getSystemErrorCode;
+using SurgSim::Devices::Internal::getSystemErrorText;
namespace SurgSim
{
-namespace Device
+namespace Devices
{
@@ -153,13 +153,10 @@ struct RawMultiAxisScaffold::StateData
{
public:
/// Initialize the state.
- StateData() : isApiInitialized(false)
+ StateData()
{
}
- /// 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;
@@ -173,15 +170,10 @@ private:
};
-RawMultiAxisScaffold::RawMultiAxisScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) :
- m_logger(logger), m_state(new StateData)
+RawMultiAxisScaffold::RawMultiAxisScaffold() :
+ m_logger(Framework::Logger::getLogger("Devices/RawMultiAxis")), 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.";
+ SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold created.";
}
@@ -203,33 +195,14 @@ RawMultiAxisScaffold::~RawMultiAxisScaffold()
}
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 (!findUnusedDeviceAndRegister(device, &numUsedDevicesSeen))
{
if (numUsedDevicesSeen > 0)
{
@@ -244,6 +217,7 @@ bool RawMultiAxisScaffold::registerDevice(RawMultiAxisDevice* device)
return false;
}
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << "registered.";
return true;
}
@@ -264,13 +238,11 @@ bool RawMultiAxisScaffold::unregisterDevice(const RawMultiAxisDevice* const devi
m_state->activeDeviceList.erase(matching);
// the iterator is now invalid but that's OK
found = true;
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << " unregistered.";
}
}
- if (! found)
- {
- SURGSIM_LOG_WARNING(m_logger) << "RawMultiAxis: Attempted to release a non-registered device.";
- }
+ SURGSIM_LOG_IF(!found, m_logger, WARNING) << "Attempted to release a non-registered device " << device->getName();
return found;
}
@@ -313,7 +285,7 @@ void RawMultiAxisScaffold::setAxisDominance(const RawMultiAxisDevice* device, bo
bool RawMultiAxisScaffold::runInputFrame(RawMultiAxisScaffold::DeviceData* info)
{
info->deviceObject->pullOutput();
- if (! updateDevice(info))
+ if (!updateDevice(info))
{
return false;
}
@@ -364,7 +336,7 @@ bool RawMultiAxisScaffold::updateDevice(RawMultiAxisScaffold::DeviceData* info)
}
bool didUpdate = false;
- if (! info->deviceHandle->updateStates(&(info->axisStates), &(info->buttonStates), &didUpdate))
+ 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;
@@ -385,7 +357,7 @@ bool RawMultiAxisScaffold::updateDevice(RawMultiAxisScaffold::DeviceData* info)
// happen before filtering, and putting dominance AND filtering (and integrator) into components is too much.
Vector3d position;
Vector3d rotation;
- if (! info->useAxisDominance)
+ 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;
@@ -437,33 +409,13 @@ bool RawMultiAxisScaffold::updateDevice(RawMultiAxisScaffold::DeviceData* info)
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)
+ if (!handle)
{
int64_t error = getSystemErrorCode();
- SURGSIM_LOG_INFO(m_logger) << "RawMultiAxis: Could not open device " << path << ": error " << error <<
+ SURGSIM_LOG_INFO(m_logger) << "Could not open device " << path << ": error " << error <<
", " << getSystemErrorText(error);
}
return std::move(handle);
@@ -478,7 +430,7 @@ bool RawMultiAxisScaffold::findUnusedDeviceAndRegister(RawMultiAxisDevice* devic
// 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" <<
+ SURGSIM_ASSERT(sameObject == m_state->activeDeviceList.end()) << "Tried to register a device" <<
" which is already present!";
// Make sure the name is unique.
@@ -487,7 +439,7 @@ bool RawMultiAxisScaffold::findUnusedDeviceAndRegister(RawMultiAxisDevice* devic
[&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" <<
+ SURGSIM_LOG_CRITICAL(m_logger) << "Tried to register a device when the same name is" <<
" already present!";
return false;
}
@@ -501,7 +453,7 @@ bool RawMultiAxisScaffold::findUnusedDeviceAndRegister(RawMultiAxisDevice* devic
// Check if this is the device we wanted.
std::unique_ptr<SystemInputDeviceHandle> handle = openDevice(devicePath);
- if (! handle)
+ if (!handle)
{
// message was already printed
continue;
@@ -522,7 +474,7 @@ bool RawMultiAxisScaffold::findUnusedDeviceAndRegister(RawMultiAxisDevice* devic
reportedName << ")";
}
- if (! handle->hasTranslationAndRotationAxes())
+ if (!handle->hasTranslationAndRotationAxes())
{
continue;
}
@@ -554,7 +506,7 @@ bool RawMultiAxisScaffold::registerIfUnused(const std::string& path, RawMultiAxi
// The controller is not yet in use.
std::unique_ptr<SystemInputDeviceHandle> handle = openDevice(path);
- if (! handle)
+ if (!handle)
{
return false;
}
@@ -573,7 +525,7 @@ bool RawMultiAxisScaffold::registerIfUnused(const std::string& path, RawMultiAxi
bool RawMultiAxisScaffold::createPerDeviceThread(DeviceData* data)
{
- SURGSIM_ASSERT(! data->thread);
+ SURGSIM_ASSERT(!data->thread);
std::unique_ptr<RawMultiAxisThread> thread(new RawMultiAxisThread(this, data));
thread->start();
@@ -610,13 +562,6 @@ std::shared_ptr<RawMultiAxisScaffold> RawMultiAxisScaffold::getOrCreateSharedIns
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 Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h b/SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h
index 5f8c64a..65ee8e0 100644
--- a/SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h
+++ b/SurgSim/Devices/MultiAxis/RawMultiAxisScaffold.h
@@ -24,7 +24,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
class RawMultiAxisDevice;
@@ -33,35 +33,21 @@ class SystemInputDeviceHandle;
/// A class that implements the behavior of RawMultiAxisDevice objects.
///
-/// \sa SurgSim::Device::RawMultiAxisDevice
+/// \sa SurgSim::Devices::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);
+ RawMultiAxisScaffold();
/// 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.
+ /// The scaffold is managed using a SharedInstance 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;
@@ -111,14 +97,6 @@ private:
/// \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.
@@ -157,12 +135,9 @@ private:
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 Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_MULTIAXIS_RAWMULTIAXISSCAFFOLD_H
diff --git a/SurgSim/Devices/MultiAxis/RawMultiAxisThread.cpp b/SurgSim/Devices/MultiAxis/RawMultiAxisThread.cpp
index 5e352c7..6ce4898 100644
--- a/SurgSim/Devices/MultiAxis/RawMultiAxisThread.cpp
+++ b/SurgSim/Devices/MultiAxis/RawMultiAxisThread.cpp
@@ -17,7 +17,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
RawMultiAxisThread::RawMultiAxisThread(RawMultiAxisScaffold* scaffold, RawMultiAxisScaffold::DeviceData* deviceData) :
@@ -52,5 +52,5 @@ void RawMultiAxisThread::doBeforeStop()
m_scaffold->runAfterLastFrame(m_deviceData);
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/RawMultiAxisThread.h b/SurgSim/Devices/MultiAxis/RawMultiAxisThread.h
index 5d498fb..f2b8256 100644
--- a/SurgSim/Devices/MultiAxis/RawMultiAxisThread.h
+++ b/SurgSim/Devices/MultiAxis/RawMultiAxisThread.h
@@ -25,11 +25,11 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
/// A class implementing the thread context for sampling RawMultiAxis devices.
-/// \sa SurgSim::Device::RawMultiAxisScaffold
+/// \sa SurgSim::Devices::RawMultiAxisScaffold
class RawMultiAxisThread : public SurgSim::Framework::BasicThread
{
public:
@@ -38,17 +38,17 @@ public:
virtual ~RawMultiAxisThread();
protected:
- virtual bool doInitialize() override;
- virtual bool doStartUp() override;
- virtual bool doUpdate(double dt) override;
- virtual void doBeforeStop() override;
+ bool doInitialize() override;
+ bool doStartUp() override;
+ bool doUpdate(double dt) override;
+ void doBeforeStop() override;
private:
RawMultiAxisScaffold* m_scaffold;
RawMultiAxisScaffold::DeviceData* m_deviceData;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_MULTIAXIS_RAWMULTIAXISTHREAD_H
diff --git a/SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.cpp b/SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.cpp
index 25b6828..2ee0967 100644
--- a/SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.cpp
+++ b/SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.cpp
@@ -17,7 +17,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
@@ -34,5 +34,5 @@ void SystemInputDeviceHandle::prepareForShutdown()
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.h b/SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.h
index 516711a..0eda388 100644
--- a/SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.h
+++ b/SurgSim/Devices/MultiAxis/SystemInputDeviceHandle.h
@@ -23,7 +23,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
/// A wrapper for system-dependent access to an input/HID device.
@@ -82,7 +82,7 @@ private:
SystemInputDeviceHandle& operator=(const SystemInputDeviceHandle& other) /*= delete*/;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_MULTIAXIS_SYSTEMINPUTDEVICEHANDLE_H
diff --git a/SurgSim/Devices/MultiAxis/UnitTests/MultiAxisDeviceTest.cpp b/SurgSim/Devices/MultiAxis/UnitTests/MultiAxisDeviceTest.cpp
index c8220a0..4c3b3e8 100644
--- a/SurgSim/Devices/MultiAxis/UnitTests/MultiAxisDeviceTest.cpp
+++ b/SurgSim/Devices/MultiAxis/UnitTests/MultiAxisDeviceTest.cpp
@@ -27,7 +27,7 @@
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Testing/MockInputOutput.h"
-using SurgSim::Device::MultiAxisDevice;
+using SurgSim::Devices::MultiAxisDevice;
using SurgSim::DataStructures::DataGroup;
using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Matrix44d;
@@ -58,6 +58,15 @@ TEST(MultiAxisDeviceTest, Name)
EXPECT_EQ("TestMultiAxis", device->getName());
}
+TEST(MultiAxisDeviceTest, Factory)
+{
+ std::shared_ptr<SurgSim::Input::DeviceInterface> device;
+ ASSERT_NO_THROW(device = SurgSim::Input::DeviceInterface::getFactory().create(
+ "SurgSim::Devices::MultiAxisDevice", "Device"));
+ EXPECT_NE(nullptr, device);
+}
+
+
static void testCreateDeviceSeveralTimes(bool doSleep)
{
for (int i = 0; i < 6; ++i)
diff --git a/SurgSim/Devices/MultiAxis/UnitTests/RawMultiAxisDeviceTest.cpp b/SurgSim/Devices/MultiAxis/UnitTests/RawMultiAxisDeviceTest.cpp
index c7f7559..92d7471 100644
--- a/SurgSim/Devices/MultiAxis/UnitTests/RawMultiAxisDeviceTest.cpp
+++ b/SurgSim/Devices/MultiAxis/UnitTests/RawMultiAxisDeviceTest.cpp
@@ -22,14 +22,13 @@
#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::Devices::RawMultiAxisDevice;
+using SurgSim::Devices::RawMultiAxisScaffold;
using SurgSim::DataStructures::DataGroup;
using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Matrix44d;
@@ -37,14 +36,12 @@ 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());
@@ -54,7 +51,6 @@ TEST(RawMultiAxisDeviceTest, CreateAndInitializeDevice)
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());
@@ -62,6 +58,15 @@ TEST(RawMultiAxisDeviceTest, Name)
EXPECT_EQ("TestRawMultiAxis", device->getName());
}
+TEST(RawMultiAxisDeviceTest, Factory)
+{
+ std::shared_ptr<SurgSim::Input::DeviceInterface> device;
+ ASSERT_NO_THROW(device = SurgSim::Input::DeviceInterface::getFactory().create(
+ "SurgSim::Devices::RawMultiAxisDevice", "Device"));
+ EXPECT_NE(nullptr, device);
+}
+
+
static void testCreateDeviceSeveralTimes(bool doSleep)
{
for (int i = 0; i < 6; ++i)
@@ -79,13 +84,11 @@ static void testCreateDeviceSeveralTimes(bool doSleep)
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?";
@@ -102,7 +105,6 @@ TEST(RawMultiAxisDeviceTest, CreateSeveralDevices)
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?";
@@ -124,7 +126,6 @@ inline std::string makeString(T value)
TEST(RawMultiAxisDeviceTest, CreateAllDevices)
{
- //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
std::vector<std::shared_ptr<RawMultiAxisDevice>> devices;
for (int i = 1; ; ++i)
@@ -145,7 +146,6 @@ TEST(RawMultiAxisDeviceTest, CreateAllDevices)
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?";
@@ -186,7 +186,6 @@ TEST(RawMultiAxisDeviceTest, InputConsumer)
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?";
diff --git a/SurgSim/Devices/MultiAxis/UnitTests/RawMultiAxisScaffoldTest.cpp b/SurgSim/Devices/MultiAxis/UnitTests/RawMultiAxisScaffoldTest.cpp
index 51d8eea..96f8662 100644
--- a/SurgSim/Devices/MultiAxis/UnitTests/RawMultiAxisScaffoldTest.cpp
+++ b/SurgSim/Devices/MultiAxis/UnitTests/RawMultiAxisScaffoldTest.cpp
@@ -27,12 +27,11 @@
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Matrix.h"
-using SurgSim::Device::RawMultiAxisDevice;
-using SurgSim::Device::RawMultiAxisScaffold;
+using SurgSim::Devices::RawMultiAxisDevice;
+using SurgSim::Devices::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;
@@ -65,7 +64,6 @@ TEST(RawMultiAxisScaffoldTest, CreateAndDestroyScaffold)
TEST(RawMultiAxisScaffoldTest, ScaffoldLifeCycle)
{
- //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
std::weak_ptr<RawMultiAxisScaffold> lastScaffold;
{
std::shared_ptr<RawMultiAxisScaffold> scaffold = RawMultiAxisScaffold::getOrCreateSharedInstance();
@@ -144,7 +142,6 @@ TEST(RawMultiAxisScaffoldTest, ScaffoldLifeCycle)
TEST(RawMultiAxisScaffoldTest, CreateDeviceSeveralTimes)
{
- //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
std::weak_ptr<RawMultiAxisScaffold> lastScaffold;
for (int i = 0; i < 6; ++i)
@@ -164,7 +161,6 @@ TEST(RawMultiAxisScaffoldTest, CreateDeviceSeveralTimes)
TEST(RawMultiAxisScaffoldTest, CreateDeviceSeveralTimesWithScaffoldRef)
{
- //RawMultiAxisScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
std::shared_ptr<RawMultiAxisScaffold> lastScaffold;
for (int i = 0; i < 6; ++i)
diff --git a/SurgSim/Devices/MultiAxis/VisualTest/CMakeLists.txt b/SurgSim/Devices/MultiAxis/VisualTest/CMakeLists.txt
index e27bac2..9cc1e79 100644
--- a/SurgSim/Devices/MultiAxis/VisualTest/CMakeLists.txt
+++ b/SurgSim/Devices/MultiAxis/VisualTest/CMakeLists.txt
@@ -14,14 +14,6 @@
# limitations under the License.
-link_directories(
- ${Boost_LIBRARY_DIRS}
-)
-
-include_directories(
- "${CMAKE_CURRENT_SOURCE_DIR}"
-)
-
set(EXAMPLE_SOURCES
main.cpp
)
@@ -37,29 +29,17 @@ set(RAW_EXAMPLE_HEADERS
)
set(LIBS
- MultiAxisDevice
IdentityPoseDevice
- VisualTestCommon
+ MultiAxisDevice
SurgSimInput
- SurgSimFramework
- SurgSimDataStructures
- ${Boost_LIBRARIES}
- ${OPENGL_LIBRARIES}
+ VisualTestCommon
)
-if(WDK_FOUND)
- list(APPEND LIBS ${WDK_LIBRARIES})
-endif()
-
-add_executable(MultiAxisVisualTest
- ${EXAMPLE_SOURCES} ${EXAMPLE_HEADERS})
-surgsim_show_ide_folders(
+surgsim_add_executable(MultiAxisVisualTest
"${EXAMPLE_SOURCES}" "${EXAMPLE_HEADERS}")
target_link_libraries(MultiAxisVisualTest ${LIBS})
-add_executable(RawMultiAxisVisualTest
- ${RAW_EXAMPLE_SOURCES} ${RAW_EXAMPLE_HEADERS})
-surgsim_show_ide_folders(
+surgsim_add_executable(RawMultiAxisVisualTest
"${RAW_EXAMPLE_SOURCES}" "${RAW_EXAMPLE_HEADERS}")
target_link_libraries(RawMultiAxisVisualTest ${LIBS})
diff --git a/SurgSim/Devices/MultiAxis/VisualTest/main.cpp b/SurgSim/Devices/MultiAxis/VisualTest/main.cpp
index b7b1a44..6443fe9 100644
--- a/SurgSim/Devices/MultiAxis/VisualTest/main.cpp
+++ b/SurgSim/Devices/MultiAxis/VisualTest/main.cpp
@@ -22,8 +22,8 @@
#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
using SurgSim::Input::DeviceInterface;
-using SurgSim::Device::MultiAxisDevice;
-using SurgSim::Device::IdentityPoseDevice;
+using SurgSim::Devices::MultiAxisDevice;
+using SurgSim::Devices::IdentityPoseDevice;
int main(int argc, char** argv)
diff --git a/SurgSim/Devices/MultiAxis/VisualTest/raw_test_main.cpp b/SurgSim/Devices/MultiAxis/VisualTest/raw_test_main.cpp
index a4be13f..c71981a 100644
--- a/SurgSim/Devices/MultiAxis/VisualTest/raw_test_main.cpp
+++ b/SurgSim/Devices/MultiAxis/VisualTest/raw_test_main.cpp
@@ -22,8 +22,8 @@
#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
using SurgSim::Input::DeviceInterface;
-using SurgSim::Device::RawMultiAxisDevice;
-using SurgSim::Device::IdentityPoseDevice;
+using SurgSim::Devices::RawMultiAxisDevice;
+using SurgSim::Devices::IdentityPoseDevice;
int main(int argc, char** argv)
diff --git a/SurgSim/Devices/MultiAxis/linux/CreateInputDeviceHandle.cpp b/SurgSim/Devices/MultiAxis/linux/CreateInputDeviceHandle.cpp
index b714e6a..5e66455 100644
--- a/SurgSim/Devices/MultiAxis/linux/CreateInputDeviceHandle.cpp
+++ b/SurgSim/Devices/MultiAxis/linux/CreateInputDeviceHandle.cpp
@@ -20,7 +20,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
@@ -36,5 +36,5 @@ std::vector<std::string> enumerateInputDevicePaths(SurgSim::Framework::Logger* l
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/linux/FileDescriptor.cpp b/SurgSim/Devices/MultiAxis/linux/FileDescriptor.cpp
index 2762a97..b0ccfed 100644
--- a/SurgSim/Devices/MultiAxis/linux/FileDescriptor.cpp
+++ b/SurgSim/Devices/MultiAxis/linux/FileDescriptor.cpp
@@ -28,7 +28,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
FileDescriptor::FileDescriptor() :
@@ -162,5 +162,5 @@ bool FileDescriptor::readBytes(void* dataBuffer, size_t bytesToRead, size_t* byt
}
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/linux/FileDescriptor.h b/SurgSim/Devices/MultiAxis/linux/FileDescriptor.h
index 27c7c15..b779fca 100644
--- a/SurgSim/Devices/MultiAxis/linux/FileDescriptor.h
+++ b/SurgSim/Devices/MultiAxis/linux/FileDescriptor.h
@@ -21,7 +21,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
/// A wrapper for an UNIX-style integer file descriptor.
@@ -107,7 +107,7 @@ private:
bool m_canWrite;
};
-}; // namespace Device
+}; // namespace Devices
}; // 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
index b740a2d..0c9c410 100644
--- a/SurgSim/Devices/MultiAxis/linux/GetSystemError.cpp
+++ b/SurgSim/Devices/MultiAxis/linux/GetSystemError.cpp
@@ -21,7 +21,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
namespace Internal
{
@@ -66,5 +66,5 @@ std::string getSystemErrorText(int64_t errorCode)
}
}; // namespace Internal
-}; // namespace Device
-}; // namespace SurgSim
\ No newline at end of file
+}; // namespace Devices
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.cpp b/SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.cpp
index 22277d4..030eb41 100644
--- a/SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.cpp
+++ b/SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.cpp
@@ -24,13 +24,13 @@
#include "SurgSim/Devices/MultiAxis/linux/FileDescriptor.h"
#include "SurgSim/Framework/Log.h"
-using SurgSim::Device::Internal::getSystemErrorCode;
-using SurgSim::Device::Internal::getSystemErrorText;
+using SurgSim::Devices::Internal::getSystemErrorCode;
+using SurgSim::Devices::Internal::getSystemErrorText;
namespace SurgSim
{
-namespace Device
+namespace Devices
{
struct InputDeviceHandle::State
@@ -338,5 +338,5 @@ std::vector<int> InputDeviceHandle::getDeviceButtonsAndKeys()
return result;
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.h b/SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.h
index 9df464a..43d431a 100644
--- a/SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.h
+++ b/SurgSim/Devices/MultiAxis/linux/InputDeviceHandle.h
@@ -31,7 +31,7 @@ namespace Framework
class Logger;
}; // namespace Framework
-namespace Device
+namespace Devices
{
/// Access to an input/HID device using the Input API in Linux.
@@ -54,13 +54,13 @@ public:
static std::unique_ptr<InputDeviceHandle> open(const std::string& path,
std::shared_ptr<SurgSim::Framework::Logger> logger);
- virtual std::string getDeviceName() const override;
+ std::string getDeviceName() const override;
- virtual bool getDeviceIds(int* vendorId, int* productId) const override;
+ bool getDeviceIds(int* vendorId, int* productId) const override;
- virtual bool hasTranslationAndRotationAxes() const override;
+ bool hasTranslationAndRotationAxes() const override;
- virtual bool updateStates(AxisStates* axisStates, ButtonStates* buttonStates, bool* updated) override;
+ bool updateStates(AxisStates* axisStates, ButtonStates* buttonStates, bool* updated) override;
private:
/// Constructor.
@@ -87,7 +87,7 @@ private:
std::unique_ptr<State> m_state;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_MULTIAXIS_LINUX_INPUTDEVICEHANDLE_H
diff --git a/SurgSim/Devices/MultiAxis/win32/CreateInputDeviceHandle.cpp b/SurgSim/Devices/MultiAxis/win32/CreateInputDeviceHandle.cpp
index a30c2db..01ecf2d 100644
--- a/SurgSim/Devices/MultiAxis/win32/CreateInputDeviceHandle.cpp
+++ b/SurgSim/Devices/MultiAxis/win32/CreateInputDeviceHandle.cpp
@@ -20,7 +20,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
@@ -36,5 +36,5 @@ std::vector<std::string> enumerateInputDevicePaths(SurgSim::Framework::Logger* l
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/win32/FileHandle.cpp b/SurgSim/Devices/MultiAxis/win32/FileHandle.cpp
index e04734b..5e0e3da 100644
--- a/SurgSim/Devices/MultiAxis/win32/FileHandle.cpp
+++ b/SurgSim/Devices/MultiAxis/win32/FileHandle.cpp
@@ -28,7 +28,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
// TODO(advornik): This would be a perfect use for "constexpr", but VS 2012 doesn't support it...
@@ -182,5 +182,5 @@ bool FileHandle::readBytes(void* dataBuffer, unsigned int bytesToRead, unsigned
}
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/MultiAxis/win32/FileHandle.h b/SurgSim/Devices/MultiAxis/win32/FileHandle.h
index bd282cc..81d463a 100644
--- a/SurgSim/Devices/MultiAxis/win32/FileHandle.h
+++ b/SurgSim/Devices/MultiAxis/win32/FileHandle.h
@@ -22,7 +22,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
/// A wrapper for an Windows-style HANDLE file descriptor.
@@ -120,7 +120,7 @@ private:
uint64_t m_openFlags;
};
-}; // namespace Device
+}; // namespace Devices
}; // 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
index 18907ef..b24fdc2 100644
--- a/SurgSim/Devices/MultiAxis/win32/GetSystemError.cpp
+++ b/SurgSim/Devices/MultiAxis/win32/GetSystemError.cpp
@@ -24,7 +24,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
namespace Internal
{
@@ -62,5 +62,5 @@ std::string getSystemErrorText(int64_t errorCode)
}
}; // namespace Internal
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.cpp b/SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.cpp
index 2f1618e..41fc7ab 100644
--- a/SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.cpp
+++ b/SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.cpp
@@ -32,13 +32,13 @@ extern "C" { // sigh...
#include "SurgSim/Devices/MultiAxis/win32/FileHandle.h"
#include "SurgSim/Framework/Log.h"
-using SurgSim::Device::Internal::getSystemErrorCode;
-using SurgSim::Device::Internal::getSystemErrorText;
+using SurgSim::Devices::Internal::getSystemErrorCode;
+using SurgSim::Devices::Internal::getSystemErrorText;
namespace SurgSim
{
-namespace Device
+namespace Devices
{
// Usage page and usage IDs for interface devices; see e.g. http://www.usb.org/developers/devclass_docs/Hut1_12v2.pdf
@@ -574,5 +574,5 @@ void WdkHidDeviceHandle::decodeStateUpdates(const unsigned char* rawData, size_t
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.h b/SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.h
index 51b5e99..9c447ec 100644
--- a/SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.h
+++ b/SurgSim/Devices/MultiAxis/win32/WdkHidDeviceHandle.h
@@ -35,7 +35,7 @@ namespace Framework
class Logger;
}; // namespace Framework
-namespace Device
+namespace Devices
{
/// Access to an input/HID device using the HID API from the Windows Driver Kit.
@@ -58,15 +58,15 @@ public:
static std::unique_ptr<WdkHidDeviceHandle> open(const std::string& path,
std::shared_ptr<SurgSim::Framework::Logger> logger);
- virtual std::string getDeviceName() const override;
+ std::string getDeviceName() const override;
- virtual bool getDeviceIds(int* vendorId, int* productId) const override;
+ bool getDeviceIds(int* vendorId, int* productId) const override;
- virtual bool hasTranslationAndRotationAxes() const override;
+ bool hasTranslationAndRotationAxes() const override;
- virtual bool updateStates(AxisStates* axisStates, ButtonStates* buttonStates, bool* updated) override;
+ bool updateStates(AxisStates* axisStates, ButtonStates* buttonStates, bool* updated) override;
- virtual void prepareForShutdown() override;
+ void prepareForShutdown() override;
private:
/// Constructor.
@@ -114,7 +114,7 @@ private:
std::unique_ptr<State> m_state;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_MULTIAXIS_WIN32_WDKHIDDEVICEHANDLE_H
diff --git a/SurgSim/Devices/Nimble/CMakeLists.txt b/SurgSim/Devices/Nimble/CMakeLists.txt
new file mode 100644
index 0000000..732d167
--- /dev/null
+++ b/SurgSim/Devices/Nimble/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.
+
+
+# Choose not to link boost::regex as we dont use read_until() or async_read_until()
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBOOST_REGEX_NO_LIB")
+
+link_directories(${Boost_LIBRARY_DIRS})
+
+set(LIBS
+ ${Boost_LIBRARIES}
+ SurgSimInput
+)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+ "${EIGEN3_INCLUDE_DIR}"
+ "${Boost_INCLUDE_DIR}"
+)
+
+set(NIMBLE_DEVICE_SOURCES
+ NimbleDevice.cpp
+ NimbleScaffold.cpp
+)
+
+set(NIMBLE_DEVICE_HEADERS
+ NimbleDevice.h
+ NimbleScaffold.h
+)
+
+set(DEVICE_HEADERS ${DEVICE_HEADERS} Nimble/NimbleDevice.h PARENT_SCOPE)
+
+surgsim_add_library(
+ NimbleDevice
+ "${NIMBLE_DEVICE_SOURCES}"
+ "${NIMBLE_DEVICE_HEADERS}"
+)
+
+target_link_libraries(NimbleDevice ${LIBS})
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+
+ if(GLUT_FOUND)
+ add_subdirectory(VisualTest)
+ endif(GLUT_FOUND)
+endif()
+
+# Put NimbleDevice into folder "Nimble"
+set_target_properties(NimbleDevice PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Nimble/Nimble.dox b/SurgSim/Devices/Nimble/Nimble.dox
new file mode 100644
index 0000000..1b555f4
--- /dev/null
+++ b/SurgSim/Devices/Nimble/Nimble.dox
@@ -0,0 +1,9 @@
+/*!
+
+\page Nimble Nimble: Hand tracking using the NimbleSDK and OpenNI.
+
+NimbleSDK (http://www.threegear.com/download.html), by Three Gear Systems, is a hand tracking software which can communicate (using OpenNI) with a range of special depth cameras, and use the data to calculate the pose of the (two) hands in view.
+
+The software for NimbleSDK consists of the driver for the camera (OpenNI installer is packaged with the SDK) and the hand tracking server, which needs to be started and be running for the NimbleDevice to receive data. The steps to start the server are described here (http://www.threegear.com/latest/doc/index.html) for Windows and Mac OSX.
+
+*/
diff --git a/SurgSim/Devices/Nimble/NimbleDevice.cpp b/SurgSim/Devices/Nimble/NimbleDevice.cpp
new file mode 100644
index 0000000..fbce7b5
--- /dev/null
+++ b/SurgSim/Devices/Nimble/NimbleDevice.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 "SurgSim/Devices/Nimble/NimbleDevice.h"
+
+#include "SurgSim/Devices/Nimble/NimbleScaffold.h"
+#include "SurgSim/Framework/Log.h"
+
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::NimbleDevice, NimbleDevice);
+
+NimbleDevice::NimbleDevice(const std::string& uniqueName) :
+ SurgSim::Input::CommonDevice(uniqueName, NimbleScaffold::buildDeviceInputData()), m_trackedHandDataIndex(0)
+{
+}
+
+NimbleDevice::~NimbleDevice()
+{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+
+void NimbleDevice::setupToTrackLeftHand()
+{
+ m_trackedHandDataIndex = 0;
+}
+
+void NimbleDevice::setupToTrackRightHand()
+{
+ m_trackedHandDataIndex = 1;
+}
+
+bool NimbleDevice::initialize()
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Nimble: Attempt to initialize already initialized device.";
+ auto scaffold = NimbleScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold != nullptr) << "Unable to acquire a NimbleScaffold instance";
+
+ bool initialize = false;
+ if (scaffold->registerDevice(this))
+ {
+ m_scaffold = std::move(scaffold);
+ initialize = true;
+ }
+ return initialize;
+}
+
+bool NimbleDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized()) << "Nimble: Attempt to finalize an uninitialized device.";
+ bool ok = m_scaffold->unregisterDevice(this);
+ m_scaffold.reset();
+ return ok;
+}
+
+bool NimbleDevice::isInitialized() const
+{
+ return (m_scaffold != nullptr);
+}
+
+}; // namespace Devices
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Nimble/NimbleDevice.h b/SurgSim/Devices/Nimble/NimbleDevice.h
new file mode 100644
index 0000000..ac5f6cb
--- /dev/null
+++ b/SurgSim/Devices/Nimble/NimbleDevice.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_NIMBLE_NIMBLEDEVICE_H
+#define SURGSIM_DEVICES_NIMBLE_NIMBLEDEVICE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Input/CommonDevice.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+class NimbleScaffold;
+
+SURGSIM_STATIC_REGISTRATION(NimbleDevice);
+
+/// A class implementing the communication with the Nimble server.
+///
+/// \par Application input provided by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | pose | "pose" | %Hand pose w.r.t. JointFrameIndex.ROOT_JOINT (units are meters). |
+/// | pose | "ThumbProximal" | %Pose w.r.t. thumb proximal joint |
+/// | pose | "ThumbIntermediate" | %Pose w.r.t. thumb intermediate joint |
+/// | pose | "ThumbDistal" | %Pose w.r.t. thumb distal joint |
+/// | pose | "IndexFingerProximal" | %Pose w.r.t. index finger proximal joint |
+/// | pose | "IndexFingerIntermediate" | %Pose w.r.t. index finger intermediate joint |
+/// | pose | "IndexFingerDistal" | %Pose w.r.t. index finger distal joint |
+/// | pose | "MiddleFingerProximal" | %Pose w.r.t. middle finger proximal joint |
+/// | pose | "MiddleFingerIntermediate" | %Pose w.r.t. middle finger intermediate joint |
+/// | pose | "MiddleFingerDistal" | %Pose w.r.t. middle finger distal joint |
+/// | pose | "RingFingerProximal" | %Pose w.r.t. ring finger proximal joint |
+/// | pose | "RingFingerIntermediate" | %Pose w.r.t. ring finger intermediate joint |
+/// | pose | "RingFingerDistal" | %Pose w.r.t. ring finger distal joint |
+/// | pose | "SmallFingerProximal" | %Pose w.r.t. small finger proximal joint |
+/// | pose | "SmallFingerIntermediate" | %Pose w.r.t. small finger intermediate joint |
+/// | pose | "SmallFingerDistal" | %Pose w.r.t. small finger distal joint |
+///
+/// \par Application output used by the device: none.
+///
+/// \sa SurgSim::Input::CommonDevice, SurgSim::Input::DeviceInterface
+class NimbleDevice : public SurgSim::Input::CommonDevice
+{
+public:
+ /// Constructor.
+ ///
+ /// \param uniqueName A unique name for the device that will be used by the application.
+ explicit NimbleDevice(const std::string& uniqueName);
+
+ SURGSIM_CLASSNAME(SurgSim::Devices::NimbleDevice);
+
+ /// Destructor.
+ virtual ~NimbleDevice();
+
+ /// Set the left hand to be tracked.
+ void setupToTrackLeftHand();
+
+ /// Set the right hand to be tracked.
+ void setupToTrackRightHand();
+
+ bool initialize() override;
+
+ bool isInitialized() const override;
+
+private:
+ friend class NimbleScaffold;
+
+ bool finalize() override;
+
+ /// The shared pointer to the NimbleScaffold.
+ std::shared_ptr<NimbleScaffold> m_scaffold;
+
+ /// Indicate whether the hand tracked is left (0) or right (1).
+ size_t m_trackedHandDataIndex;
+};
+
+}; // namespace Devices
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_NIMBLE_NIMBLEDEVICE_H
diff --git a/SurgSim/Devices/Nimble/NimbleScaffold.cpp b/SurgSim/Devices/Nimble/NimbleScaffold.cpp
new file mode 100644
index 0000000..fb57a09
--- /dev/null
+++ b/SurgSim/Devices/Nimble/NimbleScaffold.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.
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+#include "SurgSim/Devices/Nimble/NimbleScaffold.h"
+
+#include <algorithm>
+#include <list>
+#include <locale>
+#include <memory>
+#include <vector>
+
+#include <boost/asio.hpp>
+#include <boost/thread/locks.hpp>
+#include <boost/thread/mutex.hpp>
+
+#include "SurgSim/Devices/Nimble/NimbleDevice.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Quaterniond;
+
+namespace
+{
+
+/// Data structure to hold the data from the Nimble hand tracking SDK, for a single hand.
+struct HandTrackingData
+{
+ /// Number of hands tracked (left, right).
+ static const size_t NUM_HANDS = 2;
+ /// Number of fingers tracked in one hand.
+ static const size_t NUM_FINGERS = 5;
+ /// Number of predefined poses for each hand.
+ static const size_t NUM_POSES = 7;
+
+ // Joint frames used for skinning.
+ //
+ // These are the frames stored in HandTrackingData.jointQuaternions/HandTrackingData.jointPositions.
+ // They are used to locate points in 3D space or define 3D frames. If you want to recognize gestures of the form
+ // "finger X is bending" it is recommended to use the FingerDOF instead. The most stable frame is the one defined
+ // by the metacarpals (WRIST_JOINT).
+ enum JointFrameIndex
+ {
+ ROOT_JOINT = 0, // The frame of the user's forearm (this is where the hand model is rooted).
+ WRIST_JOINT = 1, // The frame of the back of the hand (anatomically, this is the frame of the carpals).
+ THUMB_PROXIMAL = 2, // Thumb proximal frame, refers to the thumb metacarpal bone.
+ THUMB_INTERMEDIATE = 3, // Thumb intermediate frame, refers to the thumb proximal phalange.
+ THUMB_DISTAL = 4, // Thumb distal frame, refers to the thumb distal phalange.
+ INDEX_PROXIMAL = 5, // Index finger proximal frame, refers to the proximal phalange.
+ INDEX_INTERMEDIATE = 6, // Index finger intermediate frame, refers to the intermediate phalange.
+ INDEX_DISTAL = 7, // Index finger distal frame, refers to the distal phalange.
+ MIDDLE_PROXIMAL = 8, // Middle finger proximal frame, refers to the proximal phalange.
+ MIDDLE_INTERMEDIATE = 9, // Middle finger intermediate frame, refers to the intermediate phalange.
+ MIDDLE_DISTAL = 10, // Middle finger distal frame, refers to the distal phalange.
+ RING_PROXIMAL = 11, // Ring finger proximal frame, refers to the proximal phalange.
+ RING_INTERMEDIATE = 12, // Ring finger intermediate frame, refers to the intermediate phalange.
+ RING_DISTAL = 13, // Ring finger distal frame, refers to the distal phalange.
+ SMALL_PROXIMAL = 14, // Small finger proximal frame, refers to the proximal phalange.
+ SMALL_INTERMEDIATE = 15, // Small finger intermediate frame, refers to the intermediate phalange.
+ SMALL_DISTAL = 16, // Small finger distal frame, refers to the distal phalange.
+ NUM_JOINTS
+ };
+
+ // Degrees of freedom of the hand model.
+ //
+ // Stored in HandTrackingData.fingerDofs. Each of these is a rotation in radians that measures how far the
+ // corresponding finger is bent from its rest pose. For flexion/extension joints, positive angles indicate
+ // flexion while negative angles indicate extension.
+ //
+ // Recommended for detecting gestures of the form "finger X is bending." To know the global position/orientation
+ // of the hand, use HandTrackingData.jointPositions/HandTrackingData.jointQuaternions instead.
+ enum FingerDOF
+ {
+ THUMB_CMC_AA = 0, ///< Thumb carpal-metacarpal joint, adduction/abduction
+ THUMB_CMC_FE = 1, ///< Thumb carpal-metacarpal joint, flexion/extension
+ THUMB_MCP = 2, ///< Thumb metacarpal-phalangeal joint, flexion/extension
+ THUMB_IP = 3, ///< Thumb interphalangeal joint, flexion/extension
+ INDEX_MCP_AA = 4, ///< Index finger metacarpal-phalangeal joint, adduction/abduction
+ INDEX_MCP_FE = 5, ///< Index finger metacarpal-phalangeal joint, flexion/extension
+ INDEX_PIP = 6, ///< Index finger proximal interphalangeal joint, flexion/extension
+ MIDDLE_MCP_AA = 7, ///< Middle finger metacarpal-phalangeal joint, adduction/abduction
+ MIDDLE_MCP_FE = 8, ///< Middle finger metacarpal-phalangeal joint, flexion/extension
+ MIDDLE_PIP = 9, ///< Middle finger proximal interphalangeal joint, flexion/extension
+ RING_MCP_AA = 10, ///< Ring finger metacarpal-phalangeal joint, adduction/abduction
+ RING_MCP_FE = 11, ///< Ring finger metacarpal-phalangeal joint, flexion/extension
+ RING_PIP = 12, ///< Ring finger proximal interphalangeal joint, flexion/extension
+ SMALL_MCP_AA = 13, ///< Small finger metacarpal-phalangeal joint, adduction/abduction
+ SMALL_MCP_FE = 14, ///< Small finger metacarpal-phalangeal joint, flexion/extension
+ SMALL_PIP = 15, ///< Small finger proximal interphalangeal joint, flexion/extension
+ NUM_FINGER_DOFS_PER_HAND
+ };
+
+ struct HandData
+ {
+ /// Transform of the hand (w.r.t JointFrameIndex.ROOT_JOINT).
+ RigidTransform3d pose;
+ /// Number of times the hand was clicked. 0, 1 or 2.
+ int clickCount;
+ /// Value between 0 and 1 to specify the confidence of the poses. Currently, either 0 or 1.
+ double confidenceEstimate;
+ /// Transform of each of the joints.
+ std::array<RigidTransform3d, NUM_JOINTS> jointPoses;
+ /// Position of each of the finger tips.
+ std::array<Vector3d, NUM_FINGERS> fingerTips;
+ /// Value between 0 and 1 to specify the confidence of the hand being in one of the N_POSES poses. Sums to 1.
+ std::array<double, NUM_POSES> handPoseConfidences;
+ /// The angle of each of the finger joints.
+ std::array<double, NUM_FINGER_DOFS_PER_HAND> fingerDofs;
+ };
+
+ std::array<HandData, 2> hands;
+};
+
+/// Parse the values in the stream into a Vector3d.
+/// \param in The stream from where data is parsed.
+/// \param [out] vector The object to which the parsed values are written to.
+/// \return The stream that was sent in.
+std::istream& operator>> (std::istream& in, Vector3d& vector)
+{
+ std::istream::sentry sentry(in);
+ if (sentry)
+ {
+ in >> vector.x();
+ in >> vector.y();
+ in >> vector.z();
+ }
+ return in;
+}
+
+/// Parse the values in the stream into a Quaterniond.
+/// \param in The stream from where data is parsed.
+/// \param [out] quaternion The object to which the parsed values are written to.
+/// \return The stream that was sent in.
+std::istream& operator>> (std::istream& in, Quaterniond& quaternion)
+{
+ std::istream::sentry sentry(in);
+ if (sentry)
+ {
+ in >> quaternion.x();
+ in >> quaternion.y();
+ in >> quaternion.z();
+ in >> quaternion.w();
+ }
+ return in;
+}
+
+/// Parse the values in the stream into the HandTracking Data structure.
+/// \param in The stream from where data is parsed.
+/// \param [out] handData The object to which the parsed values are written to.
+/// \return The stream that was sent in.
+std::istream& operator>> (std::istream& in, HandTrackingData& handData)
+{
+ Vector3d position;
+ Quaterniond quaternion;
+
+ for (auto hand = handData.hands.begin(); in.good() && hand != handData.hands.end(); ++hand)
+ {
+ in >> position;
+ hand->pose.translation() = position;
+ in >> quaternion;
+ hand->pose.linear() = quaternion.matrix();
+ in >> hand->clickCount;
+ }
+
+ for (auto hand = handData.hands.begin(); in.good() && hand != handData.hands.end(); ++hand)
+ {
+ in >> hand->confidenceEstimate;
+
+ for (auto jointPose = hand->jointPoses.begin(); in.good() && jointPose != hand->jointPoses.end(); ++jointPose)
+ {
+ in >> quaternion;
+ jointPose->linear() = quaternion.matrix();
+ in >> position;
+ jointPose->translation() = position;
+ }
+
+ for (auto fingerTip = hand->fingerTips.begin(); in.good() && fingerTip != hand->fingerTips.end(); ++fingerTip)
+ {
+ in >> *fingerTip;
+ }
+ }
+
+ for (auto hand = handData.hands.begin(); in.good() && hand != handData.hands.end(); ++hand)
+ {
+ for (auto handPoseConfidence = hand->handPoseConfidences.begin(); in.good() &&
+ handPoseConfidence != hand->handPoseConfidences.end(); ++handPoseConfidence)
+ {
+ in >> *handPoseConfidence;
+ }
+ }
+
+ for (auto hand = handData.hands.begin(); in.good() && hand != handData.hands.end(); ++hand)
+ {
+ for (auto fingerDof = hand->fingerDofs.begin(); in.good() && fingerDof != hand->fingerDofs.end(); ++fingerDof)
+ {
+ in >> *fingerDof;
+ }
+ }
+
+ return in;
+}
+
+}
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+std::array<std::pair<std::string, int>, 15> NimbleScaffold::m_jointPoseNames =
+{
+ std::make_pair("ThumbProximal", HandTrackingData::THUMB_PROXIMAL),
+ std::make_pair("ThumbIntermediate", HandTrackingData::THUMB_INTERMEDIATE),
+ std::make_pair("ThumbDistal", HandTrackingData::THUMB_DISTAL),
+ std::make_pair("IndexFingerProximal", HandTrackingData::INDEX_PROXIMAL),
+ std::make_pair("IndexFingerIntermediate", HandTrackingData::INDEX_INTERMEDIATE),
+ std::make_pair("IndexFingerDistal", HandTrackingData::INDEX_DISTAL),
+ std::make_pair("MiddleFingerProximal", HandTrackingData::MIDDLE_PROXIMAL),
+ std::make_pair("MiddleFingerIntermediate", HandTrackingData::MIDDLE_INTERMEDIATE),
+ std::make_pair("MiddleFingerDistal", HandTrackingData::MIDDLE_DISTAL),
+ std::make_pair("RingFingerProximal", HandTrackingData::RING_PROXIMAL),
+ std::make_pair("RingFingerIntermediate", HandTrackingData::RING_INTERMEDIATE),
+ std::make_pair("RingFingerDistal", HandTrackingData::RING_DISTAL),
+ std::make_pair("SmallFingerProximal", HandTrackingData::SMALL_PROXIMAL),
+ std::make_pair("SmallFingerIntermediate", HandTrackingData::SMALL_INTERMEDIATE),
+ std::make_pair("SmallFingerDistal", HandTrackingData::SMALL_DISTAL)
+};
+
+struct NimbleScaffold::StateData
+{
+public:
+ /// Initialize the state.
+ StateData()
+ {
+ }
+
+ /// The socket used for connecting to the Nimble server.
+ boost::asio::ip::tcp::iostream socketStream;
+
+ /// The hand tracking data.
+ HandTrackingData handData;
+
+ /// The list of active devices.
+ std::vector<NimbleDevice*> activeDevices;
+
+ /// The mutex that protects the active device.
+ boost::mutex mutex;
+
+private:
+ // Prohibit copy construction and assignment
+ StateData(const StateData&);
+ StateData& operator=(const StateData&);
+};
+
+NimbleScaffold::NimbleScaffold() :
+ SurgSim::Framework::BasicThread("Nimble Scaffold"), m_logger(Framework::Logger::getLogger("Devices/Nimble")),
+ m_state(new StateData()), m_serverIpAddress("127.0.0.1"), m_serverPort("1988"),
+ m_serverSocketOpen(false)
+{
+ setRate(1000.0);
+ SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold created.";
+}
+
+NimbleScaffold::~NimbleScaffold()
+{
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ if (m_state->activeDevices.size() > 0)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Nimble: Destroying scaffold while devices are active!?!";
+ }
+ }
+
+ SURGSIM_LOG_DEBUG(m_logger) << "Nimble: Shared scaffold destroyed.";
+}
+
+bool NimbleScaffold::registerDevice(NimbleDevice* device)
+{
+ bool success = true;
+
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ auto found = std::find_if(m_state->activeDevices.begin(), m_state->activeDevices.end(),
+ [device](const NimbleDevice* it) { return it->getName() == device->getName(); });
+
+ if (found == m_state->activeDevices.end())
+ {
+ m_state->activeDevices.push_back(device);
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << " registered in Scaffold.";
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Attempted to register device " << device->getName() <<
+ " with the same name again.";
+ success = false;
+ }
+ }
+
+ if (success && !isRunning())
+ {
+ std::shared_ptr<SurgSim::Framework::Barrier> barrier = std::make_shared<SurgSim::Framework::Barrier>(2);
+ start(barrier);
+ barrier->wait(true); // Wait for initialize
+ barrier->wait(true); // Wait for startup
+ success = isInitialized();
+ }
+
+ return success;
+}
+
+bool NimbleScaffold::unregisterDevice(const NimbleDevice* device)
+{
+ bool success = true;
+
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ auto found = std::find(m_state->activeDevices.begin(), m_state->activeDevices.end(), device);
+
+ if (found != m_state->activeDevices.end())
+ {
+ m_state->activeDevices.erase(found);
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << " unregistered from Scaffold.";
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Attempted to unregister a device " << device->getName() <<
+ " from Scaffold which is not registered.";
+ success = false;
+ }
+ }
+
+ if (success && isRunning() && m_state->activeDevices.size() == 0)
+ {
+ stop();
+ }
+
+ return success;
+}
+
+bool NimbleScaffold::doInitialize()
+{
+ // Connect to the Nimble hand tracking server.
+ m_serverSocketOpen = true;
+
+ m_state->socketStream.connect(m_serverIpAddress, m_serverPort);
+
+ if (!m_state->socketStream)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Nimble: Error while opening a iostream to the server: "
+ << m_state->socketStream.error().message() << ")";
+ m_serverSocketOpen = false;
+ }
+
+ return m_serverSocketOpen;
+}
+
+bool NimbleScaffold::doStartUp()
+{
+ return true;
+}
+
+bool NimbleScaffold::doUpdate(double dt)
+{
+ bool success = true;
+
+ if (!m_state->socketStream)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Nimble: Socket stream no longer good: "
+ << m_state->socketStream.error().message() << ")";
+ success = false;
+ }
+ else
+ {
+ std::string messageType;
+ m_state->socketStream >> messageType;
+
+ if (messageType == "POSE")
+ {
+ m_state->socketStream >> m_state->handData;
+ if (!m_state->socketStream.fail())
+ {
+ updateDeviceData();
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger)
+ << "Nimble: Hand data not parsed correctly.";
+ resetDeviceData();
+ }
+ }
+ else
+ {
+ m_state->socketStream.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
+ }
+ }
+
+ return success;
+}
+
+void NimbleScaffold::doBeforeStop()
+{
+ // This would be killed soon, so the socket is closed here.
+ if (m_serverSocketOpen)
+ {
+ m_state->socketStream.close();
+ if (m_state->socketStream.fail())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Nimble: Error when shutting down socket: "
+ << m_state->socketStream.error().message() << ")";
+ }
+ m_serverSocketOpen = false;
+ }
+}
+
+void NimbleScaffold::updateDeviceData()
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ for (auto it = m_state->activeDevices.begin(); it != m_state->activeDevices.end(); ++it)
+ {
+ size_t index = (*it)->m_trackedHandDataIndex;
+
+ SurgSim::DataStructures::DataGroup& inputData = (*it)->getInputData();
+ inputData.poses().set(SurgSim::DataStructures::Names::POSE, m_state->handData.hands[index].pose);
+ for (auto name = m_jointPoseNames.begin(); name != m_jointPoseNames.end(); ++name)
+ {
+ inputData.poses().set(name->first, m_state->handData.hands[index].jointPoses[name->second]);
+ }
+ (*it)->pushInput();
+ }
+}
+
+void NimbleScaffold::resetDeviceData()
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ for (auto it = m_state->activeDevices.begin(); it != m_state->activeDevices.end(); ++it)
+ {
+ SurgSim::DataStructures::DataGroup& inputData = (*it)->getInputData();
+ inputData.resetAll();
+ }
+}
+
+SurgSim::DataStructures::DataGroup NimbleScaffold::buildDeviceInputData()
+{
+ SurgSim::DataStructures::DataGroupBuilder builder;
+ builder.addPose(SurgSim::DataStructures::Names::POSE);
+ for (auto name = m_jointPoseNames.begin(); name != m_jointPoseNames.end(); ++name)
+ {
+ builder.addPose(name->first);
+ }
+ return builder.createData();
+}
+
+std::shared_ptr<NimbleScaffold> NimbleScaffold::getOrCreateSharedInstance()
+{
+ // Using an explicit creation function gets around problems with accessing the private constructor.
+ static auto creator =
+ []() { return std::shared_ptr<NimbleScaffold>(new NimbleScaffold()); }; // NOLINT(readability/braces)
+ static SurgSim::Framework::SharedInstance<NimbleScaffold> sharedInstance(creator);
+ return sharedInstance.get();
+}
+
+} // namespace Devices
+} // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
diff --git a/SurgSim/Devices/Nimble/NimbleScaffold.h b/SurgSim/Devices/Nimble/NimbleScaffold.h
new file mode 100644
index 0000000..ac5ad62
--- /dev/null
+++ b/SurgSim/Devices/Nimble/NimbleScaffold.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_DEVICES_NIMBLE_NIMBLESCAFFOLD_H
+#define SURGSIM_DEVICES_NIMBLE_NIMBLESCAFFOLD_H
+
+#include <memory>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Framework/BasicThread.h"
+#include "SurgSim/Framework/Logger.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+class NimbleDevice;
+class NimbleThread;
+
+/// A class that manages Nimble devices.
+///
+/// \sa SurgSim::Devices::NimbleDevice
+class NimbleScaffold : public SurgSim::Framework::BasicThread
+{
+public:
+ /// Destructor.
+ ~NimbleScaffold();
+
+ /// Gets or creates the scaffold shared by all NimbleDevice instances.
+ /// The scaffold is managed using a SingleInstance object.
+ /// \return the scaffold object.
+ static std::shared_ptr<NimbleScaffold> getOrCreateSharedInstance();
+
+protected:
+ bool doInitialize() override;
+ bool doStartUp() override;
+ bool doUpdate(double dt) override;
+ void doBeforeStop() override;
+
+private:
+ /// Internal shared state data type.
+ struct StateData;
+
+ friend class NimbleDevice;
+ friend class NimbleThread;
+
+ /// Constructor.
+ NimbleScaffold();
+
+ /// Registers the specified device object.
+ /// \param device The device object to be used.
+ /// \return True if the initialization succeeds, false if it fails.
+ /// \note There can be only one NimbleDevice at a time.
+ bool registerDevice(NimbleDevice* device);
+
+ /// Unregisters the specified device object.
+ /// \param device The device object.
+ /// \return true on success, false on failure.
+ bool unregisterDevice(const NimbleDevice* device);
+
+ /// Update the devices based on the data read from the Nimble server.
+ void updateDeviceData();
+
+ /// Reset the device data.
+ void resetDeviceData();
+
+ /// 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 IP address of the Nimble hand tracking server.
+ std::string m_serverIpAddress;
+ /// The port where the server is communicating.
+ std::string m_serverPort;
+ /// Flag to indicate that the socket is opened successfully.
+ bool m_serverSocketOpen;
+
+ /// The data group name for the joint poses, and the corresponding indices within the state data.
+ static std::array<std::pair<std::string, int>, 15> m_jointPoseNames;
+};
+
+}; // namespace Devices
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_NIMBLE_NIMBLESCAFFOLD_H
\ No newline at end of file
diff --git a/SurgSim/Devices/Nimble/UnitTests/CMakeLists.txt b/SurgSim/Devices/Nimble/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..d87a54e
--- /dev/null
+++ b/SurgSim/Devices/Nimble/UnitTests/CMakeLists.txt
@@ -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_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ NimbleDeviceTest.cpp
+ NimbleScaffoldTest.cpp
+)
+
+set(LIBS
+ NimbleDevice
+)
+
+surgsim_add_unit_tests(NimbleDeviceTest)
+
+set_target_properties(NimbleDeviceTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Nimble/UnitTests/NimbleDeviceTest.cpp b/SurgSim/Devices/Nimble/UnitTests/NimbleDeviceTest.cpp
new file mode 100644
index 0000000..8d3c465
--- /dev/null
+++ b/SurgSim/Devices/Nimble/UnitTests/NimbleDeviceTest.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.
+
+/// \file
+/// Tests for the NimbleDevice class.
+
+#include <memory>
+#include <string>
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/Nimble/NimbleDevice.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::Devices::NimbleDevice;
+using SurgSim::Devices::NimbleScaffold;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Testing::MockInputOutput;
+
+TEST(NimbleDeviceTest, CreateUninitializedDevice)
+{
+ std::shared_ptr<NimbleDevice> device = std::make_shared<NimbleDevice>("TestNimble");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+}
+
+TEST(NimbleDeviceTest, CreateAndInitializeDevice)
+{
+ std::shared_ptr<NimbleDevice> device = std::make_shared<NimbleDevice>("TestNimble");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed.";
+ EXPECT_TRUE(device->isInitialized());
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(100));
+}
+
+TEST(NimbleDeviceTest, Name)
+{
+ std::shared_ptr<NimbleDevice> device = std::make_shared<NimbleDevice>("TestNimble");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_EQ("TestNimble", device->getName());
+ EXPECT_TRUE(device->initialize()) << "Initialization failed.";
+ EXPECT_EQ("TestNimble", device->getName());
+}
+
+TEST(NimbleDeviceTest, CreateDeviceSeveralTimes)
+{
+ for (int i = 0; i < 6; ++i)
+ {
+ std::shared_ptr<NimbleDevice> device = std::make_shared<NimbleDevice>("TestNimble");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed.";
+ // Rapidly initializing and destructing causes the thread destructor to be called before it has been
+ // stopped (by calling stop()), this sleep prevents the problem.
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(100));
+ }
+}
+
+TEST(NimbleDeviceTest, CreateSeveralDevices)
+{
+ std::shared_ptr<NimbleDevice> device1 = std::make_shared<NimbleDevice>("Nimble1");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed.";
+
+ std::shared_ptr<NimbleDevice> device2 = std::make_shared<NimbleDevice>("Nimble2");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device2->initialize()) << "Initialization failed.";
+}
+
+TEST(NimbleDeviceTest, InputConsumer)
+{
+ std::shared_ptr<NimbleDevice> device = std::make_shared<NimbleDevice>("TestNimbleLeft");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed.";
+
+ 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.
+ // (The Nimble server updates internally at 30Hz, but our code currently runs 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.
+ // The input is only pushed if a valid pose was detected. So, the minimum m_numTimesReceivedInput is 1.
+ EXPECT_EQ(1, consumer->m_numTimesInitializedInput);
+ EXPECT_GE(consumer->m_numTimesReceivedInput, 1);
+ EXPECT_LE(consumer->m_numTimesReceivedInput, 30);
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData(SurgSim::DataStructures::Names::POSE));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("ThumbProximal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("ThumbIntermediate"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("ThumbDistal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("IndexFingerProximal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("IndexFingerIntermediate"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("IndexFingerDistal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("MiddleFingerProximal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("MiddleFingerIntermediate"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("MiddleFingerDistal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("RingFingerProximal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("RingFingerIntermediate"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("RingFingerDistal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("SmallFingerProximal"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("SmallFingerIntermediate"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData("SmallFingerDistal"));
+}
+
+TEST(NimbleDeviceTest, OutputProducer)
+{
+ //NimbleScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ std::shared_ptr<NimbleDevice> device = std::make_shared<NimbleDevice>("TestNimble");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed.";
+
+ 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 Nimble 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/Nimble/UnitTests/NimbleScaffoldTest.cpp b/SurgSim/Devices/Nimble/UnitTests/NimbleScaffoldTest.cpp
new file mode 100644
index 0000000..9fa937c
--- /dev/null
+++ b/SurgSim/Devices/Nimble/UnitTests/NimbleScaffoldTest.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.
+
+/// \file
+/// Tests for the NimbleScaffold class and its device interactions.
+
+#include <memory>
+#include <string>
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include "SurgSim/Devices/Nimble/NimbleDevice.h"
+#include "SurgSim/Devices/Nimble/NimbleScaffold.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+
+using SurgSim::Devices::NimbleDevice;
+using SurgSim::Devices::NimbleScaffold;
+
+TEST(NimbleScaffoldTest, CreateAndDestroyScaffold)
+{
+ std::shared_ptr<NimbleScaffold> scaffold = NimbleScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created!";
+ std::weak_ptr<NimbleScaffold> scaffold1 = scaffold;
+ {
+ std::shared_ptr<NimbleScaffold> 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<NimbleScaffold> sameScaffold = NimbleScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, sameScaffold) << "Unable to get scaffold from class";
+ EXPECT_EQ(scaffold, sameScaffold) << "Scaffold mismatch!";
+ }
+
+ scaffold.reset();
+ {
+ std::shared_ptr<NimbleScaffold> dontHaveScaffold = scaffold1.lock();
+ EXPECT_EQ(nullptr, dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ }
+
+ scaffold = NimbleScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created the 2nd time!";
+ std::weak_ptr<NimbleScaffold> scaffold2 = scaffold;
+ {
+ std::shared_ptr<NimbleScaffold> 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(NimbleScaffoldTest, ScaffoldLifeCycle)
+{
+ std::weak_ptr<NimbleScaffold> lastScaffold;
+ {
+ std::shared_ptr<NimbleScaffold> scaffold = NimbleScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not created!";
+ lastScaffold = scaffold;
+ }
+ {
+ std::shared_ptr<NimbleScaffold> dontHaveScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, dontHaveScaffold) << "Able to get scaffold from weak ref (with no strong ref)";
+ lastScaffold.reset();
+ }
+
+ {
+ std::shared_ptr<NimbleDevice> device = std::make_shared<NimbleDevice>("TestNimble");
+ ASSERT_NE(nullptr, device) << "Creation failed.";
+ // note: the device is NOT initialized!
+ {
+ std::shared_ptr<NimbleScaffold> scaffold = NimbleScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold; // save the scaffold for later
+
+ std::shared_ptr<NimbleScaffold> 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<NimbleScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+ // the ("empty") device is about to get destroyed
+ }
+
+ {
+ std::shared_ptr<NimbleDevice> device = std::make_shared<NimbleDevice>("TestNimble");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed.";
+ {
+ std::shared_ptr<NimbleScaffold> scaffold = NimbleScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold; // save the scaffold for later
+
+ std::shared_ptr<NimbleScaffold> 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<NimbleScaffold> sameScaffold = lastScaffold.lock();
+ EXPECT_NE(nullptr, sameScaffold);
+
+ std::shared_ptr<NimbleScaffold> scaffold = NimbleScaffold::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<NimbleScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+
+ {
+ std::shared_ptr<NimbleDevice> device = std::make_shared<NimbleDevice>("TestNimble");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed.";
+ std::shared_ptr<NimbleScaffold> scaffold = NimbleScaffold::getOrCreateSharedInstance();
+ EXPECT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+
+ std::shared_ptr<NimbleScaffold> deadScaffold = lastScaffold.lock();
+ EXPECT_EQ(nullptr, deadScaffold);
+ }
+}
+
+
+TEST(NimbleScaffoldTest, CreateDeviceSeveralTimes)
+{
+ std::weak_ptr<NimbleScaffold> lastScaffold;
+
+ for (int i = 0; i < 6; ++i)
+ {
+ SCOPED_TRACE(i);
+ EXPECT_EQ(nullptr, lastScaffold.lock());
+ std::shared_ptr<NimbleDevice> device = std::make_shared<NimbleDevice>("TestNimble");
+ ASSERT_NE(nullptr, device) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed.";
+ std::shared_ptr<NimbleScaffold> scaffold = NimbleScaffold::getOrCreateSharedInstance();
+ ASSERT_NE(nullptr, scaffold) << "The scaffold was not retrieved!";
+ lastScaffold = scaffold;
+ // the device and the scaffold will be destroyed here
+ }
+}
+
+
+TEST(NimbleScaffoldTest, CreateDeviceSeveralTimesWithScaffoldRef)
+{
+ std::shared_ptr<NimbleScaffold> lastScaffold;
+
+ for (int i = 0; i < 6; ++i)
+ {
+ SCOPED_TRACE(i);
+ std::shared_ptr<NimbleDevice> device = std::make_shared<NimbleDevice>("TestNimble");
+ ASSERT_NE(nullptr, device) << "Attempt " << i + 1 << ": Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Attempt " << i + 1 << ": Initialization failed.";
+ std::shared_ptr<NimbleScaffold> scaffold = NimbleScaffold::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/Nimble/VisualTest/CMakeLists.txt b/SurgSim/Devices/Nimble/VisualTest/CMakeLists.txt
new file mode 100644
index 0000000..439943c
--- /dev/null
+++ b/SurgSim/Devices/Nimble/VisualTest/CMakeLists.txt
@@ -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.
+
+
+set(EXAMPLE_SOURCES
+ main.cpp
+)
+
+set(EXAMPLE_HEADERS
+)
+
+surgsim_add_executable(NimbleVisualTest
+ "${EXAMPLE_SOURCES}" "${EXAMPLE_HEADERS}")
+
+set(LIBS
+ NimbleDevice
+ SurgSimDeviceFilters
+ SurgSimInput
+ VisualTestCommon
+)
+
+target_link_libraries(NimbleVisualTest ${LIBS})
+
+set_target_properties(NimbleVisualTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Nimble/VisualTest/main.cpp b/SurgSim/Devices/Nimble/VisualTest/main.cpp
new file mode 100644
index 0000000..9fb69cf
--- /dev/null
+++ b/SurgSim/Devices/Nimble/VisualTest/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/DeviceFilters/PoseTransform.h"
+#include "SurgSim/Devices/Nimble/NimbleDevice.h"
+
+#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
+
+using SurgSim::Input::DeviceInterface;
+using SurgSim::Devices::PoseTransform;
+using SurgSim::Devices::NimbleDevice;
+
+int main(int argc, char** argv)
+{
+ auto leftDevice = std::make_shared<NimbleDevice>("NimbleDeviceLeft");
+ auto leftDeviceFilter = std::make_shared<PoseTransform>("NimbleDeviceLeftDeviceFilter");
+ leftDeviceFilter->setTranslationScale(0.0001);
+ leftDevice->addInputConsumer(leftDeviceFilter);
+ leftDevice->setOutputProducer(leftDeviceFilter);
+ leftDevice->initialize();
+
+ auto rightDevice = std::make_shared<NimbleDevice>("NimbleDeviceRight");
+ rightDevice->setupToTrackRightHand();
+ auto rightDeviceFilter = std::make_shared<PoseTransform>("NimbleDeviceRightDeviceFilter");
+ rightDeviceFilter->setTranslationScale(0.0001);
+ rightDevice->addInputConsumer(rightDeviceFilter);
+ rightDevice->setOutputProducer(rightDeviceFilter);
+ rightDevice->initialize();
+
+ runToolSquareTest(leftDeviceFilter, rightDeviceFilter, "Move the hands to control the sphere/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/Novint/CMakeLists.txt b/SurgSim/Devices/Novint/CMakeLists.txt
index 8c98bea..7237066 100644
--- a/SurgSim/Devices/Novint/CMakeLists.txt
+++ b/SurgSim/Devices/Novint/CMakeLists.txt
@@ -19,34 +19,39 @@ find_package(NovintHdalSdk REQUIRED)
link_directories(${Boost_LIBRARY_DIRS})
set(LIBS
+ SurgSimDataStructures
+ SurgSimFramework
+ SurgSimInput
+ SurgSimMath
${Boost_LIBRARIES}
- ${NOVINT_HDAL_SDK_LIBRARIES}
+ ${NOVINTHDALSDK_LIBRARIES}
+ ${YAML_CPP_LIBRARIES}
)
include_directories(
"${CMAKE_CURRENT_SOURCE_DIR}"
- "${NOVINT_HDAL_SDK_INCLUDE_DIR}"
+ "${NOVINTHDALSDK_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
)
+set(DEVICE_HEADERS ${DEVICE_HEADERS}
+ Novint/NovintDevice.h #NOLINT
+ PARENT_SCOPE)
+
# TODO(advornik): the installation should NOT copy all the headers...
surgsim_add_library(
NovintDevice
- "${NOVINT_DEVICE_SOURCES}" "${NOVINT_DEVICE_HEADERS}"
- SurgSim/Devices/Novint
+ "${NOVINT_DEVICE_SOURCES}"
+ "${NOVINT_DEVICE_HEADERS}"
)
target_link_libraries(NovintDevice ${LIBS})
diff --git a/SurgSim/Devices/Novint/Novint7DofDevice.cpp b/SurgSim/Devices/Novint/Novint7DofDevice.cpp
deleted file mode 100644
index 0b5a855..0000000
--- a/SurgSim/Devices/Novint/Novint7DofDevice.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 524fe3a..0000000
--- a/SurgSim/Devices/Novint/Novint7DofDevice.h
+++ /dev/null
@@ -1,72 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
deleted file mode 100644
index 0e22c8b..0000000
--- a/SurgSim/Devices/Novint/NovintCommonDevice.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 43fff8d..0000000
--- a/SurgSim/Devices/Novint/NovintCommonDevice.h
+++ /dev/null
@@ -1,97 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
index 943e22c..7333a53 100644
--- a/SurgSim/Devices/Novint/NovintDevice.cpp
+++ b/SurgSim/Devices/Novint/NovintDevice.cpp
@@ -15,42 +15,199 @@
#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/DataStructures/DataStructuresConvert.h"
#include "SurgSim/Devices/Novint/NovintScaffold.h"
-#include "SurgSim/DataStructures/DataGroup.h"
-#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Math/MathConvert.h"
-using SurgSim::Math::Vector3d;
-using SurgSim::Math::Matrix44d;
-using SurgSim::Math::Matrix33d;
-using SurgSim::Math::RigidTransform3d;
+using SurgSim::DataStructures::OptionalValue;
-using SurgSim::DataStructures::DataGroup;
-using SurgSim::DataStructures::DataGroupBuilder;
+namespace SurgSim
+{
+namespace Devices
+{
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::NovintDevice, NovintDevice);
-namespace SurgSim
+NovintDevice::NovintDevice(const std::string& uniqueName) :
+ Input::CommonDevice(uniqueName, NovintScaffold::buildDeviceInputData()),
+ m_positionScale(1.0),
+ m_orientationScale(1.0),
+ m_7DofDevice(false),
+ m_maxForce(8.9),
+ m_antigrav(Math::Vector3d::Zero())
{
-namespace Device
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(NovintDevice, OptionalValue<std::string>, InitializationName,
+ getOptionalInitializationName, setOptionalInitializationName);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(NovintDevice, OptionalValue<std::string>, SerialNumber,
+ getOptionalSerialNumber, setOptionalSerialNumber);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(NovintDevice, bool, 7DofDevice, is7DofDevice, set7DofDevice);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(NovintDevice, double, PositionScale, getPositionScale, setPositionScale);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(NovintDevice, double, OrientationScale, getOrientationScale, setOrientationScale);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(NovintDevice, double, MaxForce, getMaxForce, setMaxForce);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(NovintDevice, Math::Vector3d, Antigrav, getAntigrav, setAntigrav);
+}
+
+NovintDevice::~NovintDevice()
{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+void NovintDevice::setSerialNumber(const std::string& serialNumber)
+{
+ SURGSIM_ASSERT(!m_initializationName.hasValue()) << "Cannot set serialNumber for a NovintDevice named " <<
+ getName() << ", which already has an initializationName.";
+ SURGSIM_ASSERT(!isInitialized()) <<
+ "Cannot setSerialNumber after the device named " <<
+ getName() << " has been initialized.";
+ m_serialNumber.setValue(serialNumber);
+}
+
+bool NovintDevice::getSerialNumber(std::string* serialNumber) const
+{
+ const bool hasValue = m_serialNumber.hasValue();
+ if (hasValue)
+ {
+ *serialNumber = m_serialNumber.getValue();
+ }
+ return hasValue;
+}
-NovintDevice::NovintDevice(const std::string& uniqueName, const std::string& initializationName) :
- NovintCommonDevice(uniqueName, initializationName)
+void NovintDevice::setInitializationName(const std::string& initializationName)
{
+ SURGSIM_ASSERT(!m_serialNumber.hasValue()) << "Cannot set initializationName for a NovintDevice named " <<
+ getName() << ", which already has a serialNumber.";
+ SURGSIM_ASSERT(!isInitialized()) <<
+ "Cannot setInitializationName after the device named " <<
+ getName() << " has been initialized.";
+ m_initializationName.setValue(initializationName);
}
+bool NovintDevice::getInitializationName(std::string* initializationName) const
+{
+ const bool hasValue = m_initializationName.hasValue();
+ if (hasValue)
+ {
+ *initializationName = m_initializationName.getValue();
+ }
+ return hasValue;
+}
-NovintDevice::~NovintDevice()
+bool NovintDevice::initialize()
+{
+ SURGSIM_ASSERT(!isInitialized()) << getName() << " already initialized.";
+ auto scaffold = NovintScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold != nullptr);
+
+ bool initialize = false;
+ if (scaffold->registerDevice(this))
+ {
+ m_scaffold = std::move(scaffold);
+ initialize = true;
+ }
+ return initialize;
+}
+
+bool NovintDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized()) << getName() << " is not initialized, cannot finalize.";
+ bool result = m_scaffold->unregisterDevice(this);
+ m_scaffold.reset();
+ return result;
+}
+
+bool NovintDevice::isInitialized() const
+{
+ return (m_scaffold != nullptr);
+}
+
+void NovintDevice::setPositionScale(double scale)
+{
+ m_positionScale = scale;
+ if (m_scaffold)
+ {
+ m_scaffold->setPositionScale(this, m_positionScale);
+ }
+}
+
+double NovintDevice::getPositionScale() const
+{
+ return m_positionScale;
+}
+
+void NovintDevice::setOrientationScale(double scale)
{
+ m_orientationScale = scale;
+ if (m_scaffold)
+ {
+ m_scaffold->setOrientationScale(this, m_orientationScale);
+ }
}
+double NovintDevice::getOrientationScale() const
+{
+ return m_orientationScale;
+}
+
+void NovintDevice::set7DofDevice(bool val)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot set 7Dof status after initialization.";
+ m_7DofDevice = val;
+}
+
+bool NovintDevice::is7DofDevice() const
+{
+ return m_7DofDevice;
+}
+
+void NovintDevice::setMaxForce(double force)
+{
+ SURGSIM_ASSERT(!isInitialized()) <<
+ "Cannot setMaxForce after the device named " <<
+ getName() << " has been initialized.";
+ SURGSIM_ASSERT(force >= 0.0) << "Cannot set a negative maximum force magnitude on device named " << getName();
+ m_maxForce = force;
+}
+
+double NovintDevice::getMaxForce() const
+{
+ return m_maxForce;
+}
+
+void NovintDevice::setAntigrav(Math::Vector3d antigrav)
+{
+ SURGSIM_ASSERT(!isInitialized()) <<
+ "Cannot setAntigrav after the device named " <<
+ getName() << " has been initialized.";
+ m_antigrav = antigrav;
+}
+
+Math::Vector3d NovintDevice::getAntigrav() const
+{
+ return m_antigrav;
+}
+
+const OptionalValue<std::string>& NovintDevice::getOptionalInitializationName() const
+{
+ return m_initializationName;
+}
+
+void NovintDevice::setOptionalInitializationName(const OptionalValue<std::string>& name)
+{
+ m_initializationName = name;
+}
+
+const OptionalValue<std::string>& NovintDevice::getOptionalSerialNumber() const
+{
+ return m_serialNumber;
+}
+
+void NovintDevice::setOptionalSerialNumber(const OptionalValue<std::string>& serial)
+{
+ m_serialNumber = serial;
+}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/Novint/NovintDevice.h b/SurgSim/Devices/Novint/NovintDevice.h
index 8573efe..8f99949 100644
--- a/SurgSim/Devices/Novint/NovintDevice.h
+++ b/SurgSim/Devices/Novint/NovintDevice.h
@@ -16,59 +16,176 @@
#ifndef SURGSIM_DEVICES_NOVINT_NOVINTDEVICE_H
#define SURGSIM_DEVICES_NOVINT_NOVINTDEVICE_H
-#include <memory>
#include <string>
-#include "SurgSim/Devices/Novint/NovintCommonDevice.h"
+#include "SurgSim/DataStructures/OptionalValue.h"
+#include "SurgSim/Input/CommonDevice.h"
namespace SurgSim
{
-namespace Device
+namespace Devices
{
+class NovintScaffold;
+SURGSIM_STATIC_REGISTRATION(NovintDevice);
/// 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.
+/// Grip will not be able to produce torques unless it is accessed using the NovintOsgDevice 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.
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | pose | "pose" | %Device pose (units are meters)*. |
+/// | scalar | "toolDof" | %7th Dof (e.g., handle open/close angle) |
+/// | 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. |
+/// | bool | "isPositionHomed" | %Device homing status, position only. |
+/// | bool | "isOrientationHomed" | %Device homing status, orientation only. |
+/// * The workspace of an off-the-shelf Novint Falcon is +z points out from the face of the Falcon, +y points up, and
+/// +x points to your right if you are looking at the face of the Falcon (a right-hand frame). The workspace of
+/// both 7dof OSG Falcons in a pair is +x towards the right Falcon, +y up, and +z towards the user
+/// (a right-hand frame). Thus, if the 7dof OSG Falcons are setup in a typical configuration such that the
+/// left Falcon is to the user's left and the right Falcon is to the user's right, then the workspace will be the
+/// same as an off-the-shelf Falcon placed directly facing the user. The identity orientation for an OSG Falcon
+/// is with the handle of the tool parallel to the floor (pitch), and pointed towards the user (yaw)...therefore
+/// in the home pose, the "business end" of the tool points away from the user along the -z axis.
+/// **\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). |
+/// | vector | "torque" | %Device output torque (in newton-meters, 7Dof only). |
/// | bool | "gravityCompensation" | Enable or disable hardware gravity compensation. |
///
-/// \sa Novint7DofHapticDevice
-/// \sa NovintCommonDevice, SurgSim::Input::CommonDevice, SurgSim::Input::DeviceInterface
-class NovintDevice : public NovintCommonDevice
+class NovintDevice : 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.
- NovintDevice(const std::string& uniqueName, const std::string& initializationName);
+ /// \param name A unique name for the device that will be used by the application.
+ explicit NovintDevice(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Devices::NovintDevice);
/// Destructor.
virtual ~NovintDevice();
+
+ /// Sets the serial number used to register this device with the hardware library.
+ /// An empty string indicates the first available device.
+ /// \param serialNumber The serial number.
+ /// \sa setInitializationName
+ void setSerialNumber(const std::string& serialNumber);
+
+ /// Gets the serial number used to register this device with the hardware library.
+ /// \param [out] serialNumber The serial number, if set.
+ /// \return true if the serialNumber has been set.
+ bool getSerialNumber(std::string* serialNumber) const;
+
+ /// Sets the name used to register this device with the hardware library.
+ /// To use an initialization name, the 'devices.yaml' file must be in the working directory,
+ /// (see example at Data/devices.yaml) with an entry of the form "initializationName: serialNumber".
+ /// An empty string indicates the first available device.
+ /// \param initializationName The initialization name.
+ /// \sa setSerialNumber
+ void setInitializationName(const std::string& initializationName);
+
+ /// Gets the name used to register this device with the hardware library.
+ /// \param [out] initializationName The initialization name, if set.
+ /// \return true if the initializationName has been set
+ bool getInitializationName(std::string* initializationName) const;
+
+ bool initialize() override;
+
+ bool isInitialized() const 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.
+ /// \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;
+
+ /// Sets whether or not this is supposed to be a 7Dof device.
+ /// If the hardware is not actually 7Dof, the resulting device will never home.
+ /// If the hardware is 7Dof, and this is not called, the resulting device will act like a typical 3Dof.
+ /// \param val true for a 7Dof device.
+ void set7DofDevice(bool val);
+ /// 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;
+
+ /// Set the maximum force that can be sent to the device. Higher force values will be scaled to this magnitude.
+ /// Generally Falcons are robust enough that commanding excessive forces will not cause a problem,
+ /// but in-development systems may overheat if over-driven.
+ /// \param force The maximum force magnitude (in Newtons) that will be sent to the hardware.
+ void setMaxForce(double force);
+
+ /// \return The maximum force (in Newtons) that can be sent to the device.
+ double getMaxForce() const;
+
+ /// Set a constant force that gets added to all forces sent to the hardware.
+ /// \param antigrav The anti-gravity force in Newtons.
+ void setAntigrav(Math::Vector3d antigrav);
+
+ /// \return The constant force that gets added to all forces sent to the hardware (in Newtons).
+ Math::Vector3d getAntigrav() const;
+
+protected:
+ /// True if the device is 7Dof, false if the device is 3Dof.
+ bool m_7DofDevice;
+
+ /// The maximum force magnitude (in Newtons) that should be sent to the hardware.
+ double m_maxForce;
+
+ /// The anti-gravity force in Newtons.
+ Math::Vector3d m_antigrav;
+
+private:
+ friend class NovintScaffold;
+
+ bool finalize() override;
+
+ ///@{
+ /// Used for serializing optional properties
+ const DataStructures::OptionalValue<std::string>& getOptionalInitializationName() const;
+ void setOptionalInitializationName(const DataStructures::OptionalValue<std::string>& name);
+ const DataStructures::OptionalValue<std::string>& getOptionalSerialNumber() const;
+ void setOptionalSerialNumber(const DataStructures::OptionalValue<std::string>& serial);
+ ///@}
+
+ /// The scaffold handles all the communication with the SDK.
+ std::shared_ptr<NovintScaffold> m_scaffold;
+
+ /// The name passed to the SDK to specify which hardware device should be used.
+ SurgSim::DataStructures::OptionalValue<std::string> m_initializationName;
+
+ /// The serial number passed to the SDK to specify which hardware device should be used.
+ SurgSim::DataStructures::OptionalValue<std::string> m_serialNumber;
+
+ /// 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 Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_NOVINT_NOVINTDEVICE_H
diff --git a/SurgSim/Devices/Novint/NovintScaffold.cpp b/SurgSim/Devices/Novint/NovintScaffold.cpp
index 352e700..4f5ee53 100644
--- a/SurgSim/Devices/Novint/NovintScaffold.cpp
+++ b/SurgSim/Devices/Novint/NovintScaffold.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,126 +16,165 @@
#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 <vector>
+#include <yaml-cpp/yaml.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/ApplicationData.h"
#include "SurgSim/Framework/Clock.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/SharedInstance.h"
+#include "SurgSim/Framework/Timer.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
+namespace
{
-class NovintScaffold::Handle
+/// Convert a HDLError to text.
+/// Error code and descriptions are from Novint's hdlErrors.h file.
+std::string convertErrorCodeToString(HDLError errorCode)
{
-public:
- Handle() :
- m_deviceHandle(HDL_INVALID_HANDLE),
- m_scaffold(NovintScaffold::getOrCreateSharedInstance())
+ 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>";
}
+}
- Handle(const std::string& deviceName, const std::string& initializationName) :
- m_deviceHandle(HDL_INVALID_HANDLE),
- m_scaffold(NovintScaffold::getOrCreateSharedInstance())
+/// Check for HDAL 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 isFatalError(const std::string& message)
+{
+ HDLError errorCode = hdlGetError();
+ if (errorCode == HDL_NO_ERROR)
{
- create(deviceName, initializationName);
+ return false;
}
- ~Handle()
- {
- SURGSIM_ASSERT(! isValid()) << "Expected destroy() to be called before Handle object destruction.";
- }
+ // 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 isFatal = isFatalError(message);
- bool isValid() const
- {
- return (m_deviceHandle != HDL_INVALID_HANDLE);
- }
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getLogger("Devices/Novint")) << message << std::endl <<
+ " Error text from HDAL: '" << convertErrorCodeToString(errorCode) << "'" << std::endl <<
+ " Error code: 0x" << std::hex << std::setw(4) << std::setfill('0') << errorCode << std::endl;
- bool create(const std::string& deviceName, const std::string& initializationName)
- {
- SURGSIM_ASSERT(! isValid());
+ return (isFatal || (errorCode != HDL_ERROR_STACK_OVERFLOW));
+}
+};
- HDLDeviceHandle deviceHandle = HDL_INVALID_HANDLE;
- std::string hdalName = initializationName;
- const char* hdalNameToPassSdk = hdalName.c_str();
- if (hdalName.length() == 0)
+namespace SurgSim
+{
+namespace Devices
+{
+
+class NovintScaffold::Handle
+{
+public:
+ explicit Handle(const std::string& info, bool initBySerialNumber = true) : m_deviceHandle(HDL_INVALID_HANDLE)
+ {
+ if (initBySerialNumber)
+ {
+ m_deviceHandle = hdlInitDeviceBySerialNumber(info.c_str());
+ }
+ else
{
- hdalName = "Default Falcon";
- hdalNameToPassSdk = nullptr; // This is how the HDAL API initializes default Falcon.
+ m_deviceHandle = hdlInitNamedDevice(info.c_str());
}
- deviceHandle = hdlInitNamedDevice(hdalNameToPassSdk);
- if (m_scaffold->checkForFatalError("Failed to initialize"))
+ if (isFatalError("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;
+ SURGSIM_LOG_INFO(Framework::Logger::getLogger("Devices/Novint")) <<
+ (initBySerialNumber ? "HDAL serial number: '" : "device name: '") << info << "'";
}
- else if (deviceHandle == HDL_INVALID_HANDLE)
+ else if (!isValid())
{
- 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;
+ if (initBySerialNumber)
+ {
+ SURGSIM_LOG_SEVERE(Framework::Logger::getLogger("Devices/Novint")) <<
+ "No error during initializing device with serial number: '" << info <<
+ "', but an invalid handle returned. Is that Novint device plugged in?";
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(Framework::Logger::getLogger("Devices/Novint")) <<
+ "No error during initializing device named: '" << info <<
+ "', but an invalid handle returned. Is that Novint device plugged in? " <<
+ "Did the HDAL find hdal.ini? Is the initialization name in the hdal.ini?";
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_DEBUG(Framework::Logger::getLogger("Devices/Novint")) <<
+ "Handle " << m_deviceHandle << " created for device: '" << info << "'.";
}
-
- m_deviceHandle = deviceHandle;
- return true;
}
- bool destroy()
+ ~Handle()
{
- SURGSIM_ASSERT(isValid());
-
- HDLDeviceHandle deviceHandle = m_deviceHandle;
- if (deviceHandle == HDL_INVALID_HANDLE)
+ if (isValid())
{
- return false;
+ isFatalError("Error prior to calling hdlUninitDevice");
+ SURGSIM_LOG_DEBUG(Framework::Logger::getLogger("Devices/Novint")) <<
+ "Handle " << m_deviceHandle << " destructing, calling hdlUninitDevice.";
+ hdlUninitDevice(m_deviceHandle);
+ m_deviceHandle = HDL_INVALID_HANDLE;
+ isFatalError("Error calling hdlUninitDevice");
+ SURGSIM_LOG_DEBUG(Framework::Logger::getLogger("Devices/Novint")) <<
+ "Handle " << m_deviceHandle << " destructed, hdlUninitDevice called.";
}
- m_deviceHandle = HDL_INVALID_HANDLE;
+ }
- hdlUninitDevice(deviceHandle);
- m_scaffold->checkForFatalError("Couldn't disable device");
- return true;
+ bool isValid() const
+ {
+ return (m_deviceHandle != HDL_INVALID_HANDLE);
}
HDLDeviceHandle get() const
{
- SURGSIM_ASSERT(isValid());
+ SURGSIM_ASSERT(isValid()) << "HDL device handle is not valid.";
return m_deviceHandle;
}
@@ -146,59 +185,50 @@ private:
/// 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())
+ explicit Callback(NovintScaffold* scaffold) : m_callbackHandle(HDL_INVALID_HANDLE)
{
- create();
- }
-
- ~Callback()
- {
- if (m_haveCallback)
+ SURGSIM_ASSERT(scaffold != nullptr) << "Callback::create needs non-nullptr scaffold.";
+ m_callbackHandle = hdlCreateServoOp(run, scaffold, false);
+ if (!isFatalError("Failed to create servoOp callback"))
{
- destroy();
+ if (!isValid())
+ {
+ SURGSIM_LOG_SEVERE(Framework::Logger::getLogger("Devices/Novint")) <<
+ "Servo operation created, but invalid servo operation entry handle returned." << std::endl <<
+ " Error details: unknown (HDAL returned an invalid servo operation entry handle)";
+ }
+ else
+ {
+ SURGSIM_LOG_DEBUG(Framework::Logger::getLogger("Devices/Novint")) <<
+ "Callback created, hdlCreateServoOp called.";
+ }
}
}
- bool isValid() const
- {
- return m_haveCallback;
- }
-
- bool create()
+ ~Callback()
{
- 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"))
+ if (isValid())
{
- return false;
+ isFatalError("Error prior to stopping haptic callback");
+ SURGSIM_LOG_DEBUG(Framework::Logger::getLogger("Devices/Novint")) <<
+ "Callback destructing, calling hdlDestroyServoOp...";
+ hdlDestroyServoOp(m_callbackHandle);
+ SURGSIM_LOG_IF(!isFatalError("Error stopping haptic callback"),
+ Framework::Logger::getLogger("Devices/Novint"), DEBUG) <<
+ "Callback destructing, hdlDestroyServoOp called.";
+ m_callbackHandle = HDL_INVALID_HANDLE;
}
- m_haveCallback = true;
- return true;
}
- bool destroy()
+ bool isValid() const
{
- SURGSIM_ASSERT(m_haveCallback);
- hdlDestroyServoOp(m_callbackHandle);
- if (m_scaffold->checkForFatalError("Couldn't stop haptic callback"))
- {
- return false;
- }
- m_haveCallback = false;
- return true;
+ return (m_callbackHandle != HDL_INVALID_HANDLE);
}
private:
@@ -211,61 +241,63 @@ private:
/// \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.
+ /// The haptic loop callback handle (or HDL_INVALID_HANDLE if not valid).
HDLOpHandle m_callbackHandle;
- /// True if the callback has been created (and not destroyed).
- bool m_haveCallback;
- /// The scaffold.
- std::shared_ptr<NovintScaffold> m_scaffold;
};
+HDLServoOpExitCode NovintScaffold::Callback::run(void* data)
+{
+ static_cast<NovintScaffold*>(data)->runHapticFrame();
+ return HDL_SERVOOP_CONTINUE;
+}
+
struct NovintScaffold::DeviceData
{
/// Initialize the state.
- DeviceData(const std::string& apiName, NovintCommonDevice* device) :
- initializationName(apiName),
+ explicit DeviceData(NovintDevice* device) :
+ initializationName(""),
+ serialNumber(""),
deviceObject(device),
isPositionHomed(false),
isOrientationHomed(false),
isDeviceHomed(false),
isDeviceHeld(false),
isDevice7Dof(device->is7DofDevice()),
+ maxForce(device->getMaxForce()),
+ antigrav(device->getAntigrav()),
isDeviceRollAxisReversed(false),
eulerAngleOffsetRoll(0.0),
eulerAngleOffsetYaw(0.0),
eulerAngleOffsetPitch(0.0),
+ toolDof(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;
+ std::string initializationName;
+ /// The HDAL device serial number.
+ std::string serialNumber;
/// The corresponding device object.
- NovintCommonDevice* const deviceObject;
+ NovintDevice* const deviceObject;
/// The device handle wrapper.
- NovintScaffold::Handle deviceHandle;
- /// Time of the initialization of the handle.
- Clock::time_point initializationTime;
+ std::shared_ptr<NovintScaffold::Handle> deviceHandle;
/// The joint angles for the device orientation.
Vector3d jointAngles;
@@ -281,6 +313,10 @@ struct NovintScaffold::DeviceData
bool isDeviceHeld;
/// True if this is a 7DoF device.
bool isDevice7Dof;
+ /// The maximum force magnitude (in Newtons) to send to the device.
+ double maxForce;
+ /// The constant force added to all forces sent to the device (in Newtons).
+ Vector3d antigrav;
/// True if the roll axis of a 7DoF device has reverse polarity because the device is left-handed.
bool isDeviceRollAxisReversed;
@@ -290,15 +326,13 @@ struct NovintScaffold::DeviceData
double eulerAngleOffsetYaw;
/// The offset added to the pitch Euler angle.
double eulerAngleOffsetPitch;
+ /// The tool's degree-of-freedom, e.g., the handle's open/close angle.
+ double toolDof;
/// 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;
@@ -325,7 +359,7 @@ struct NovintScaffold::StateData
{
public:
/// Initialize the state.
- StateData() : isApiInitialized(false)
+ StateData() : isApiInitialized(false), logger(Framework::Logger::getLogger("Devices/Novint"))
{
}
@@ -333,35 +367,39 @@ public:
bool isApiInitialized;
/// Wrapper for the haptic loop callback handle.
- std::unique_ptr<NovintScaffold::Callback> callback;
+ std::unique_ptr<Callback> callback;
+
+ /// The registered devices.
+ std::list<std::unique_ptr<DeviceData>> registeredDevices;
+
+ /// The map from serial number to Handle for all devices that were available when the SDK was initialized.
+ std::unordered_map<std::string, std::shared_ptr<Handle>> serialToHandle;
- /// The list of known devices.
- std::list<std::unique_ptr<NovintScaffold::DeviceData>> activeDeviceList;
+ /// List of devices that have been unregistered and should have their forces, torques, and gravity compensation
+ /// zeroed in the next update.
+ std::list<std::shared_ptr<Handle>> unregisteredHandles;
- /// The mutex that protects the list of known devices.
+ /// The map from name to serial number for all devices.
+ std::map<std::string, std::string> nameToSerial;
+
+ /// The mutex that protects the list of registered devices.
boost::mutex mutex;
+ /// Time of the initialization of the latest handle.
+ Framework::Clock::time_point initializationTime;
+
+ /// Timer to measure update rate.
+ Framework::Timer timer;
+
+ /// Logger used by the scaffold and all devices.
+ std::shared_ptr<SurgSim::Framework::Logger> logger;
+
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)
{
@@ -372,296 +410,402 @@ static inline T clampToRange(T value, T rangeMin, T rangeMax)
return value;
}
-
-
-NovintScaffold::NovintScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) :
- m_logger(logger), m_state(new StateData)
+NovintScaffold::NovintScaffold() : 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)
+ while (hdlGetError() != HDL_NO_ERROR)
{
- errorCode = hdlGetError();
}
}
+ m_state->timer.setMaxNumberOfFrames(5000);
+
+ // The canonical HDAL approach (Programmer's Guide, section 4.7 Multiple devices) is:
+ // 1) hdlInitXXXX on all devices that will be used by this application,
+ // 2) hdlStart (must be after all hdlInitX and before hdlCreateServoOp), then
+ // 3) hdlCreateServoOp (starts the callback).
+ // Note: 1. If no device is initialized (i.e. hdlInitXXX returned an invalid handle),
+ // don't call hdlStart() or it will crash.
+ // 2. In order to use Novint 7Dof device, device MUST BE initialized by name (NOT by serial number).
+ // Novint 3Dof device could be used/initialized either by name or serial number.
+ // 3. If a Novint device is in 'cut-out' state (happened with E3 binary/serial number),
+ // HDAL library will crash.
+
+ // Load the list of Novint devices (devices.yaml) user wants to use.
+ m_state->nameToSerial = getNameMap();
+ if (createAllHandles())
+ {
+ hdlStart();
+ if (!isFatalError("Couldn't start HDAL scheduler"))
+ {
+ SURGSIM_LOG_DEBUG(m_state->logger) << "Scheduler started, hdlStart called.";
- SURGSIM_LOG_DEBUG(m_logger) << "Novint: Shared scaffold created.";
+ std::unique_ptr<Callback> callback(new Callback(this));
+ if (callback->isValid())
+ {
+ m_state->callback = std::move(callback);
+ m_state->isApiInitialized = true;
+ SURGSIM_LOG_DEBUG(m_state->logger) << "Callback scheduled; Scaffold created successfully.";
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_state->logger) << "Failed to create a callback.";
+ hdlStop();
+ isFatalError("Couldn't stop HDAL scheduler");
+ }
+ }
+ }
}
-
NovintScaffold::~NovintScaffold()
{
- if (m_state->callback)
- {
- destroyHapticLoop();
- }
- // The following block controls the duration of the mutex being locked.
+ if (m_state->isApiInitialized)
{
- boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ // The HDAL seems to do bad things (and the CRT complains) if we uninitialize the device too soon.
+ const int MINIMUM_LIFETIME_MILLISECONDS = 500;
+ Framework::Clock::time_point earliestEndTime =
+ m_state->initializationTime + boost::chrono::milliseconds(MINIMUM_LIFETIME_MILLISECONDS);
+ boost::this_thread::sleep_until(earliestEndTime);
- 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();
- }
+ m_state->callback = nullptr;
+ SURGSIM_LOG_DEBUG(m_state->logger) << "Callback reset.\nStopping HDAL scheduler...";
- if (m_state->isApiInitialized)
+ hdlStop();
+ SURGSIM_LOG_IF(!isFatalError("Couldn't stop HDAL scheduler"), m_state->logger, DEBUG) <<
+ "HDAL scheduler stopped, hdlStop called.";
+
+ boost::this_thread::sleep_for(boost::chrono::milliseconds(10));
+ if (!m_state->registeredDevices.empty())
{
- finalizeSdk();
+ SURGSIM_LOG_SEVERE(m_state->logger) << "Destroying scaffold while there are still registered devices!?!";
+ m_state->registeredDevices.clear();
}
+
+ destroyAllHandles();
+ SURGSIM_LOG_DEBUG(m_state->logger) << "Scaffold destroyed.";
}
- SURGSIM_LOG_DEBUG(m_logger) << "Novint: Shared scaffold destroyed.";
}
-
-bool NovintScaffold::registerDevice(NovintCommonDevice* device)
+bool NovintScaffold::registerDevice(NovintDevice* device)
{
boost::lock_guard<boost::mutex> lock(m_state->mutex);
- if (! m_state->isApiInitialized)
+ // Make sure the serial number is unique.
+ std::string serialNumber;
+ if ((device->getSerialNumber(&serialNumber)) && (!serialNumber.empty()))
{
- if (! initializeSdk())
+ auto& sameSerialNumber = std::find_if(m_state->registeredDevices.cbegin(), m_state->registeredDevices.cend(),
+ [&serialNumber](const std::unique_ptr<DeviceData>& info)
+ {
+ return info->serialNumber == serialNumber;
+ });
+ if (sameSerialNumber != m_state->registeredDevices.end())
{
+ SURGSIM_LOG_CRITICAL(m_state->logger) << "Tried to register a device when the same serial number " <<
+ serialNumber <<" is already present!";
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())
+ std::string initializationName;
+ if ((device->getInitializationName(&initializationName)) && (!initializationName.empty()))
{
- SURGSIM_LOG_CRITICAL(m_logger) << "Novint: Tried to register a device when the same initialization" <<
- " (HDAL) name is already present!";
- return false;
+ auto& sameInitializationName = std::find_if(m_state->registeredDevices.cbegin(),
+ m_state->registeredDevices.cend(),
+ [&initializationName](const std::unique_ptr<DeviceData>& info)
+ {
+ return info->initializationName == initializationName;
+ });
+ if (sameInitializationName != m_state->registeredDevices.end())
+ {
+ SURGSIM_LOG_CRITICAL(m_state->logger) <<
+ "Tried to register a device when the same initialization (HDAL) name " << initializationName <<
+ " 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()))
+ std::unique_ptr<DeviceData> info(new DeviceData(device));
+ info->serialNumber = serialNumber;
+ info->initializationName = initializationName;
+
+ if (!initializeDeviceState(info.get()))
{
return false; // message already printed
}
- info->initializationTime = Clock::now();
- m_state->activeDeviceList.emplace_back(std::move(info));
+ m_state->registeredDevices.emplace_back(std::move(info));
+ SURGSIM_LOG_INFO(m_state->logger) << "Device " << device->getName() << " initialized. Maximum force is " <<
+ m_state->registeredDevices.back()->maxForce << " Newtons. Antigrav is (" <<
+ m_state->registeredDevices.back()->antigrav.transpose() << ") Newtons";
- 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)
+bool NovintScaffold::unregisterDevice(const NovintDevice* const device)
{
+ bool result = false;
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())
+ auto& matching = std::find_if(m_state->registeredDevices.begin(), m_state->registeredDevices.end(),
+ [&device](const std::unique_ptr<DeviceData>& info)
+ {
+ return info->deviceObject == device;
+ });
+ if (matching != m_state->registeredDevices.end())
{
savedInfo = std::move(*matching);
- m_state->activeDeviceList.erase(matching);
+ m_state->registeredDevices.erase(matching);
+ m_state->unregisteredHandles.push_back(savedInfo->deviceHandle);
+ SURGSIM_LOG_INFO(m_state->logger) << "Device " << device->getName() << " finalized.";
+ result = true;
// the iterator is now invalid but that's OK
}
- haveOtherDevices = (m_state->activeDeviceList.size() > 0);
}
+ SURGSIM_LOG_IF(!result, m_state->logger, SEVERE) << "Attempted to release a non-registered device.";
+ return result;
+}
- bool status = true;
- if (! savedInfo)
+std::shared_ptr<NovintScaffold::Handle>
+ NovintScaffold::findHandleByInitializationName(const std::string& initializationName)
+{
+ std::shared_ptr<Handle> handle;
+ if (initializationName.empty())
{
- SURGSIM_LOG_WARNING(m_logger) << "Novint: Attempted to release a non-registered device.";
- status = false;
+ // get the first available
+ for (auto& it : m_state->serialToHandle)
+ {
+ auto& possibleHandle = it.second;
+ auto& matching = std::find_if(m_state->registeredDevices.begin(), m_state->registeredDevices.end(),
+ [&possibleHandle](const std::unique_ptr<DeviceData>& info)
+ {
+ return info->deviceHandle == possibleHandle;
+ });
+ if (matching == m_state->registeredDevices.end())
+ {
+ handle = possibleHandle;
+ break;
+ }
+ }
+ if (handle == nullptr)
+ {
+ SURGSIM_ASSERT(m_state->serialToHandle.size() == m_state->registeredDevices.size()) <<
+ "Failed to find an un-registered device when the number of registered devices is not equal to" <<
+ " the number of devices found at startup.";
+ SURGSIM_LOG_SEVERE(m_state->logger) <<
+ "Attempted to register a default device, but no more devices are available." <<
+ " There were " << m_state->serialToHandle.size() << " devices available at program start.";
+ }
}
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)
+ if (m_state->nameToSerial.count(initializationName) > 0)
{
- createHapticLoop();
+ const std::string& serial = m_state->nameToSerial[initializationName];
+ if (m_state->serialToHandle.count(serial) > 0)
+ {
+ handle = m_state->serialToHandle[serial];
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_state->logger) << "Attempted to register a device named '" << initializationName <<
+ "', which should map to serial number " << serial <<
+ ", but no device with that serial number is available. " <<
+ "Is it plugged in? Is the correct hdal.ini found and does it have an entry for this name?";
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_state->logger) << "Attempted to register a device named '" << initializationName
+ << "', but that name does not map to a serial number. The configuration file (devices.yaml) was "
+ << (m_state->nameToSerial.empty() ?
+ "not found, did not contain a YAML node (the map from name to serial number), or the map was empty."
+ : "found, but the device name is not a key in the contained map from name to serial number.");
}
}
- return status;
+ return handle;
}
bool NovintScaffold::initializeDeviceState(DeviceData* info)
{
- SURGSIM_ASSERT(! info->deviceHandle.isValid());
+ SURGSIM_ASSERT(info->deviceHandle == nullptr) << "The raw handle should be nullptr before initialization.";
- if (! info->deviceHandle.create(info->deviceObject->getName(), info->deviceObject->getInitializationName()))
+ if (info->serialNumber.empty())
{
- return false; // message was already printed
+ info->deviceHandle = findHandleByInitializationName(info->initializationName);
}
-
- // Select the handle.
- hdlMakeCurrent(info->deviceHandle.get());
- checkForFatalError("Couldn't enable the handle");
-
- if (info->isDevice7Dof)
+ else
{
- 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)
+ if (m_state->serialToHandle.count(info->serialNumber) > 0)
{
- 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.;
+ info->deviceHandle = m_state->serialToHandle[info->serialNumber];
}
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.;
+ SURGSIM_LOG_SEVERE(m_state->logger) <<
+ "Attempted to register a device by serial number for serial number " <<
+ info->serialNumber << ", but no device with that serial number is available.";
}
}
+ m_state->unregisteredHandles.remove(info->deviceHandle);
- return true;
+ return info->deviceHandle != nullptr;
}
-
-bool NovintScaffold::finalizeDeviceState(DeviceData* info)
+bool NovintScaffold::updateDeviceOutput(DeviceData* info, bool pulledOutput)
{
- bool status = false;
- if (info->deviceHandle.isValid())
+ hdlMakeCurrent(info->deviceHandle->get()); // This device is now "current", and all hdlXxx calls apply to it.
+ bool fatalError = isFatalError("hdlMakeCurrent()");
+
+ info->force.setZero();
+ info->torque.setZero();
+ if (info->isDeviceHomed && pulledOutput)
{
- status = info->deviceHandle.destroy();
+ bool desiredGravityCompensation = false;
+ if (info->deviceObject->getOutputData().booleans().get("gravityCompensation", &desiredGravityCompensation))
+ {
+ setGravityCompensation(info, desiredGravityCompensation);
+ }
+ calculateForceAndTorque(info);
}
- return status;
-}
+ // Set the force command (in newtons).
+ const double norm = info->force.norm();
+ if (norm > info->maxForce)
+ {
+ info->force = info->force.normalized() * info->maxForce;
+ }
+ hdlGripSetAttributev(HDL_GRIP_FORCE, 0, info->force.data()); // 2nd arg is index; output force is always "vector #0"
+ fatalError = fatalError || isFatalError("hdlGripSetAttributev(HDL_GRIP_FORCE)");
-bool NovintScaffold::updateDevice(DeviceData* info)
-{
- const SurgSim::DataStructures::DataGroup& outputData = info->deviceObject->getOutputData();
+ if (info->isDevice7Dof)
+ {
+ // 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 = fatalError || isFatalError("hdlGripSetAttributesd(HDL_GRIP_TORQUE)");
+ }
+ return !fatalError;
+}
+bool NovintScaffold::updateDeviceInput(DeviceData* info)
+{
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)");
+ hdlMakeCurrent(info->deviceHandle->get()); // This device is now "current", and all hdlXxx calls apply to it.
+ bool fatalError = isFatalError("hdlMakeCurrent()");
info->buttonStates.fill(false);
hdlGripGetAttributesb(HDL_GRIP_BUTTON, static_cast<int>(info->buttonStates.size()), info->buttonStates.data());
- fatalError = checkForFatalError(fatalError, "hdlGripGetAttributesb(HDL_GRIP_BUTTON)");
+ fatalError = fatalError || isFatalError("hdlGripGetAttributesb(HDL_GRIP_BUTTON)");
- // Get the additional 7DoF data if available.
- if (info->isDevice7Dof)
+ checkDeviceHoming(info);
+ if (info->isPositionHomed)
{
- // 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;
+ hdlGripGetAttributev(HDL_GRIP_POSITION, 0, info->scaledPose.translation().data());
+ fatalError = fatalError || isFatalError("hdlGripGetAttributev(HDL_GRIP_POSITION)");
+ info->scaledPose.translation() *= info->positionScale;
}
- checkDeviceHoming(info);
-
- info->force.setZero();
- info->torque.setZero();
- if (info->isDeviceHomed)
+ // Get the additional 7DoF data if available.
+ if (info->isDevice7Dof)
{
- bool desiredGravityCompensation = false;
- if (outputData.booleans().get("gravityCompensation", &desiredGravityCompensation))
+ if (info->isPositionHomed)
{
- setGravityCompensation(info, desiredGravityCompensation);
+ // The 7dofs should have +x towards the right Falcon (i.e., to the user's right),
+ // +y up, and +z towards the user (i.e., right-hand frame based on x & y), so we transform from
+ // the per-device HDAL workspace.
+ static const Matrix33d leftTranslationTransform = makeRotationMatrix(M_PI_2, Vector3d::UnitY().eval());
+ static const Matrix33d rightTranslationTransform = makeRotationMatrix(-M_PI_2, Vector3d::UnitY().eval());
+
+ if (info->isDeviceRollAxisReversed)
+ {
+ info->scaledPose.translation() = leftTranslationTransform * info->scaledPose.translation();
+ }
+ else
+ {
+ info->scaledPose.translation() = rightTranslationTransform * info->scaledPose.translation();
+ }
}
- calculateForceAndTorque(info);
- }
+ int gripStatus[2] = { 0, 0 };
+ // OSG2 grips report their "handedness" in the LSB of the second raw status byte
+ // We re-check this state each update because sometimes the first few callbacks have incorrect states.
+ hdlGripGetAttributes(HDL_GRIP_STATUS, 2, gripStatus);
+ if (isFatalError("Cannot get grip status"))
+ {
+ // HDL reported an error. An error message was already logged.
+ return false;
+ }
+ info->eulerAngleOffsetRoll = 0.0;
+ bool leftHanded = (gripStatus[1] & 0x01) != 0;
+ if (leftHanded)
+ {
+ info->isDeviceRollAxisReversed = true;
+ info->eulerAngleOffsetYaw = -0.5;
+ info->eulerAngleOffsetPitch = -0.9;
+ }
+ else
+ {
+ info->isDeviceRollAxisReversed = false;
+ info->eulerAngleOffsetYaw = 0.3;
+ info->eulerAngleOffsetPitch = -0.7;
+ }
- // 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)");
+ if (info->isOrientationHomed)
+ {
+ // 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 = fatalError || isFatalError("hdlGripGetAttributesd(HDL_GRIP_ANGLE)");
+
+ // The zero values are NOT the home orientation, and we are flipping some axes to map to our desired
+ // device workspace (from the underlying per-device HDAL workspace).
+ if (info->isDeviceRollAxisReversed)
+ {
+ info->jointAngles[0] = -angles[0] + info->eulerAngleOffsetRoll;
+ info->jointAngles[1] = angles[1] + info->eulerAngleOffsetYaw;
+ info->jointAngles[2] = angles[2] + info->eulerAngleOffsetPitch;
+ }
+ else
+ {
+ info->jointAngles[0] = angles[0] + info->eulerAngleOffsetRoll;
+ info->jointAngles[1] = angles[1] + info->eulerAngleOffsetYaw;
+ info->jointAngles[2] = -angles[2] + info->eulerAngleOffsetPitch;
+ }
- // 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)");
+ /* HW-Nov-12-2015
+ Testing on Nov 10, 2015 shows that
+ hdlGripGetAttributesd(HDL_GRIP_ANGLE, 3, &info->toolDof);
+ gives the correct reading for the 7th Dof value.
+ However, it didn't give correct value in follow up tests.
+ 'angles[3]' has the value for the 7th Dof now (but it didn't on Nov 10's test).
+ hdlGripGetAttributesd(HDL_GRIP_ANGLE, 3, &info->toolDof); should be kept in mind.
+ */
+ info->toolDof = angles[3]; // Get the reading for 7th Dof, the open/close angle of the tool.
+
+ // For the Falcon 7DoF grip, the axes are perpendicular and the joint angles are Euler angles.
+ // The home position (identity orientation) is with the tool parallel to the ground with the handle pointed
+ // at the user (and some roll angle).
+ Matrix33d rotationX = makeRotationMatrix(info->jointAngles[2] * info->orientationScale,
+ Vector3d(Vector3d::UnitX()));
+ Matrix33d rotationY = makeRotationMatrix(info->jointAngles[1] * info->orientationScale,
+ Vector3d(Vector3d::UnitY()));
+ Matrix33d rotationZ = makeRotationMatrix(info->jointAngles[0] * info->orientationScale,
+ Vector3d(Vector3d::UnitZ()));
+ info->scaledPose.linear() = rotationY * rotationX * rotationZ;
+ }
+ }
setInputData(info);
-
return !fatalError;
}
@@ -687,114 +831,122 @@ void NovintScaffold::checkDeviceHoming(DeviceData* info)
info->isDeviceHeld = true; // ...I guess
}
- if (info->isPositionHomed && info->isOrientationHomed && ! info->isDeviceHomed)
+ 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);
+ static const Vector3d forwardDirection = Vector3d::UnitZ();
+ double forwardMetric = forwardDirection.dot(info->scaledPose.linear() * forwardDirection);
+ SURGSIM_LOG_DEBUG(m_state->logger) << "NovintDevice " << info->deviceObject->getName()
+ << " is position- and orientation-homed, but needs to be near the home orientation. forwardMetric "
+ << forwardMetric << " vs. threshold " << info->forwardPointingPoseThreshold;
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;
+ const DataGroup& outputData = info->deviceObject->getOutputData();
+ outputData.vectors().get(DataStructures::Names::FORCE, &(info->force));
+ info->force += info->antigrav;
// 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);
+ Math::Vector6d deltaPosition;
+ DataGroup::DynamicMatrixType springJacobian;
+ bool havespringJacobian = outputData.matrices().get(DataStructures::Names::SPRING_JACOBIAN, &springJacobian);
if (havespringJacobian)
{
RigidTransform3d poseForNominal = info->scaledPose;
- outputData.poses().get(SurgSim::DataStructures::Names::INPUT_POSE, &poseForNominal);
+ outputData.poses().get(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);
+ // Unfortunately, the current version of the OSG Falcon does not provide the roll angle via
+ // hdlGripGetAttributesd(HDL_GRIP_ANGLE, ...) so here we do not know the actual orientation of a 7Dof robot
+ // (and a 3Dof robot will not change orientation).
+ // Math::computeRotationVector(info->scaledPose, poseForNominal, &rotationVector);
+ Math::setSubVector(info->scaledPose.translation() - poseForNominal.translation(), 0, 3, &deltaPosition);
+ Math::setSubVector(rotationVector, 1, 3, &deltaPosition);
info->force += springJacobian.block<3,6>(0, 0) * deltaPosition;
}
+ /* Since the HDAL does not provide a velocity, NovintScaffold cannot use the damper Jacobian.
+ // TODO(ryanbeasley): consider adding a velocity filter setting to NovintDevice/DeviceData.
// 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);
+ Math::Vector6d deltaVelocity;
+ DataGroup::DynamicMatrixType damperJacobian;
+ bool havedamperJacobian = outputData.matrices().get(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);
+ outputData.vectors().get(DataStructures::Names::INPUT_LINEAR_VELOCITY, &linearVelocityForNominal);
Vector3d angularVelocityForNominal = angularVelocity;
- outputData.vectors().get(SurgSim::DataStructures::Names::INPUT_ANGULAR_VELOCITY, &angularVelocityForNominal);
+ outputData.vectors().get(DataStructures::Names::INPUT_ANGULAR_VELOCITY, &angularVelocityForNominal);
- SurgSim::Math::setSubVector(linearVelocity - linearVelocityForNominal, 0, 3, &deltaVelocity);
- SurgSim::Math::setSubVector(angularVelocity - angularVelocityForNominal, 1, 3, &deltaVelocity);
+ Math::setSubVector(linearVelocity - linearVelocityForNominal, 0, 3, &deltaVelocity);
+ 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;
+ // Transform the forces from our device-space into the individual Falcon spaces
+ // (+z out of Falcon front, +y up, +x to right of Falcon when looking at it from the front).
+ static const Matrix33d leftForceTransform = makeRotationMatrix(-M_PI_2, Vector3d::UnitY().eval());
+ static const Matrix33d rightForceTransform = makeRotationMatrix(M_PI_2, Vector3d::UnitY().eval());
+
+ if (info->isDeviceRollAxisReversed)
+ {
+ info->force = leftForceTransform * info->force;
+ }
+ else
+ {
+ info->force = rightForceTransform * info->force;
+ }
+ Vector3d torque = Vector3d::Zero();
+ outputData.vectors().get(DataStructures::Names::TORQUE, &torque);
if (havespringJacobian)
{
torque += springJacobian.block<3,6>(3, 0) * deltaPosition;
}
- if (havedamperJacobian)
+ // Since the HDAL does not provide a velocity, we cannot use the damper Jacobian.
+ // TODO(ryanbeasley): consider adding a velocity filter setting to NovintDevice/DeviceData.
+ //if (havedamperJacobian)
+ //{
+ // torque += damperJacobian.block<3,6>(3, 0) * deltaVelocity;
+ //}
+
+ static const Matrix33d rightTorqueTransform = makeRotationMatrix(M_PI, Vector3d::UnitY().eval());
+ if (!info->isDeviceRollAxisReversed)
{
- torque += damperJacobian.block<3,6>(3, 0) * deltaVelocity;
+ torque.head<3>() = rightTorqueTransform * torque.head<3>();
}
// 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 rotationX = makeRotationMatrix(info->jointAngles[2], 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!
+ Matrix33d rotationZ = makeRotationMatrix(info->jointAngles[0], Vector3d(Vector3d::UnitZ()));
+ // NB: the order of rotations is (rotY * rotX * rotZ), 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());
+ Vector3d jointAxisX = rotationY * Vector3d::UnitX();
+ Vector3d jointAxisZ = rotationY * (rotationX * Vector3d::UnitZ());
// 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;
@@ -803,13 +955,13 @@ void NovintScaffold::calculateForceAndTorque(DeviceData* info)
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
+ // Note that the Y and X 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();
+ Vector3d fakeAxisZ = jointAxisY.cross(jointAxisX).normalized();
Matrix33d fakeBasisMatrix;
- fakeBasisMatrix.col(0) = fakeAxisX;
+ fakeBasisMatrix.col(0) = jointAxisX;
fakeBasisMatrix.col(1) = jointAxisY;
- fakeBasisMatrix.col(2) = jointAxisZ;
+ fakeBasisMatrix.col(2) = fakeAxisZ;
const double mediumBasisDeterminantThreshold = 0.6;
const double smallBasisDeterminantThreshold = 0.4;
@@ -826,7 +978,7 @@ void NovintScaffold::calculateForceAndTorque(DeviceData* info)
// 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));
+ (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
@@ -834,16 +986,16 @@ void NovintScaffold::calculateForceAndTorque(DeviceData* info)
Matrix33d fakeDecompositionMatrix = fakeBasisMatrix.inverse();
fakeDecompositionMatrix.row(0) = Vector3d::Zero();
- decompositionMatrix = basisMatrix.inverse() * ratio + fakeDecompositionMatrix * (1.-ratio);
+ decompositionMatrix = basisMatrix.inverse() * ratio + fakeDecompositionMatrix * (1.0 - 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
+ // The "fake" basis matrix replaces the Z axis with a fake (so it's always invertible), but the output Z
// 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,
+ decompositionMatrix.row(2) = Vector3d::Zero();
+ // Moreover, near the degenerate position the Z axis free-spins but is aligned with Y,
// so we want to reduce Y torques as well.
//double ratio = (basisDeterminant / smallBasisDeterminantThreshold);
double ratio = 0;
@@ -855,166 +1007,207 @@ void NovintScaffold::calculateForceAndTorque(DeviceData* info)
// 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);
+ // roll axis: torque = 41.97 mNm when command = 2000 (but flipped in left grip!)
+ const double rollTorqueScale = axisTorqueMax / 41.97e-3;
+ // yaw axis: torque = 95.92 mNm when command = 2000
+ const double yawTorqueScale = axisTorqueMax / 95.92e-3;
+ // pitch axis: torque = 95.92 mNm when command = 2000
+ const double pitchTorqueScale = axisTorqueMax / 95.92e-3;
+
+ info->torque[0] = 0; // Roll torque currently disabled because the roll sensor is too jittery.
info->torque[1] = clampToRange(yawTorqueScale * info->torqueScale.y() * axisTorqueVector.y(),
- axisTorqueMin, axisTorqueMax);
- info->torque[2] = clampToRange(pitchTorqueScale * info->torqueScale.z() * axisTorqueVector.z(),
- axisTorqueMin, axisTorqueMax);
+ axisTorqueMin, axisTorqueMax);
+ info->torque[2] = clampToRange(pitchTorqueScale * info->torqueScale.x() * axisTorqueVector.x(),
+ axisTorqueMin, axisTorqueMax);
info->torque[3] = 0;
- if (info->isDeviceRollAxisReversed) // commence swearing.
+ if (info->isDeviceRollAxisReversed)
{
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);
+ DataGroup& inputData = info->deviceObject->getInputData();
+ inputData.poses().set(DataStructures::Names::POSE, info->scaledPose);
+ inputData.scalars().set(DataStructures::Names::TOOLDOF, info->toolDof);
+ inputData.booleans().set(DataStructures::Names::BUTTON_1, info->buttonStates[0]);
+ inputData.booleans().set(DataStructures::Names::BUTTON_2, info->buttonStates[1]);
+ inputData.booleans().set(DataStructures::Names::BUTTON_3, info->buttonStates[2]);
+ inputData.booleans().set(DataStructures::Names::BUTTON_4, info->buttonStates[3]);
+ inputData.booleans().set(DataStructures::Names::IS_HOMED, info->isDeviceHomed);
+ inputData.booleans().set(DataStructures::Names::IS_POSITION_HOMED, info->isPositionHomed);
+ inputData.booleans().set(DataStructures::Names::IS_ORIENTATION_HOMED, info->isOrientationHomed);
}
-
-bool NovintScaffold::initializeSdk()
+bool NovintScaffold::createAllHandles()
{
- SURGSIM_ASSERT(! m_state->isApiInitialized);
+ // The Scaffold does not know which devices will be initialized, so we will try to initialize all the
+ // devices (by name) listed in device.yaml first.
+ // Then use hdlCatalogDevices to get the serial numbers for other connected devices not listed in device.yaml,
+ // and initialize them (by serial number).
+ // When registerDevice is called the name can be matched to a Handle created from a serial number.
+ // Note: initializaiton name is case insensitive.
+ for (const auto& item : m_state->nameToSerial)
+ {
+ std::string deviceName = item.first;
+ std::transform(deviceName.begin(), deviceName.end(), deviceName.begin(), tolower);
+ if (deviceName == "default")
+ {
+ SURGSIM_LOG_INFO(m_state->logger) << "'" << item.first <<
+ "' is system reserved. No Novint device should be named 'Default' (case insensitive).";
+ continue;
+ }
- // nothing to do!
+ // initialize by name
+ auto handle = std::make_shared<NovintScaffold::Handle>(item.first, false);
+ SURGSIM_LOG_IF(!storeHandleIfValid(handle, item.second), m_state->logger, WARNING) <<
+ "Failed to initialize Novint device: " << item.first << " (Serial #: " << item.second << ")";
+ }
- m_state->isApiInitialized = true;
- return true;
-}
+ char serials[HDL_MAX_DEVICES * HDL_SERNUM_BUFFSIZE];
+ const int numDevices = hdlCatalogDevices(HDL_NOT_OPEN_BY_ANY_APP, &(serials[0]), NULL);
+ isFatalError("Failed to get catalog of devices.");
+ for (int i = 0; i < numDevices; ++i)
+ {
+ const std::string serial(&(serials[i * HDL_SERNUM_BUFFSIZE]), HDL_SERNUM_BUFFSIZE - 1);
+ SURGSIM_LOG_DEBUG(m_state->logger) << "Found serial number " << serial << ".";
-bool NovintScaffold::finalizeSdk()
-{
- SURGSIM_ASSERT(m_state->isApiInitialized);
+ // Device with serial number 'serial' already initialized.
+ if (m_state->serialToHandle.find(serial) != m_state->serialToHandle.end())
+ {
+ SURGSIM_LOG_DEBUG(m_state->logger) << "Device with serial number " << serial << " already created.";
+ continue;
+ }
- // nothing to do!
+ auto handle = std::make_shared<NovintScaffold::Handle>(serial);
+ SURGSIM_LOG_IF(!storeHandleIfValid(handle, serial), m_state->logger, WARNING) <<
+ "Failed to initialize Falcon with serial " << serial << ".";
+ }
+ m_state->initializationTime = Framework::Clock::now();
+ SURGSIM_LOG_DEBUG(m_state->logger) << "All device handles created.";
- m_state->isApiInitialized = false;
- return true;
+ return !m_state->serialToHandle.empty();
}
-
-bool NovintScaffold::runHapticFrame()
+void NovintScaffold::destroyAllHandles()
{
- boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ SURGSIM_LOG_DEBUG(m_state->logger) << "Destroying all Handles...";
+ m_state->registeredDevices.clear();
+ m_state->unregisteredHandles.clear();
+ m_state->serialToHandle.clear();
+ SURGSIM_LOG_DEBUG(m_state->logger) << "Handles destroyed.";
+}
- for (auto it = m_state->activeDeviceList.begin(); it != m_state->activeDeviceList.end(); ++it)
+bool NovintScaffold::storeHandleIfValid(const std::shared_ptr<Handle>& handle, const std::string& serial)
+{
+ bool result = false;
+ if (handle->isValid())
{
- (*it)->deviceObject->pullOutput();
- if (updateDevice((*it).get()))
- {
- (*it)->deviceObject->pushInput();
- }
+ m_state->serialToHandle[serial] = handle;
+ hdlMakeCurrent(handle->get());
+ isFatalError("Failed to make device current.");
+ result = true;
}
-
- return true;
+ return result;
}
-
-bool NovintScaffold::createHapticLoop()
+std::map<std::string, std::string> NovintScaffold::getNameMap()
{
- SURGSIM_ASSERT(! m_state->callback);
-
- if (! startScheduler())
+ std::map<std::string, std::string> map;
+ std::vector<std::string> paths;
+ paths.push_back(".");
+ Framework::ApplicationData applicationData(paths);
+ std::string filePath;
+ if (applicationData.tryFindFile("devices.yaml", &filePath))
{
- return false;
+ SURGSIM_LOG_DEBUG(m_state->logger) << "Found devices.yaml at '" << filePath << "'.";
+ YAML::Node node = YAML::LoadFile(filePath);
+ map = node["Novint"].as<std::map<std::string, std::string>>();
}
-
- std::unique_ptr<Callback> callback(new Callback);
- if (! callback->isValid())
+ else
{
- stopScheduler();
- return false;
+ SURGSIM_LOG_INFO(m_state->logger) << "Failed to find devices.yaml, cannot map names to serial numbers.";
}
-
- m_state->callback = std::move(callback);
- return true;
+ return map;
}
-
-bool NovintScaffold::destroyHapticLoop()
+void NovintScaffold::runHapticFrame()
{
- 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"))
+ m_state->timer.markFrame();
+ if (m_state->timer.getCurrentNumberOfFrames() == m_state->timer.getMaxNumberOfFrames())
{
- return false;
+ SURGSIM_LOG_INFO(m_state->logger) << std::setprecision(4)
+ << "Rate: " << m_state->timer.getAverageFrameRate() << "Hz "
+ << "(min individual frame " << 1.0 / m_state->timer.getMaxFramePeriod() << "Hz).";
+ m_state->timer.setMaxNumberOfFrames(static_cast<size_t>(m_state->timer.getAverageFrameRate() * 5.0));
+ m_state->timer.start();
}
- return true;
-}
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ for (auto& it = m_state->registeredDevices.begin(); it != m_state->registeredDevices.end(); ++it)
+ {
+ if (updateDeviceInput((*it).get()))
+ {
+ (*it)->deviceObject->pushInput();
+ }
+ }
+ for (auto& it = m_state->registeredDevices.begin(); it != m_state->registeredDevices.end(); ++it)
+ {
+ updateDeviceOutput(it->get(), (*it)->deviceObject->pullOutput());
+ }
-bool NovintScaffold::stopScheduler()
-{
- hdlStop();
- if (checkForFatalError("Couldn't stop the scheduler"))
+ bool desiredGravityCompensation = false;
+ Vector3d force = Vector3d::Zero();
+ Vector4d torque = Vector4d::Zero();
+ for (auto& handle : m_state->unregisteredHandles)
{
- return false;
+ if (handle->isValid())
+ {
+ hdlMakeCurrent(handle->get());
+
+ hdlGripSetAttributeb(HDL_GRIP_GRAVITY_COMP, 1, &desiredGravityCompensation);
+ isFatalError("Cannot set gravity compensation state on recently unregistered device.");
+
+ hdlGripSetAttributev(HDL_GRIP_FORCE, 0, force.data());
+ isFatalError("hdlGripSetAttributev(HDL_GRIP_FORCE)");
+
+ hdlGripSetAttributesd(HDL_GRIP_TORQUE, 4, torque.data());
+ isFatalError("hdlGripSetAttributesd(HDL_GRIP_TORQUE)");
+ }
}
- return true;
+ m_state->unregisteredHandles.clear();
}
-
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)"))
+ if (isFatalError("Cannot get gravity compensation (#1)"))
{
- return false; // HDAL reported an error; an error message was already logged.
+ return false;
}
bool state2 = false;
hdlGripGetAttributeb(HDL_GRIP_GRAVITY_COMP, 1, &state2);
- if (checkForFatalError("Cannot get gravity compensation (#2)"))
+ if (isFatalError("Cannot get gravity compensation (#2)"))
{
- return false; // HDAL reported an error; an error message was already logged.
+ return false;
}
if (state1 == true && state2 == false)
{
- SURGSIM_LOG_WARNING(m_logger) << "getting gravity compensation state for '" << info->deviceObject->getName() <<
- "' does nothing!";
+ SURGSIM_LOG_WARNING(m_state->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?!?";
+ SURGSIM_LOG_WARNING(m_state->logger) << "getting gravity compensation state for '" <<
+ info->deviceObject->getName() << "' keeps changing?!?";
return false;
}
@@ -1033,37 +1226,30 @@ bool NovintScaffold::enforceGravityCompensation(const NovintScaffold::DeviceData
{
bool state = gravityCompensationState;
hdlGripSetAttributeb(HDL_GRIP_GRAVITY_COMP, 1, &state);
- if (checkForFatalError("Cannot set gravity compensation state"))
+ if (isFatalError("Cannot set gravity compensation state"))
{
- return false; // HDAL reported an error; an error message was already logged.
+ return false;
}
- if (! getGravityCompensation(info, &state))
+ if (!getGravityCompensation(info, &state))
{
- return false; // HDAL reported an error; an error message was already logged.
+ return false;
}
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.";
- }
+ SURGSIM_LOG_INFO(m_state->logger) << "gravity compensation for '" << info->deviceObject->getName() <<
+ "' changed to " << (gravityCompensationState? "enabled." : "disabled." );
}
return true;
}
}
- SURGSIM_LOG_WARNING(m_logger) << "failed to set gravity compensation for '" << info->deviceObject->getName() <<
- "' to " << (gravityCompensationState ? "enabled" : "disabled") << " after " << maxAttempts << " attempts";
+ SURGSIM_LOG_WARNING(m_state->logger) << "failed to set gravity compensation for '" <<
+ info->deviceObject->getName() << "' to " <<
+ (gravityCompensationState ? "enabled" : "disabled") << " after " << maxAttempts << " attempts";
return false;
}
@@ -1081,118 +1267,56 @@ bool NovintScaffold::setGravityCompensation(const NovintScaffold::DeviceData* in
return enforceGravityCompensation(info, gravityCompensationState);
}
-
-static std::string convertErrorCodeToString(HDLError errorCode)
+DataGroup NovintScaffold::buildDeviceInputData()
{
- // 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);
+ DataStructures::DataGroupBuilder builder;
+ builder.addPose(DataStructures::Names::POSE);
+ builder.addScalar(DataStructures::Names::TOOLDOF);
+ builder.addBoolean(DataStructures::Names::BUTTON_1);
+ builder.addBoolean(DataStructures::Names::BUTTON_2);
+ builder.addBoolean(DataStructures::Names::BUTTON_3);
+ builder.addBoolean(DataStructures::Names::BUTTON_4);
+ builder.addBoolean(DataStructures::Names::IS_HOMED);
+ builder.addBoolean(DataStructures::Names::IS_POSITION_HOMED);
+ builder.addBoolean(DataStructures::Names::IS_ORIENTATION_HOMED);
return builder.createData();
}
-void NovintScaffold::setPositionScale(const NovintCommonDevice* device, double scale)
+void NovintScaffold::setPositionScale(const NovintDevice* 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())
+ auto& matching = std::find_if(m_state->registeredDevices.begin(), m_state->registeredDevices.end(),
+ [&device](const std::unique_ptr<DeviceData>& info)
+ {
+ return info->deviceObject == device;
+ });
+ if (matching != m_state->registeredDevices.end())
{
boost::lock_guard<boost::mutex> lock((*matching)->parametersMutex);
(*matching)->positionScale = scale;
}
}
-void NovintScaffold::setOrientationScale(const NovintCommonDevice* device, double scale)
+void NovintScaffold::setOrientationScale(const NovintDevice* 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())
+ auto& matching = std::find_if(m_state->registeredDevices.begin(), m_state->registeredDevices.end(),
+ [&device](const std::unique_ptr<DeviceData>& info)
+ {
+ return info->deviceObject == device;
+ });
+ if (matching != m_state->registeredDevices.end())
{
boost::lock_guard<boost::mutex> lock((*matching)->parametersMutex);
(*matching)->orientationScale = scale;
}
}
-
std::shared_ptr<NovintScaffold> NovintScaffold::getOrCreateSharedInstance()
{
- static SurgSim::Framework::SharedInstance<NovintScaffold> sharedInstance;
+ static 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 Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/Novint/NovintScaffold.h b/SurgSim/Devices/Novint/NovintScaffold.h
index 3929d58..fe482e6 100644
--- a/SurgSim/Devices/Novint/NovintScaffold.h
+++ b/SurgSim/Devices/Novint/NovintScaffold.h
@@ -17,52 +17,40 @@
#define SURGSIM_DEVICES_NOVINT_NOVINTSCAFFOLD_H
#include <memory>
+#include <string>
-#include "SurgSim/Framework/Logger.h"
#include "SurgSim/DataStructures/DataGroup.h"
namespace SurgSim
{
-namespace Device
+namespace Devices
{
-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
+/// \sa SurgSim::Devices::NovintDevice
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);
+ NovintScaffold();
/// 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.
+ /// The scaffold is managed using a SharedInstance 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:
+ NovintScaffold(const NovintScaffold&) /*= delete*/;
+ NovintScaffold& operator=(const NovintScaffold&) /*= delete*/;
/// Internal shared state data type.
struct StateData;
@@ -73,37 +61,42 @@ private:
/// Wrapper for the HDAL device handle.
class Handle;
- friend class NovintCommonDevice;
friend class NovintDevice;
- /// Registers the specified device object.
+ /// Registers the specified device object, and starts the servo loop if necessary.
/// 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);
+ bool registerDevice(NovintDevice* 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);
+ bool unregisterDevice(const NovintDevice* 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.
+ /// Get the Handle associated with an initialization name, if any.
+ /// \param initializationName The initialization name (from the configuration file).
+ /// \return Shared pointer to Handle, or nullptr if not found.
+ std::shared_ptr<NovintScaffold::Handle> findHandleByInitializationName(const std::string& initializationName);
+
+ /// Updates the device information for a single device's input.
+ /// \param info The device data.
/// \return true on success.
- bool finalizeDeviceState(DeviceData* info);
+ bool updateDeviceInput(DeviceData* info);
- /// Updates the device information for a single device.
+ /// Updates the device information for a single device's output. If pullOutput failed, zeros forces & torques.
/// \param info The device data.
+ /// \param pulledOutput true if the most recent pullOutput succeeded.
/// \return true on success.
- bool updateDevice(DeviceData* info);
+ bool updateDeviceOutput(DeviceData* info, bool pulledOutput);
/// 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
@@ -127,35 +120,24 @@ private:
/// \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();
+ /// Gets the map from name to serial number.
+ /// \return The map.
+ std::map<std::string, std::string> getNameMap();
- /// Creates the haptic loop callback.
- /// \return true on success.
- bool createHapticLoop();
+ /// Creates a NovintScaffold::Handle for each device connected when the first registerDevice is called.
+ /// \return True if there are device handles created; false otherwise.
+ bool createAllHandles();
- /// 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();
+ /// Destroys all the initialized handles.
+ void destroyAllHandles();
- /// Starts the HDAL scheduler.
- /// \return true on success.
- bool startScheduler();
+ /// Store the handle if it is valid.
+ /// \return true If handle is valid and stored; false, otherwise.
+ bool storeHandleIfValid(const std::shared_ptr<Handle>& handle, const std::string& serial);
- /// Stops the HDAL scheduler.
- /// \return true on success.
- bool stopScheduler();
+ /// Executes the operations for a single haptic frame.
+ /// Should only be called from the context of a HDAL callback.
+ void runHapticFrame();
/// Gets the gravity compensation flag for the current device.
/// \param info The device data.
@@ -182,46 +164,24 @@ private:
/// \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);
+ void setPositionScale(const NovintDevice* 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);
+ void setOrientationScale(const NovintDevice* 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 Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_NOVINT_NOVINTSCAFFOLD_H
diff --git a/SurgSim/Devices/Novint/UnitTests/CMakeLists.txt b/SurgSim/Devices/Novint/UnitTests/CMakeLists.txt
index 69cbac2..2d4a09e 100644
--- a/SurgSim/Devices/Novint/UnitTests/CMakeLists.txt
+++ b/SurgSim/Devices/Novint/UnitTests/CMakeLists.txt
@@ -19,7 +19,6 @@ include_directories(
)
set(UNIT_TEST_SOURCES
- Novint7DofDeviceTest.cpp
NovintDeviceTest.cpp
NovintScaffoldTest.cpp
)
@@ -30,7 +29,7 @@ set(LIBS
SurgSimFramework
SurgSimDataStructures
${Boost_LIBRARIES}
- ${NOVINT_HDAL_SDK_LIBRARIES}
+ ${NOVINTHDALSDK_LIBRARIES}
)
surgsim_add_unit_tests(NovintDeviceTest)
diff --git a/SurgSim/Devices/Novint/UnitTests/Novint7DofDeviceTest.cpp b/SurgSim/Devices/Novint/UnitTests/Novint7DofDeviceTest.cpp
deleted file mode 100644
index b8328a9..0000000
--- a/SurgSim/Devices/Novint/UnitTests/Novint7DofDeviceTest.cpp
+++ /dev/null
@@ -1,219 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
index 4398104..2a78386 100644
--- a/SurgSim/Devices/Novint/UnitTests/NovintDeviceTest.cpp
+++ b/SurgSim/Devices/Novint/UnitTests/NovintDeviceTest.cpp
@@ -16,77 +16,131 @@
/// \file
/// Tests for the NovintDevice class.
-#include <memory>
-#include <string>
-#include <boost/thread.hpp>
#include <boost/chrono.hpp>
+#include <boost/thread.hpp>
#include <gtest/gtest.h>
-#include "SurgSim/Devices/Novint/NovintDevice.h"
-//#include "SurgSim/Devices/Novint/NovintScaffold.h" // only needed if calling setDefaultLogLevel()
+#include <memory>
+#include <string>
+
#include "SurgSim/DataStructures/DataGroup.h"
-#include "SurgSim/Math/RigidTransform.h"
-#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/DataStructures/OptionalValue.h"
+#include "SurgSim/Devices/Novint/NovintDevice.h"
#include "SurgSim/Framework/Clock.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Testing/MockInputOutput.h"
-using SurgSim::Device::NovintDevice;
-using SurgSim::Device::NovintScaffold;
+using SurgSim::Devices::NovintDevice;
using SurgSim::DataStructures::DataGroup;
using SurgSim::Framework::Clock;
-using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Matrix44d;
+using SurgSim::Math::RigidTransform3d;
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)
+// These initialization names should be set to two of the names used in devices.yaml (or be empty strings to just get
+// the first device). The serial number should be a serial number for one of the attached Falcons (or be an empty
+// string to just get the first device.)
+const std::string NOVINT_TEST_DEVICE_NAME = "FALCON_1";
+const std::string NOVINT_TEST_DEVICE_NAME_2 = "FALCON_2";
+const std::string NOVINT_TEST_DEVICE_SERIAL_NUMBER = "14QAVEFF";
+
+TEST(NovintDeviceTest, CreateAndInitializeDeviceByName)
{
- //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
- std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon");
ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+
+ std::string initializationName = "";
+ EXPECT_FALSE(device->getInitializationName(&initializationName));
+ EXPECT_NO_THROW(device->setInitializationName(NOVINT_TEST_DEVICE_NAME));
+ ASSERT_TRUE(device->getInitializationName(&initializationName));
+ EXPECT_EQ(NOVINT_TEST_DEVICE_NAME, initializationName);
+
+ std::string serialNumber;
+ EXPECT_FALSE(device->getSerialNumber(&serialNumber));
+ EXPECT_ANY_THROW(device->setSerialNumber(""));
+
+ EXPECT_FALSE(device->isInitialized());
+ EXPECT_EQ("TestFalcon", device->getName());
+
+ EXPECT_NEAR(8.9, device->getMaxForce(), 1e-9);
+ EXPECT_NO_THROW(device->setMaxForce(20.0));
+ EXPECT_NEAR(20.0, device->getMaxForce(), 1e-9);
+
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ ASSERT_TRUE(device->isInitialized());
+ ASSERT_ANY_THROW(device->initialize()) << "Initialized the same device twice.";
+ ASSERT_ANY_THROW(device->setInitializationName("OtherName"));
+ ASSERT_ANY_THROW(device->setMaxForce(20.0)) << "Set maximum force after initialization.";
+ EXPECT_EQ("TestFalcon", device->getName());
+
+ const double positionScale = 2.0;
+ device->setPositionScale(positionScale);
+ EXPECT_EQ(positionScale, device->getPositionScale());
+
+ const double orientationScale = 2.0;
+ device->setOrientationScale(orientationScale);
+ EXPECT_EQ(orientationScale, device->getOrientationScale());
}
-TEST(NovintDeviceTest, CreateAndInitializeDevice)
+TEST(NovintDeviceTest, CreateAndInitializeDeviceBySerialNumber)
{
- //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
- std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon");
ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+
+ std::string serialNumber = "";
+ EXPECT_FALSE(device->getSerialNumber(&serialNumber));
+ device->setSerialNumber(NOVINT_TEST_DEVICE_SERIAL_NUMBER);
+ ASSERT_TRUE(device->getSerialNumber(&serialNumber));
+ EXPECT_EQ(NOVINT_TEST_DEVICE_SERIAL_NUMBER, serialNumber);
+
+ std::string initializationName;
+ EXPECT_FALSE(device->getInitializationName(&initializationName));
+ EXPECT_ANY_THROW(device->setInitializationName(""));
+
EXPECT_FALSE(device->isInitialized());
+ EXPECT_EQ("TestFalcon", device->getName());
+
ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
EXPECT_TRUE(device->isInitialized());
+ EXPECT_EQ("TestFalcon", device->getName());
+
+ const double positionScale = 2.0;
+ device->setPositionScale(positionScale);
+ EXPECT_EQ(positionScale, device->getPositionScale());
+
+ const double orientationScale = 2.0;
+ device->setOrientationScale(orientationScale);
+ EXPECT_EQ(orientationScale, device->getOrientationScale());
}
-// 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)
+TEST(NovintDeviceTest, CreateAndInitializeDefaultDevices)
{
- //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
- std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", "");
+ 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());
+ std::string initializationName;
+ EXPECT_FALSE(device->getInitializationName(&initializationName));
+ std::string serialNumber;
+ EXPECT_FALSE(device->getSerialNumber(&serialNumber));
+
+ std::shared_ptr<NovintDevice> device2 = std::make_shared<NovintDevice>("TestFalcon2");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device2->isInitialized());
+ if (!device2->initialize())
+ {
+ std::cerr << "[Warning: second Novint did not come up; is it plugged in?]" << std::endl;
+ }
}
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);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon");
ASSERT_TRUE(device != nullptr) << "Device creation failed.";
ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
if (doSleep)
@@ -99,57 +153,65 @@ static void testCreateDeviceSeveralTimes(bool doSleep)
TEST(NovintDeviceTest, CreateDeviceSeveralTimes)
{
- //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
testCreateDeviceSeveralTimes(true);
}
-TEST(NovintDeviceTest, CreateSeveralDevices)
+TEST(NovintDeviceTest, CreateTwoDevices)
{
- //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
- std::shared_ptr<NovintDevice> device1 = std::make_shared<NovintDevice>("Novint1", NOVINT_TEST_DEVICE_NAME);
+ std::shared_ptr<NovintDevice> device1 = std::make_shared<NovintDevice>("Novint1");
ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ device1->setInitializationName(NOVINT_TEST_DEVICE_NAME);
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);
+ std::shared_ptr<NovintDevice> device2 = std::make_shared<NovintDevice>("Novint2");
ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
- if (! device2->initialize())
+ device2->setInitializationName(NOVINT_TEST_DEVICE_NAME_2);
+ if (!device2->initialize())
{
std::cerr << "[Warning: second Novint did not come up; is it plugged in?]" << std::endl;
}
}
-TEST(NovintDeviceTest, CreateDevicesWithSameName)
+TEST(NovintDeviceTest, CreateDevicesWithSameInitializationName)
{
- //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.";
+ if (NOVINT_TEST_DEVICE_NAME != "")
+ {
+ std::shared_ptr<NovintDevice> device1 = std::make_shared<NovintDevice>("Novint1");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ device1->setInitializationName(NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+
+ std::shared_ptr<NovintDevice> device2 = std::make_shared<NovintDevice>("Novint2");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ device2->setInitializationName(NOVINT_TEST_DEVICE_NAME);
+ ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate initialization name.";
+ }
}
-TEST(NovintDeviceTest, CreateDevicesWithSameInitializationName)
+TEST(NovintDeviceTest, CreateDevicesWithSameSerialNumber)
{
- //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.";
+ if (NOVINT_TEST_DEVICE_SERIAL_NUMBER != "")
+ {
+ std::shared_ptr<NovintDevice> device1 = std::make_shared<NovintDevice>("Novint1");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ device1->setSerialNumber(NOVINT_TEST_DEVICE_SERIAL_NUMBER);
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+
+ std::shared_ptr<NovintDevice> device2 = std::make_shared<NovintDevice>("Novint2");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ device2->setSerialNumber(NOVINT_TEST_DEVICE_SERIAL_NUMBER);
+ ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate serial number.";
+ }
}
TEST(NovintDeviceTest, InputConsumer)
{
- //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
- std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon");
ASSERT_TRUE(device != nullptr) << "Device creation failed.";
- EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ device->setInitializationName(NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ /// Wait a while for the HDL to initialize and start running the callback.
+ boost::this_thread::sleep_for(boost::chrono::milliseconds(1000));
std::shared_ptr<MockInputOutput> consumer = std::make_shared<MockInputOutput>();
EXPECT_EQ(0, consumer->m_numTimesReceivedInput);
@@ -164,7 +226,7 @@ TEST(NovintDeviceTest, InputConsumer)
// 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));
+ boost::this_thread::sleep_for(boost::chrono::milliseconds(1000));
EXPECT_TRUE(device->removeInputConsumer(consumer));
@@ -172,8 +234,8 @@ TEST(NovintDeviceTest, InputConsumer)
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_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));
@@ -181,10 +243,12 @@ TEST(NovintDeviceTest, InputConsumer)
TEST(NovintDeviceTest, OutputProducer)
{
- //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
- std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon");
ASSERT_TRUE(device != nullptr) << "Device creation failed.";
- EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ device->setInitializationName(NOVINT_TEST_DEVICE_NAME);
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
+ /// Wait a while for the HDL to initialize and start running the callback.
+ boost::this_thread::sleep_for(boost::chrono::milliseconds(1000));
std::shared_ptr<MockInputOutput> producer = std::make_shared<MockInputOutput>();
EXPECT_EQ(0, producer->m_numTimesRequestedOutput);
@@ -196,7 +260,7 @@ TEST(NovintDeviceTest, OutputProducer)
// 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));
+ boost::this_thread::sleep_for(boost::chrono::milliseconds(1000));
EXPECT_TRUE(device->removeOutputProducer(producer));
@@ -204,6 +268,41 @@ TEST(NovintDeviceTest, OutputProducer)
EXPECT_FALSE(device->removeOutputProducer(producer));
// Check the number of invocations.
- EXPECT_GE(producer->m_numTimesRequestedOutput, 10*700);
- EXPECT_LE(producer->m_numTimesRequestedOutput, 10*1300);
+ EXPECT_GE(producer->m_numTimesRequestedOutput, 700);
+ EXPECT_LE(producer->m_numTimesRequestedOutput, 1300);
+}
+
+TEST(NovintDeviceTest, FactoryCreation)
+{
+ std::shared_ptr<SurgSim::Input::DeviceInterface> device;
+ ASSERT_NO_THROW(device = SurgSim::Input::DeviceInterface::getFactory().create("SurgSim::Devices::NovintDevice",
+ "TestFalcon"));
+ EXPECT_EQ("SurgSim::Devices::NovintDevice", device->getClassName());
+}
+
+TEST(NovintDeviceTest, AccessibleTest)
+{
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon");
+
+ std::string name = "initName";
+ SurgSim::DataStructures::OptionalValue<std::string> optionalName(name);
+ EXPECT_NO_THROW(device->setValue("InitializationName", optionalName));
+ std::string actualName;
+ ASSERT_TRUE(device->getInitializationName(&actualName));
+ EXPECT_EQ(name, actualName);
+
+ std::string number = "serialNumber";
+ SurgSim::DataStructures::OptionalValue<std::string> optionalNumber(number);
+ EXPECT_NO_THROW(device->setValue("SerialNumber", optionalNumber));
+ std::string actualNumber;
+ ASSERT_TRUE(device->getSerialNumber(&actualNumber));
+ EXPECT_EQ(number, actualNumber);
+
+ ASSERT_FALSE(device->is7DofDevice());
+ EXPECT_NO_THROW(device->setValue("7DofDevice", true));
+ EXPECT_TRUE(device->is7DofDevice());
+
+ EXPECT_NEAR(8.9, device->getMaxForce(), 1e-9);
+ EXPECT_NO_THROW(device->setValue("MaxForce", 20.0));
+ EXPECT_NEAR(20.0, device->getMaxForce(), 1e-9);
}
diff --git a/SurgSim/Devices/Novint/UnitTests/NovintScaffoldTest.cpp b/SurgSim/Devices/Novint/UnitTests/NovintScaffoldTest.cpp
index fed6e5b..525bfcf 100644
--- a/SurgSim/Devices/Novint/UnitTests/NovintScaffoldTest.cpp
+++ b/SurgSim/Devices/Novint/UnitTests/NovintScaffoldTest.cpp
@@ -16,28 +16,20 @@
/// \file
/// Tests for the NovintScaffold class and its device interactions.
-#include <memory>
-#include <string>
-#include <boost/thread.hpp>
#include <boost/chrono.hpp>
+#include <boost/thread.hpp>
#include <gtest/gtest.h>
+#include <memory>
+#include <string>
+
#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;
+using SurgSim::Devices::NovintDevice;
+using SurgSim::Devices::NovintScaffold;
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;
@@ -70,7 +62,6 @@ TEST(NovintScaffoldTest, CreateAndDestroyScaffold)
TEST(NovintScaffoldTest, ScaffoldLifeCycle)
{
- //NovintScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
std::weak_ptr<NovintScaffold> lastScaffold;
{
std::shared_ptr<NovintScaffold> scaffold = NovintScaffold::getOrCreateSharedInstance();
@@ -84,7 +75,7 @@ TEST(NovintScaffoldTest, ScaffoldLifeCycle)
}
{
- std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon");
ASSERT_NE(nullptr, device) << "Creation failed. Is a Novint device plugged in?";
// note: the device is NOT initialized!
{
@@ -105,7 +96,7 @@ TEST(NovintScaffoldTest, ScaffoldLifeCycle)
}
{
- std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon");
ASSERT_NE(nullptr, device) << "Device creation failed.";
ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Novint device plugged in?";
{
@@ -135,7 +126,7 @@ TEST(NovintScaffoldTest, ScaffoldLifeCycle)
}
{
- std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon", NOVINT_TEST_DEVICE_NAME);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon");
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();
@@ -149,14 +140,13 @@ TEST(NovintScaffoldTest, ScaffoldLifeCycle)
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);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon");
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();
@@ -169,13 +159,12 @@ TEST(NovintScaffoldTest, CreateDeviceSeveralTimes)
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);
+ std::shared_ptr<NovintDevice> device = std::make_shared<NovintDevice>("TestFalcon");
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();
diff --git a/SurgSim/Devices/Novint/VisualTest/CMakeLists.txt b/SurgSim/Devices/Novint/VisualTest/CMakeLists.txt
index ff6737f..f79c890 100644
--- a/SurgSim/Devices/Novint/VisualTest/CMakeLists.txt
+++ b/SurgSim/Devices/Novint/VisualTest/CMakeLists.txt
@@ -14,14 +14,6 @@
# limitations under the License.
-link_directories(
- ${Boost_LIBRARY_DIRS}
-)
-
-include_directories(
- "${CMAKE_CURRENT_SOURCE_DIR}"
-)
-
set(EXAMPLE_SOURCES
main.cpp
)
@@ -29,39 +21,14 @@ set(EXAMPLE_SOURCES
set(EXAMPLE_HEADERS
)
-set(SEVEN_DOF_EXAMPLE_SOURCES
- falcon_7dof_main.cpp
-)
-
-set(SEVEN_DOF_EXAMPLE_HEADERS
-)
-
set(LIBS
- NovintDevice
IdentityPoseDevice
- VisualTestCommon
+ NovintDevice
SurgSimInput
- SurgSimFramework
- SurgSimDataStructures
- ${NOVINT_HDAL_SDK_LIBRARIES}
- ${Boost_LIBRARIES}
- ${OPENGL_LIBRARIES}
+ VisualTestCommon
)
-add_executable(NovintVisualTest
- ${EXAMPLE_SOURCES} ${EXAMPLE_HEADERS})
-target_link_libraries(NovintVisualTest ${LIBS})
-surgsim_show_ide_folders(
+surgsim_add_executable(NovintVisualTest
"${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"
+target_link_libraries(NovintVisualTest ${LIBS})
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
deleted file mode 100644
index 5c0194d..0000000
--- a/SurgSim/Devices/Novint/VisualTest/falcon_7dof_main.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
index a8f59c9..a8888be 100644
--- a/SurgSim/Devices/Novint/VisualTest/main.cpp
+++ b/SurgSim/Devices/Novint/VisualTest/main.cpp
@@ -15,36 +15,23 @@
#include <memory>
-#include "SurgSim/Input/DeviceInterface.h"
-#include "SurgSim/Devices/Novint/NovintDevice.h"
#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
-
+#include "SurgSim/Devices/Novint/NovintDevice.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";
-
+// The initialization name of the NovintDevice. An empty string will use the first available Falcon.
+static const char* const NOVINT_DEVICE_NAME = "";
+// true if the Novint should be 7Dof, false if the Novint should be 3Dof.
+static const bool NOVINT_7_DOF = false;
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");
+ auto toolDevice = std::make_shared<SurgSim::Devices::NovintDevice>("NovintDevice");
+ toolDevice->setInitializationName(NOVINT_DEVICE_NAME);
+ toolDevice->set7DofDevice(NOVINT_7_DOF);
+ auto squareDevice = std::make_shared<SurgSim::Devices::IdentityPoseDevice>("IdentityPoseDevice");
- runToolSquareTest(toolDevice, squareDevice,
- //2345678901234567890123456789012345678901234567890123456789012345678901234567890
- "Move the Novint Falcon device to move the sphere tool.");
+ runToolSquareTest(toolDevice, squareDevice, "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.
diff --git a/SurgSim/Devices/Oculus/CMakeLists.txt b/SurgSim/Devices/Oculus/CMakeLists.txt
new file mode 100644
index 0000000..4ea4bf0
--- /dev/null
+++ b/SurgSim/Devices/Oculus/CMakeLists.txt
@@ -0,0 +1,67 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2015, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT 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(OculusSdk)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+ "${OCULUSSDK_INCLUDE_DIR}"
+)
+
+set(OCULUS_DEVICE_SOURCES
+ OculusDevice.cpp
+ OculusDisplaySettings.cpp
+ OculusScaffold.cpp
+ OculusView.cpp
+)
+
+set(OCULUS_DEVICE_HEADERS
+ OculusDevice.h
+ OculusDisplaySettings.h
+ OculusScaffold.h
+ OculusView.h
+)
+
+set(DEVICE_HEADERS ${DEVICE_HEADERS} Oculus/OculusDevice.h Oculus/OculusView.h PARENT_SCOPE)
+
+surgsim_add_library(
+ OculusDevice
+ "${OCULUS_DEVICE_SOURCES}"
+ "${OCULUS_DEVICE_HEADERS}"
+)
+
+set(LIBS
+ SurgSimDataStructures
+ SurgSimFramework
+ SurgSimGraphics
+ SurgSimInput
+ ${OCULUSSDK_LIBRARY}
+ ${OPENSCENEGRAPH_LIBRARIES}
+)
+
+target_link_libraries(OculusDevice ${LIBS})
+
+if(BUILD_TESTING)
+ # The unit tests will be built whenever the library is built.
+ add_subdirectory(UnitTests)
+ add_subdirectory(SceneTest)
+
+ if(GLUT_FOUND)
+ add_subdirectory(VisualTest)
+ endif(GLUT_FOUND)
+endif()
+
+# Put OculusDevice into folder "Devices"
+set_target_properties(OculusDevice PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Oculus/Oculus.dox b/SurgSim/Devices/Oculus/Oculus.dox
new file mode 100644
index 0000000..47641b0
--- /dev/null
+++ b/SurgSim/Devices/Oculus/Oculus.dox
@@ -0,0 +1,24 @@
+/*!
+
+\page Oculus Oculus: Oculus Rift DK2
+
+This adds support for Oculus Rift DK2 virtual reality display.
+
+Supported Version
+-----------------
+Version 0.5.0.1 beta is the ONLY SDK tested and supported for Windows and GNU/Linux.
+
+Dependencies
+------------
+Oculus Rift SDK: SDK is available for Windows and GNU/Linux on Oculus Rift Developer
+<A HREF="https://developer.oculus.com/downloads/">website</A>.
+
+For Windows, you must first install the Oculus Runtime package and download the Oculus SDK.
+Then set an environment variable OCULUSSDK_DIR to the directory containing <tt>LibOVR</tt> (but
+do not include 'LibOVR' in the variable).
+For GNU/Linux, download the SDK package, extract it, run its configuration script FIRST, build the
+library, then install it. If the SDK is in a custom location, you must set an environment variable
+OCULUSSDK_DIR pointing to the install path. The default installed one will be picked if both exist.
+
+Then, in CMake, enable BUILD_DEVICE_OCULUS.
+*/
diff --git a/SurgSim/Devices/Oculus/OculusDevice.cpp b/SurgSim/Devices/Oculus/OculusDevice.cpp
new file mode 100644
index 0000000..dbdf4a9
--- /dev/null
+++ b/SurgSim/Devices/Oculus/OculusDevice.cpp
@@ -0,0 +1,99 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Oculus/OculusDevice.h"
+
+#include "SurgSim/Devices/Oculus/OculusScaffold.h"
+#include "SurgSim/Framework/Log.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::OculusDevice, OculusDevice);
+
+OculusDevice::OculusDevice(const std::string& name) :
+ SurgSim::Input::CommonDevice(name, OculusScaffold::buildDeviceInputData()),
+ m_nearPlane(0.1f),
+ m_farPlane(10.0f)
+{
+}
+
+OculusDevice::~OculusDevice()
+{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+
+
+bool OculusDevice::initialize()
+{
+ SURGSIM_ASSERT(!isInitialized()) << getName() << " already initialized.";
+ auto scaffold = OculusScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold != nullptr);
+
+ bool initialize = false;
+ if (scaffold->registerDevice(this))
+ {
+ m_scaffold = std::move(scaffold);
+ initialize = true;
+ }
+ return initialize;
+}
+
+bool OculusDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized()) << getName() << " is not initialized, cannot finalize.";
+ bool result = m_scaffold->unregisterDevice(this);
+ m_scaffold.reset();
+ return result;
+}
+
+bool OculusDevice::isInitialized() const
+{
+ return (m_scaffold != nullptr);
+}
+
+void OculusDevice::setNearPlane(float nearPlane)
+{
+ SURGSIM_ASSERT(nearPlane > 0.0f) << "Can not use a negative near plane.";
+ SURGSIM_LOG_IF(nearPlane > m_farPlane, SurgSim::Framework::Logger::getLogger("Device/OculusDevice"), WARNING) <<
+ __FUNCTION__ << "Trying to set a near plane that is greater than the far plane.";
+ m_nearPlane = nearPlane;
+}
+
+float OculusDevice::getNearPlane() const
+{
+ return m_nearPlane;
+}
+
+void OculusDevice::setFarPlane(float farPlane)
+{
+ SURGSIM_ASSERT(farPlane > 0.0f) << "Can not use a negative far plane.";
+ SURGSIM_LOG_IF(farPlane < m_nearPlane, SurgSim::Framework::Logger::getLogger("Device/OculusDevice"), WARNING) <<
+ __FUNCTION__ << "Trying to set a far plane that is smaller than the near plane.";
+ m_farPlane = farPlane;
+}
+
+float OculusDevice::getFarPlane() const
+{
+ return m_farPlane;
+}
+
+}; // namespace Devices
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Oculus/OculusDevice.h b/SurgSim/Devices/Oculus/OculusDevice.h
new file mode 100644
index 0000000..6ee0435
--- /dev/null
+++ b/SurgSim/Devices/Oculus/OculusDevice.h
@@ -0,0 +1,96 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_OCULUS_OCULUSDEVICE_H
+#define SURGSIM_DEVICES_OCULUS_OCULUSDEVICE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Input/CommonDevice.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+class OculusScaffold;
+
+SURGSIM_STATIC_REGISTRATION(OculusDevice);
+
+/// A class implementing the communication with Oculus Rift DK2.
+///
+/// \par Application input provided by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | pose | "pose" | %Device pose (units are meters). |
+/// | matrix | "leftProjectionMatix" | %Projection matrix for left eye |
+/// | matrix | "rghtProjectionMatix" | %Projection matrix for right eye |
+///
+/// \par Application output used by the device: none.
+/// \note The axes of the pose of the HMD are a right-handed system: X & Z are in the plane of the floor,
+/// with X pointing to the camera's left (i.e. to the HMD's right) and Z pointing towards the camera,
+/// while Y points up from the floor.
+/// By default the tracking origin is located one meter away from the positional tracking camera in the direction
+/// of the optical axis but with the same height as the camera.
+/// \sa SurgSim::Input::CommonDevice, SurgSim::Input::DeviceInterface
+class OculusDevice : public SurgSim::Input::CommonDevice
+{
+public:
+ /// Constructor.
+ /// \param name A unique name for the device.
+ explicit OculusDevice(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Devices::OculusDevice);
+
+ /// Destructor.
+ virtual ~OculusDevice();
+
+ bool initialize() override;
+
+ bool isInitialized() const override;
+
+ /// Set the near plane
+ /// \param nearPlane The near plane
+ void setNearPlane(float nearPlane);
+
+ /// \return The near plane
+ float getNearPlane() const;
+
+ /// Set the far plane
+ /// \param farPlane The far plane
+ void setFarPlane(float farPlane);
+
+ /// \return The far plane
+ float getFarPlane() const;
+
+private:
+ friend class OculusScaffold;
+
+ bool finalize() override;
+
+ /// Near Plane
+ float m_nearPlane;
+
+ /// Far Plane
+ float m_farPlane;
+
+ /// Communication with hardware is handled by scaffold.
+ std::shared_ptr<OculusScaffold> m_scaffold;
+};
+
+}; // namespace Devices
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_OCULUS_OCULUSDEVICE_H
diff --git a/SurgSim/Devices/Oculus/OculusDisplaySettings.cpp b/SurgSim/Devices/Oculus/OculusDisplaySettings.cpp
new file mode 100644
index 0000000..afa4edd
--- /dev/null
+++ b/SurgSim/Devices/Oculus/OculusDisplaySettings.cpp
@@ -0,0 +1,68 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Oculus/OculusDisplaySettings.h"
+
+#include "SurgSim/Graphics/OsgMatrixConversions.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+OculusDisplaySettings::OculusDisplaySettings() : m_leftEyeProjectionMatrix(osg::Matrixd()),
+ m_rightEyeProjectionMatrix(osg::Matrixd())
+{
+}
+
+OculusDisplaySettings::OculusDisplaySettings(const osg::DisplaySettings* displaySettings) :
+ osg::DisplaySettings(*displaySettings),
+ m_leftEyeProjectionMatrix(osg::Matrixd()),
+ m_rightEyeProjectionMatrix(osg::Matrixd())
+{
+}
+
+void OculusDisplaySettings::setLeftEyeProjectionMatrix(const SurgSim::Math::Matrix44d& matrix)
+{
+ m_leftEyeProjectionMatrix = SurgSim::Graphics::toOsg(matrix);
+}
+
+SurgSim::Math::Matrix44d OculusDisplaySettings::getLeftEyeProjectionMatrix() const
+{
+ return SurgSim::Graphics::fromOsg(m_leftEyeProjectionMatrix);
+}
+
+void OculusDisplaySettings::setRightEyeProjectionMatrix(const SurgSim::Math::Matrix44d& matrix)
+{
+ m_rightEyeProjectionMatrix = SurgSim::Graphics::toOsg(matrix);
+}
+
+SurgSim::Math::Matrix44d OculusDisplaySettings::getRightEyeProjectionMatrix() const
+{
+ return SurgSim::Graphics::fromOsg(m_rightEyeProjectionMatrix);
+}
+
+osg::Matrixd OculusDisplaySettings::computeLeftEyeProjectionImplementation(const osg::Matrixd&) const
+{
+ return m_leftEyeProjectionMatrix;
+}
+
+osg::Matrixd OculusDisplaySettings::computeRightEyeProjectionImplementation(const osg::Matrixd&) const
+{
+ return m_rightEyeProjectionMatrix;
+}
+
+}; // namespace Devices
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/Oculus/OculusDisplaySettings.h b/SurgSim/Devices/Oculus/OculusDisplaySettings.h
new file mode 100644
index 0000000..ce078a8
--- /dev/null
+++ b/SurgSim/Devices/Oculus/OculusDisplaySettings.h
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_OCULUS_OCULUSDISPLAYSETTINGS_H
+#define SURGSIM_DEVICES_OCULUS_OCULUSDISPLAYSETTINGS_H
+
+#include <osg/DisplaySettings>
+
+#include "SurgSim/Math/Matrix.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+/// A customized osg::DisplaySettings, to be used with Oculus device.
+/// It passes customized projection matrices to OSG for rendering.
+class OculusDisplaySettings : public osg::DisplaySettings
+{
+public:
+ /// Constructor
+ OculusDisplaySettings();
+
+ /// Constructor
+ /// \param displaySettings An instance of osg::DisplaySettings
+ explicit OculusDisplaySettings(const osg::DisplaySettings* displaySettings);
+
+ /// Set the projection matrix of the left eye
+ /// \param matrix Projection matrix for left eye
+ void setLeftEyeProjectionMatrix(const SurgSim::Math::Matrix44d& matrix);
+
+ /// Get the projection matrix of the left eye
+ /// \return Projection matrix for left eye
+ SurgSim::Math::Matrix44d getLeftEyeProjectionMatrix() const;
+
+ /// Set the projection matrix of the right eye
+ /// \param matrix Projection matrix for right eye
+ void setRightEyeProjectionMatrix(const SurgSim::Math::Matrix44d& matrix);
+
+ /// Get the projection matrix of the right eye
+ /// \return Projection matrix for right eye
+ SurgSim::Math::Matrix44d getRightEyeProjectionMatrix() const;
+
+ /// This method returns the projection matrix set by setLeftEyeProjectionMatrix() method.
+ /// OSG calls this overriding function to get the left eye projection matrix to use.
+ /// The parameter passed in is NOT used.
+ osg::Matrixd computeLeftEyeProjectionImplementation(const osg::Matrixd&) const override;
+
+ /// This method returns the projection matrix set by setRighttEyeProjectionMatrix() method.
+ /// OSG calls this overiding function to get the right eye projection matrix to use.
+ /// The parameter passed in is NOT used.
+ osg::Matrixd computeRightEyeProjectionImplementation(const osg::Matrixd&) const override;
+
+private:
+ /// Left eye projection matrix
+ osg::Matrixd m_leftEyeProjectionMatrix;
+
+ /// Right eye projection matrix
+ osg::Matrixd m_rightEyeProjectionMatrix;
+};
+
+}; // namespace Devices
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_OCULUS_OCULUSDISPLAYSETTINGS_H
\ No newline at end of file
diff --git a/SurgSim/Devices/Oculus/OculusScaffold.cpp b/SurgSim/Devices/Oculus/OculusScaffold.cpp
new file mode 100644
index 0000000..6e5fdd7
--- /dev/null
+++ b/SurgSim/Devices/Oculus/OculusScaffold.cpp
@@ -0,0 +1,322 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Oculus/OculusScaffold.h"
+
+#include <boost/thread/locks.hpp>
+#include <boost/thread/mutex.hpp>
+#include <list>
+#include <memory>
+
+#include <OVR_Version.h>
+#if 6 == OVR_MAJOR_VERSION
+#include <OVR_CAPI_0_6_0.h>
+#elif 5 == OVR_MAJOR_VERSION
+#include <OVR_CAPI_0_5_0.h>
+#endif
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/Oculus/OculusDevice.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::DataStructures::DataGroupBuilder;
+using SurgSim::Framework::Logger;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+struct OculusScaffold::DeviceData
+{
+ /// Constructor
+ /// \param device Device to be wrapped
+ explicit DeviceData(OculusDevice* device) :
+ deviceObject(device),
+ handle(nullptr)
+ {
+#if 6 == OVR_MAJOR_VERSION
+ ovrHmd_Create(0, &handle);
+#elif 5 == OVR_MAJOR_VERSION
+ handle = ovrHmd_Create(0);
+#endif
+ }
+
+ ~DeviceData()
+ {
+ if (handle != nullptr)
+ {
+ ovrHmd_Destroy(handle);
+ }
+ }
+
+ /// The device object wrapped in this class.
+ OculusDevice* const deviceObject;
+
+ /// Device handle
+ ovrHmd handle;
+};
+
+struct OculusScaffold::StateData
+{
+ /// List of registered devices.
+ std::list<std::unique_ptr<OculusScaffold::DeviceData>> registeredDevices;
+
+ /// The mutex that protects the list of registered devices.
+ boost::mutex mutex;
+};
+
+OculusScaffold::OculusScaffold() :
+ Framework::BasicThread("OculusScaffold"),
+ m_logger(Logger::getLogger("Devices/Oculus")),
+ m_state(new StateData)
+{
+ SURGSIM_LOG_DEBUG(m_logger) << "Oculus: Shared scaffold created.";
+
+ // Oculus DK2 tracks Gyroscope, Accelerometer, Magnetometer at 1000Hz and
+ // positional tracking is done at 60Hz.
+ setRate(1000.0);
+
+}
+
+OculusScaffold::~OculusScaffold()
+{
+ ovr_Shutdown();
+}
+
+bool OculusScaffold::registerDevice(OculusDevice* device)
+{
+ bool success = true;
+ if (!isRunning())
+ {
+ std::shared_ptr<SurgSim::Framework::Barrier> barrier = std::make_shared<SurgSim::Framework::Barrier>(2);
+ start(barrier);
+ barrier->wait(true); // Wait for initialize
+ barrier->wait(true); // Wait for startup
+ success = isInitialized();
+ }
+
+ if (success)
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ const std::string deviceName = device->getName();
+ auto sameName = [&deviceName](const std::unique_ptr<DeviceData>& info)
+ {
+ return info->deviceObject->getName() == deviceName;
+ };
+ auto found = std::find_if(m_state->registeredDevices.cbegin(), m_state->registeredDevices.cend(), sameName);
+
+ if (found == m_state->registeredDevices.end())
+ {
+ std::unique_ptr<DeviceData> info(new DeviceData(device));
+ success = doRegisterDevice(info.get());
+ if (success)
+ {
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << ": Registered";
+ m_state->registeredDevices.emplace_back(std::move(info));
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Tried to register a device when the same name, '" <<
+ device->getName() << "', is already present!";
+ success = false;
+ }
+ }
+
+ if (isRunning() && m_state->registeredDevices.size() == 0)
+ {
+ stop();
+ }
+
+ return success;
+}
+
+bool OculusScaffold::doRegisterDevice(DeviceData* info)
+{
+ if (ovrHmd_Detect() < 1)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "No available Oculus device detected.";
+ return false;
+ }
+
+ if (m_state->registeredDevices.size() > 0)
+ {
+ SURGSIM_LOG_SEVERE(m_logger)
+ << "There is one registered Oculus device already. OculusScaffold only supports one device right now";
+ return false;
+ }
+
+ if (info->handle == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to obtain a handle to Oculus Device. Is an Oculus device plugged in?";
+ return false;
+ }
+
+#if 6 == OVR_MAJOR_VERSION
+ if (ovrSuccess != ovrHmd_ConfigureTracking(info->handle, ovrTrackingCap_Orientation |
+ ovrTrackingCap_MagYawCorrection |
+ ovrTrackingCap_Position, 0))
+#elif 5 == OVR_MAJOR_VERSION
+ if (ovrTrue != ovrHmd_ConfigureTracking(info->handle, ovrTrackingCap_Orientation |
+ ovrTrackingCap_MagYawCorrection |
+ ovrTrackingCap_Position, 0))
+#endif
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Failed to configure an Oculus Device. Registration failed";
+ return false;
+ }
+
+ // Query the HMD for the left and right projection matrices.
+ ovrFovPort defaultLeftFov = info->handle->DefaultEyeFov[ovrEyeType::ovrEye_Left];
+ ovrFovPort defaultRightFov = info->handle->DefaultEyeFov[ovrEyeType::ovrEye_Right];
+
+ float nearPlane = info->deviceObject->getNearPlane();
+ float farPlane = info->deviceObject->getFarPlane();
+
+ ovrMatrix4f leftProjection = ovrMatrix4f_Projection(defaultLeftFov, nearPlane, farPlane,
+ ovrProjectionModifier::ovrProjection_RightHanded);
+ ovrMatrix4f rightProjection = ovrMatrix4f_Projection(defaultRightFov, nearPlane, farPlane,
+ ovrProjectionModifier::ovrProjection_RightHanded);
+
+ DataGroup& inputData = info->deviceObject->getInputData();
+ inputData.matrices().set(DataStructures::Names::LEFT_PROJECTION_MATRIX,
+ Eigen::Map<const Eigen::Matrix<float, 4, 4, Eigen::RowMajor>>
+ (&leftProjection.M[0][0]).cast<double>());
+ inputData.matrices().set(DataStructures::Names::RIGHT_PROJECTION_MATRIX,
+ Eigen::Map<const Eigen::Matrix<float, 4, 4, Eigen::RowMajor>>
+ (&rightProjection.M[0][0]).cast<double>());
+ info->deviceObject->pushInput();
+
+ return true;
+}
+
+bool OculusScaffold::unregisterDevice(const OculusDevice* const device)
+{
+ bool result = false;
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ auto match = std::find_if(m_state->registeredDevices.begin(), m_state->registeredDevices.end(),
+ [&device](const std::unique_ptr<DeviceData>& info)
+ {
+ return info->deviceObject == device;
+ });
+
+ if (match != m_state->registeredDevices.end())
+ {
+ m_state->registeredDevices.erase(match);
+ // the iterator is now invalid but that's OK
+ SURGSIM_LOG_INFO(m_logger) << "Device " << getName() << ": unregistered.";
+ result = true;
+ }
+ }
+
+ // #threadsafety After unregistering, another thread could be in the process of registering.
+ if (isRunning() && m_state->registeredDevices.size() == 0)
+ {
+ stop();
+ }
+
+ SURGSIM_LOG_IF(!result, m_logger, SEVERE) << "Attempted to release a non-registered device.";
+
+ return result;
+}
+
+bool OculusScaffold::doInitialize()
+{
+#if 6 == OVR_MAJOR_VERSION
+ if (ovrSuccess != ovr_Initialize(nullptr))
+#elif 5 == OVR_MAJOR_VERSION
+ if (ovrTrue != ovr_Initialize(nullptr))
+#endif
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Oculus SDK initialization failed.";
+ }
+
+ return true;
+}
+
+bool OculusScaffold::doStartUp()
+{
+ return true;
+}
+
+bool OculusScaffold::doUpdate(double dt)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ for (auto& device : m_state->registeredDevices)
+ {
+ DataGroup& inputData = device->deviceObject->getInputData();
+
+ // Query the HMD for the current tracking state.
+ // If time in 2nd parameter is now or earlier, no pose prediction is made.
+ // Pose is reported in a right handed coordinate system, X->RIGHT, Y->UP, Z->OUT.
+ // Positive rotation is counterclockwise.
+ // The XZ plane is ALWAYS aligned with the ground regardless of camera orientation.
+ // Default tracking origin is one meter away from the camera.
+ ovrTrackingState ts = ovrHmd_GetTrackingState(device->handle, ovr_GetTimeInSeconds());
+ if (ts.StatusFlags & (ovrStatus_OrientationTracked | ovrStatus_PositionTracked))
+ {
+ ovrPosef ovrPose = ts.HeadPose.ThePose;
+
+ Vector3d position(ovrPose.Position.x, ovrPose.Position.y, ovrPose.Position.z);
+ Quaterniond orientation(ovrPose.Orientation.w, ovrPose.Orientation.x,
+ ovrPose.Orientation.y, ovrPose.Orientation.z);
+ RigidTransform3d pose = makeRigidTransform(orientation, position);
+
+ inputData.poses().set(DataStructures::Names::POSE, pose);
+ }
+ else
+ {
+ inputData.poses().reset(DataStructures::Names::POSE);
+ }
+
+ device->deviceObject->pushInput();
+ }
+
+ return true;
+}
+
+SurgSim::DataStructures::DataGroup OculusScaffold::buildDeviceInputData()
+{
+ DataGroupBuilder builder;
+ builder.addPose(DataStructures::Names::POSE);
+ builder.addMatrix(DataStructures::Names::LEFT_PROJECTION_MATRIX);
+ builder.addMatrix(DataStructures::Names::RIGHT_PROJECTION_MATRIX);
+ return builder.createData();
+}
+
+std::shared_ptr<OculusScaffold> OculusScaffold::getOrCreateSharedInstance()
+{
+ static auto creator = []()
+ {
+ return std::shared_ptr<OculusScaffold>(new OculusScaffold);
+ };
+ static Framework::SharedInstance<OculusScaffold> sharedInstance(creator);
+ return sharedInstance.get();
+}
+
+}; // namespace Devices
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/Oculus/OculusScaffold.h b/SurgSim/Devices/Oculus/OculusScaffold.h
new file mode 100644
index 0000000..dcbd320
--- /dev/null
+++ b/SurgSim/Devices/Oculus/OculusScaffold.h
@@ -0,0 +1,106 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_OCULUS_OCULUSSCAFFOLD_H
+#define SURGSIM_DEVICES_OCULUS_OCULUSSCAFFOLD_H
+
+#include <memory>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Framework/BasicThread.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+class Logger;
+}
+};
+
+namespace SurgSim
+{
+
+namespace Devices
+{
+class OculusDevice;
+
+/// A class that manages Oculus Rift DK2 devices.
+///
+/// \sa SurgSim::Devices::OculusDevice
+class OculusScaffold : SurgSim::Framework::BasicThread
+{
+public:
+ /// Destructor.
+ ~OculusScaffold();
+
+protected:
+ bool doInitialize() override;
+ bool doStartUp() override;
+ bool doUpdate(double dt) override;
+
+private:
+ /// Internal shared state data type.
+ struct StateData;
+
+ /// Internal per-device information.
+ struct DeviceData;
+
+ friend class OculusDevice;
+
+ /// Constructor.
+ OculusScaffold();
+
+ /// Gets or creates the scaffold shared by all OculusDevice instances.
+ /// The scaffold is managed using a SharedInstance object, so it will be destroyed when all devices are released.
+ /// \return the scaffold object.
+ static std::shared_ptr<OculusScaffold> getOrCreateSharedInstance();
+
+ /// Registers the specified device object.
+ /// \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(OculusDevice* device);
+
+ /// Do the Oculus SDK specific registration
+ /// \param info The device data
+ /// \return true on success, false on failure.
+ bool doRegisterDevice(DeviceData* info);
+
+ /// Unregisters the specified device object.
+ /// \param device The device object.
+ /// \return true on success, false on failure.
+ bool unregisterDevice(const OculusDevice* device);
+
+ /// Initializes Oculus SDK.
+ /// \return true on success; false otherwise.
+ bool initializeSdk();
+
+ /// Finalizes (de-initializes) Oculus SDK.
+ /// \return true on success; false otherwise.
+ bool finalizeSdk();
+
+ /// 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 Devices
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_OCULUS_OCULUSSCAFFOLD_H
diff --git a/SurgSim/Devices/Oculus/OculusView.cpp b/SurgSim/Devices/Oculus/OculusView.cpp
new file mode 100644
index 0000000..af7ed8c
--- /dev/null
+++ b/SurgSim/Devices/Oculus/OculusView.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/Devices/Oculus/OculusView.h"
+
+#include <osg/DisplaySettings>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Devices/Oculus/OculusDisplaySettings.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Input/InputComponent.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Devices::OculusView, OculusView);
+
+OculusView::OculusView(const std::string& name) : OsgView(name)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OculusView, std::shared_ptr<SurgSim::Framework::Component>, InputComponent,
+ getInputComponent, setInputComponent);
+
+ // Default Settings of Oculus DK2
+ setFullScreen(true);
+ setDisplayType(View::DISPLAY_TYPE_HMD);
+ setStereoMode(View::STEREO_MODE_HORIZONTAL_SPLIT);
+ setScreenWidth(0.0631);
+ setScreenHeight(0.071);
+ setEyeSeparation(0.06);
+ setScreenDistance(0.10);
+ setTargetScreen(1); // Assume Oculus HMD has ID '1'.
+
+ std::array<int, 2> dimensions = {1920, 1080};
+ setDimensions(dimensions);
+}
+
+OculusView::~OculusView()
+{
+}
+
+void OculusView::setInputComponent(std::shared_ptr<Framework::Component> input)
+{
+ m_inputComponent = Framework::checkAndConvert<Input::InputComponent>(input, "SurgSim::Input::InputComponent");
+}
+
+std::shared_ptr<Input::InputComponent> OculusView::getInputComponent() const
+{
+ return m_inputComponent;
+}
+
+osg::ref_ptr<osg::DisplaySettings> OculusView::createDisplaySettings() const
+{
+ SURGSIM_ASSERT(m_inputComponent != nullptr) << "No InputComponent is connected to this view.";
+
+ osg::ref_ptr<SurgSim::Devices::OculusDisplaySettings> displaySettings =
+ new SurgSim::Devices::OculusDisplaySettings(OsgView::createDisplaySettings());
+
+ SurgSim::DataStructures::DataGroup dataGroup;
+ m_inputComponent->getData(&dataGroup);
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType leftProjectionMatrix;
+ SurgSim::DataStructures::DataGroup::DynamicMatrixType rightProjectionMatrix;
+
+ if (dataGroup.matrices().get(SurgSim::DataStructures::Names::LEFT_PROJECTION_MATRIX, &leftProjectionMatrix) &&
+ dataGroup.matrices().get(SurgSim::DataStructures::Names::RIGHT_PROJECTION_MATRIX, &rightProjectionMatrix))
+ {
+ displaySettings->setLeftEyeProjectionMatrix(leftProjectionMatrix.block<4,4>(0, 0));
+ displaySettings->setRightEyeProjectionMatrix(rightProjectionMatrix.block<4,4>(0, 0));
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getLogger("OculusView")) <<
+ "No projection matrices for left/right eye.";
+ }
+ return displaySettings;
+}
+
+}; // namespace Devices
+}; // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Devices/Oculus/OculusView.h b/SurgSim/Devices/Oculus/OculusView.h
new file mode 100644
index 0000000..8d8ee14
--- /dev/null
+++ b/SurgSim/Devices/Oculus/OculusView.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_DEVICES_OCULUS_OCULUSVIEW_H
+#define SURGSIM_DEVICES_OCULUS_OCULUSVIEW_H
+
+#include <memory>
+#include <osg/ref_ptr>
+#include <string>
+
+#include "SurgSim/Graphics/OsgView.h"
+
+namespace osg
+{
+class DisplaySettings;
+}
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+class Component;
+}
+
+namespace Input
+{
+class InputComponent;
+}
+
+namespace Devices
+{
+
+SURGSIM_STATIC_REGISTRATION(OculusView);
+
+/// OculusView is a customization of SurgSim::Graphics::OsgView with projection matrices pulled from the Oculus device.
+class OculusView : public SurgSim::Graphics::OsgView
+{
+public:
+ /// Constructor
+ /// \param name Name of the view
+ explicit OculusView(const std::string& name);
+
+ /// Destructor
+ ~OculusView();
+
+ SURGSIM_CLASSNAME(SurgSim::Devices::OculusView);
+
+ /// Set the InputComponent this view connects to.
+ /// Projection matrices of the Oculus device are passed via the DataGroup the InputComponent carries.
+ /// \param input The InputComponent
+ void setInputComponent(std::shared_ptr<Framework::Component> input);
+
+ /// \return The InputComponnet this view connects
+ std::shared_ptr<Input::InputComponent> getInputComponent() const;
+
+protected:
+ osg::ref_ptr<osg::DisplaySettings> createDisplaySettings() const override;
+
+private:
+ /// The InputComponent this view connects.
+ std::shared_ptr<Input::InputComponent> m_inputComponent;
+};
+
+}; // namespace Graphics
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_OCULUS_OCULUSVIEW_H
diff --git a/SurgSim/Devices/Oculus/SceneTest/CMakeLists.txt b/SurgSim/Devices/Oculus/SceneTest/CMakeLists.txt
new file mode 100644
index 0000000..eee859b
--- /dev/null
+++ b/SurgSim/Devices/Oculus/SceneTest/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(Boost 1.54 COMPONENTS program_options)
+
+link_directories(
+ ${Boost_LIBRARY_DIRS}
+)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+ "${OPENTHREADS_INCLUDE_DIR}"
+)
+
+set(SOURCES
+ OculusSceneTest.cpp
+)
+
+set(HEADERS
+)
+
+# Configure the path for the data files
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+surgsim_add_executable(OculusSceneTest "${SOURCES}" "${HEADERS}")
+
+SET(LIBS
+ SurgSimFramework
+ SurgSimBlocks
+ SurgSimGraphics
+ SurgSimDataStructures
+ SurgSimInput
+ OculusDevice
+ IdentityPoseDevice
+ ${Boost_LIBRARIES}
+ ${YAML_CPP_LIBRARIES}
+)
+
+target_link_libraries(OculusSceneTest ${LIBS})
+
+set_target_properties(OculusSceneTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Oculus/SceneTest/Data/CameraText.yaml b/SurgSim/Devices/Oculus/SceneTest/Data/CameraText.yaml
new file mode 100644
index 0000000..51cbcbb
--- /dev/null
+++ b/SurgSim/Devices/Oculus/SceneTest/Data/CameraText.yaml
@@ -0,0 +1,22 @@
+- SurgSim::Framework::BasicSceneElement:
+ Name: CameraText
+ Components:
+ - SurgSim::Blocks::DriveElementFromInputBehavior:
+ Name: Driver
+ Source:
+ SurgSim::Input::InputComponent:
+ Name: Input
+ Id: 9b77c4e2-e9ea-484f-bfca-72121189dcb6
+ - SurgSim::Graphics::OsgTextRepresentation:
+ Name: Text
+ Text: This is the text for this text I would hope it gets wrapped
+ MaximumWidth: 0.4
+ FontSize: 0.02
+ UseScreenSpace: false
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [-0.2, 0, -1.5]
+ Color: [0.81, 0.81, 0.0, 1.0]
+ DrawBackground: true
+ BackgroundColor: [0.05, 0.05, 0.6, 1.0]
+ BackgroundMargin: 0.005
diff --git a/SurgSim/Devices/Oculus/SceneTest/Data/Cube.yaml b/SurgSim/Devices/Oculus/SceneTest/Data/Cube.yaml
new file mode 100644
index 0000000..39db158
--- /dev/null
+++ b/SurgSim/Devices/Oculus/SceneTest/Data/Cube.yaml
@@ -0,0 +1,8 @@
+- SurgSim::Framework::BasicSceneElement:
+ Name: Graphics
+ Components:
+ - SurgSim::Graphics::OsgBoxRepresentation:
+ Name: Box
+ Size: [0.1, 0.1, 0.1]
+ - SurgSim::Graphics::OsgAxesRepresentation:
+ Name: Axes
\ No newline at end of file
diff --git a/SurgSim/Devices/Oculus/SceneTest/Data/MonoView.yaml b/SurgSim/Devices/Oculus/SceneTest/Data/MonoView.yaml
new file mode 100644
index 0000000..486f2d0
--- /dev/null
+++ b/SurgSim/Devices/Oculus/SceneTest/Data/MonoView.yaml
@@ -0,0 +1,17 @@
+- SurgSim::Framework::BasicSceneElement:
+ Name: View
+ Components:
+ - SurgSim::Graphics::OsgView:
+ Name: View
+ Camera:
+ SurgSim::Graphics::OsgCamera:
+ Id: 9018dd38-c1ae-4e80-a27a-dc24e4b75d35
+ Name: Camera
+ CameraPosition: [0, 0.5, 1]
+ CameraLookAt: [0, 0, 0]
+ CameraManipulatorEnabled: true
+ - SurgSim::Graphics::OsgCamera:
+ Id: 9018dd38-c1ae-4e80-a27a-dc24e4b75d35
+ Name: Camera
+ ProjectionMatrix: [[2.414213562373095, 0, 0, 0], [0, 2.414213562373095, 0, 0], [0, 0, -1.002002002002002, -0.02002002002002002], [0, 0, -1, 0]]
+ RenderGroupReference: __OssDefault__
\ No newline at end of file
diff --git a/SurgSim/Devices/Oculus/SceneTest/Data/OculusView.yaml b/SurgSim/Devices/Oculus/SceneTest/Data/OculusView.yaml
new file mode 100644
index 0000000..3fed599
--- /dev/null
+++ b/SurgSim/Devices/Oculus/SceneTest/Data/OculusView.yaml
@@ -0,0 +1,36 @@
+- SurgSim::Framework::BasicSceneElement:
+ Name: View
+ Components:
+ - SurgSim::Blocks::DriveElementFromInputBehavior:
+ Name: Driver
+ Source:
+ SurgSim::Input::InputComponent:
+ Name: Input
+ Id: 9b77c4e2-e9ea-484f-bfca-72121189dcb6
+ - SurgSim::Input::InputComponent:
+ Name: Input
+ Id: 9b77c4e2-e9ea-484f-bfca-72121189dcb6
+ DeviceName: Tracker
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0.5, 2.0]
+ - SurgSim::Devices::OculusView:
+ Name: View
+ TargetScreen: 2
+ Camera:
+ SurgSim::Graphics::OsgCamera:
+ Id: 9018dd38-c1ae-4e80-a27a-dc24e4b75d35
+ Name: Camera
+ EyeSeparation: 0.06
+ ScreenHeight: 0.071
+ ScreenDistance: 0.10
+ ScreenWidth: 0.0631
+ InputComponent:
+ SurgSim::Input::InputComponent:
+ Name: Input
+ Id: 9b77c4e2-e9ea-484f-bfca-72121189dcb6
+ - SurgSim::Graphics::OsgCamera:
+ Id: 9018dd38-c1ae-4e80-a27a-dc24e4b75d35
+ Name: Camera
+ ProjectionMatrix: [[0.8390996311772799, 0, 0, 0], [0, 0.8390996311772799, 0, 0], [0, 0, -1.002002002002002, -0.02002002002002002], [0, 0, -1, 0]]
+ RenderGroupReference: __OssDefault__
\ No newline at end of file
diff --git a/SurgSim/Devices/Oculus/SceneTest/Data/OsgView.yaml b/SurgSim/Devices/Oculus/SceneTest/Data/OsgView.yaml
new file mode 100644
index 0000000..22185d2
--- /dev/null
+++ b/SurgSim/Devices/Oculus/SceneTest/Data/OsgView.yaml
@@ -0,0 +1,38 @@
+- SurgSim::Framework::BasicSceneElement:
+ Name: View
+ Components:
+ - SurgSim::Blocks::DriveElementFromInputBehavior:
+ Name: Driver
+ Source:
+ SurgSim::Input::InputComponent:
+ Name: Input
+ Id: 9b77c4e2-e9ea-484f-bfca-72121189dcb6
+ - SurgSim::Input::InputComponent:
+ Name: Input
+ Id: 9b77c4e2-e9ea-484f-bfca-72121189dcb6
+ DeviceName: Tracker
+ LocalPose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0.5, 2.0]
+ - SurgSim::Graphics::OsgView:
+ Name: View
+ FullScreen: true
+ Dimensions:
+ - 1920
+ - 1080
+ TargetScreen: 2
+ Camera:
+ SurgSim::Graphics::OsgCamera:
+ Id: 9018dd38-c1ae-4e80-a27a-dc24e4b75d35
+ Name: Camera
+ DisplayType: 1
+ StereoMode: 2
+ EyeSeparation: 0.06
+ ScreenHeight: 0.071
+ ScreenDistance: 0.10
+ ScreenWidth: 0.0631
+ - SurgSim::Graphics::OsgCamera:
+ Id: 9018dd38-c1ae-4e80-a27a-dc24e4b75d35
+ Name: Camera
+ ProjectionMatrix: [[0.8390996311772799, 0, 0, 0], [0, 0.8390996311772799, 0, 0], [0, 0, -1.002002002002002, -0.02002002002002002], [0, 0, -1, 0]]
+ RenderGroupReference: __OssDefault__
\ No newline at end of file
diff --git a/SurgSim/Devices/Oculus/SceneTest/OculusSceneTest.cpp b/SurgSim/Devices/Oculus/SceneTest/OculusSceneTest.cpp
new file mode 100644
index 0000000..61279de
--- /dev/null
+++ b/SurgSim/Devices/Oculus/SceneTest/OculusSceneTest.cpp
@@ -0,0 +1,157 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 <array>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <boost/program_options.hpp>
+
+#include "SurgSim/Blocks/Blocks.h"
+#include "SurgSim/Framework/Framework.h"
+#include "SurgSim/Graphics/Graphics.h"
+#include "SurgSim/Input/Input.h"
+#include "SurgSim/Math/Math.h"
+
+// Include this for linking only
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+#include "SurgSim/Devices/Oculus/OculusDevice.h"
+#include "SurgSim/Devices/Oculus/OculusView.h"
+
+using SurgSim::Framework::Logger;
+using SurgSim::Framework::Runtime;
+using SurgSim::Math::Vector3d;
+
+namespace po = boost::program_options;
+
+int main(int argc, char* argv[])
+{
+ // Parse command-line parameters
+ std::string sceneFileName;
+ po::options_description visible("Allowed options");
+ visible.add_options()("help", "produce help message")
+ ("config-file", po::value<std::string>(), "The config file to use");
+
+ po::options_description hidden("Hidden options");
+ hidden.add_options()("input-file", po::value<std::vector<std::string>>(), "input file");
+
+ po::options_description all("All options");
+ all.add(visible).add(hidden);
+
+ po::positional_options_description positional;
+ positional.add("input-file", -1);
+ po::variables_map variables;
+ po::store(po::command_line_parser(argc, argv).options(all).positional(positional).run(), variables);
+
+ if (variables.count("help"))
+ {
+ std::cout << visible << "\n";
+ return 1;
+ }
+
+ if (variables.count("input-file") == 0)
+ {
+ std::cout << "You need to supply one or more input files.\n";
+ return 1;
+ }
+
+ std::shared_ptr<Runtime> runtime;
+
+ if (variables.count("config-file") == 1)
+ {
+ runtime = std::make_shared<Runtime>(variables["config-file"].as<std::string>());
+ }
+ else
+ {
+ runtime = std::make_shared<Runtime>();
+ }
+
+ auto data = runtime->getApplicationData();
+
+ runtime->addManager(std::make_shared<SurgSim::Graphics::OsgManager>());
+ runtime->addManager(std::make_shared<SurgSim::Framework::BehaviorManager>());
+
+ std::string sceneFile;
+ if (variables.count("scene") == 1)
+ {
+ runtime->loadScene(variables["config-file"].as<std::string>());
+ }
+
+ std::array<std::string, 2> devices =
+ {
+ "SurgSim::Devices::OculusDevice",
+ "SurgSim::Devices::IdentityPoseDevice"
+ };
+
+ std::shared_ptr<SurgSim::Input::DeviceInterface> device;
+ for (auto deviceName : devices)
+ {
+ try
+ {
+ device = SurgSim::Input::DeviceInterface::getFactory().create(deviceName, "Tracker");
+ }
+ catch (std::exception e)
+ {
+ continue;
+ }
+ if (device->initialize())
+ {
+ break;
+ }
+ else
+ {
+ device = nullptr;
+ }
+ }
+ SURGSIM_ASSERT(device != nullptr) << "Could not initialize any kind of tracking device.";
+
+ auto inputManager = std::make_shared<SurgSim::Input::InputManager>();
+ runtime->addManager(inputManager);
+ inputManager->addDevice(device);
+
+ auto scene = runtime->getScene();
+
+ auto files = variables["input-file"].as<std::vector<std::string>>();
+ for (auto file : files)
+ {
+ auto appData = runtime->getApplicationData();
+ std::string path;
+
+#ifdef WIN32
+ // Fix windows backslashes coming in from the command-line, these may be absolute paths
+ std::replace_if(file.begin(), file.end(), [](const char& c)
+ {
+ return c == '\\';
+ }, '/');
+#endif
+
+ // In this case we circumvent the assertion that will stop execution if the file can't be found and
+ // try to show as many scenery objects as possible
+ if (appData->tryFindFile(file, &path))
+ {
+ runtime->addSceneElements(file);
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(Logger::getDefaultLogger())
+ << "Can't find " << file;
+ }
+ }
+
+ runtime->execute();
+ return 0;
+}
diff --git a/SurgSim/Devices/Oculus/SceneTest/config.txt.in b/SurgSim/Devices/Oculus/SceneTest/config.txt.in
new file mode 100644
index 0000000..e5a45ad
--- /dev/null
+++ b/SurgSim/Devices/Oculus/SceneTest/config.txt.in
@@ -0,0 +1,2 @@
+${SURGSIM_SOURCE_DIR}/Data/
+${CMAKE_CURRENT_SOURCE_DIR}/Data/
\ No newline at end of file
diff --git a/SurgSim/Devices/Oculus/UnitTests/CMakeLists.txt b/SurgSim/Devices/Oculus/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..df805ce
--- /dev/null
+++ b/SurgSim/Devices/Oculus/UnitTests/CMakeLists.txt
@@ -0,0 +1,41 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2015, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES 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
+ OculusDeviceTest.cpp
+ OculusDisplaySettingsTests.cpp
+ OculusViewTests.cpp
+)
+
+set(UNIT_TEST_HEADERS
+)
+
+set(LIBS
+ SurgSimDataStructures
+ SurgSimInput
+ SurgSimTesting
+ OculusDevice
+)
+
+surgsim_add_unit_tests(OculusDeviceTest)
+
+target_link_libraries(OculusDeviceTest ${LIBS})
+
+# Put OculusDeviceTest into folder "Devices"
+set_target_properties(OculusDeviceTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Oculus/UnitTests/OculusDeviceTest.cpp b/SurgSim/Devices/Oculus/UnitTests/OculusDeviceTest.cpp
new file mode 100644
index 0000000..e97797b
--- /dev/null
+++ b/SurgSim/Devices/Oculus/UnitTests/OculusDeviceTest.cpp
@@ -0,0 +1,118 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/chrono.hpp>
+#include <boost/thread.hpp>
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Devices/Oculus/OculusDevice.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::Devices::OculusDevice;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Input::InputConsumerInterface;
+using SurgSim::Testing::MockInputOutput;
+
+TEST(OculusDeviceTest, CreateAndInitializeDevice)
+{
+ auto device = std::make_shared<OculusDevice>("Oculus");
+ ASSERT_TRUE(nullptr != device) << "Device creation failed.";
+
+ EXPECT_FALSE(device->isInitialized());
+ EXPECT_EQ("Oculus", device->getName());
+
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is an Oculus device plugged in?";
+
+ EXPECT_TRUE(device->isInitialized());
+ EXPECT_EQ("Oculus", device->getName());
+
+ EXPECT_FLOAT_EQ(0.1f, device->getNearPlane());
+ EXPECT_FLOAT_EQ(10.0f, device->getFarPlane());
+}
+
+TEST(OculusDeviceTest, SetAndGetNearAndFarPlanes)
+{
+ auto device = std::make_shared<OculusDevice>("Oculus");
+ EXPECT_FLOAT_EQ(0.1f, device->getNearPlane());
+ EXPECT_FLOAT_EQ(10.0f, device->getFarPlane());
+
+ EXPECT_THROW(device->setNearPlane(-0.1f), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(device->setFarPlane(-10.0f), SurgSim::Framework::AssertionFailure);
+
+ EXPECT_NO_THROW(device->setNearPlane(0.2f));
+ EXPECT_NO_THROW(device->setFarPlane(20.0f));
+
+ EXPECT_FLOAT_EQ(0.2f, device->getNearPlane());
+ EXPECT_FLOAT_EQ(20.0f, device->getFarPlane());
+}
+
+TEST(OculusDeviceTest, Factory)
+{
+ std::shared_ptr<SurgSim::Input::DeviceInterface> device;
+ ASSERT_NO_THROW(device = SurgSim::Input::DeviceInterface::getFactory().create(
+ "SurgSim::Devices::OculusDevice", "Device"));
+ EXPECT_NE(nullptr, device);
+}
+
+TEST(OculusDeviceTest, RegisterMoreThanOneDevice)
+{
+ auto device1 = std::make_shared<OculusDevice>("Oculus");
+ ASSERT_TRUE(nullptr != device1) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is an Oculus device plugged in?";
+
+ auto device2 = std::make_shared<OculusDevice>("Oculus2");
+ ASSERT_TRUE(nullptr != device2) << "Device creation failed.";
+ ASSERT_FALSE(device2->initialize()) << "Initialization should not have succeceded, only one device allowed.";
+}
+
+TEST(OculusDeviceTest, InputConsumer)
+{
+ auto device = std::make_shared<OculusDevice>("Oculus");
+ ASSERT_TRUE(nullptr != device) << "Device creation failed.";
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is an Oculus device plugged in?";
+
+ auto 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.
+ boost::this_thread::sleep_for(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, 800);
+ EXPECT_LE(consumer->m_numTimesReceivedInput, 1200);
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData(SurgSim::DataStructures::Names::POSE));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.matrices().
+ hasData(SurgSim::DataStructures::Names::LEFT_PROJECTION_MATRIX));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.matrices().
+ hasData(SurgSim::DataStructures::Names::RIGHT_PROJECTION_MATRIX));
+}
diff --git a/SurgSim/Devices/Oculus/UnitTests/OculusDisplaySettingsTests.cpp b/SurgSim/Devices/Oculus/UnitTests/OculusDisplaySettingsTests.cpp
new file mode 100644
index 0000000..56d8b14
--- /dev/null
+++ b/SurgSim/Devices/Oculus/UnitTests/OculusDisplaySettingsTests.cpp
@@ -0,0 +1,52 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 <osg/DisplaySettings>
+
+#include "SurgSim/Devices/Oculus/OculusDisplaySettings.h"
+#include "SurgSim/Math/Matrix.h"
+
+namespace
+{
+const double epsilon = 1e-10;
+}
+
+using SurgSim::Devices::OculusDisplaySettings;
+
+TEST(OculusDisplaySettingsTest, Constructor)
+{
+ EXPECT_NO_THROW(OculusDisplaySettings displaySettings);
+ osg::ref_ptr<osg::DisplaySettings> osgDisplaySettings = new osg::DisplaySettings;
+ EXPECT_NO_THROW(OculusDisplaySettings displaySettings(osgDisplaySettings));
+}
+
+TEST(OculusDisplaySettingsTest, SetAndGetProjectionMatrix)
+{
+ OculusDisplaySettings displaySettings;
+ SurgSim::Math::Matrix44d leftProjectionMatrix = SurgSim::Math::Matrix44d::Identity() * 2.0;
+ SurgSim::Math::Matrix44d rightProjectionMatrix = SurgSim::Math::Matrix44d::Identity() * 3.0;
+
+ EXPECT_NO_THROW(displaySettings.setLeftEyeProjectionMatrix(leftProjectionMatrix));
+ EXPECT_NO_THROW(displaySettings.setRightEyeProjectionMatrix(rightProjectionMatrix));
+
+ SurgSim::Math::Matrix44d leftRetrieved;
+ EXPECT_NO_THROW(leftRetrieved = displaySettings.getLeftEyeProjectionMatrix());
+ EXPECT_TRUE(leftProjectionMatrix.isApprox(leftRetrieved, epsilon));
+
+ SurgSim::Math::Matrix44d rightRetrieved;
+ EXPECT_NO_THROW(rightRetrieved = displaySettings.getRightEyeProjectionMatrix());
+ EXPECT_TRUE(rightProjectionMatrix.isApprox(rightRetrieved, epsilon));
+}
diff --git a/SurgSim/Devices/Oculus/UnitTests/OculusViewTests.cpp b/SurgSim/Devices/Oculus/UnitTests/OculusViewTests.cpp
new file mode 100644
index 0000000..c8a63a3
--- /dev/null
+++ b/SurgSim/Devices/Oculus/UnitTests/OculusViewTests.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
+/// Tests for the OculusView class.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Devices/Oculus/OculusView.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Input/InputComponent.h"
+
+using SurgSim::Devices::OculusView;
+
+TEST(OculusViewTests, InitTest)
+{
+ EXPECT_NO_THROW(SurgSim::Devices::OculusView("OculusView"));
+}
+
+TEST(OculusViewTests, SetAndGetInputComponent)
+{
+ auto view = std::make_shared<SurgSim::Devices::OculusView>("OculusView");
+ auto inputComponent = std::make_shared<SurgSim::Input::InputComponent>("InputComponent");
+
+ EXPECT_NO_THROW(view->setInputComponent(inputComponent));
+ EXPECT_EQ(inputComponent, view->getInputComponent());
+}
+
+TEST(OculusViewTests, Serialization)
+{
+ auto view = std::make_shared<SurgSim::Devices::OculusView>("test name");
+ std::shared_ptr<SurgSim::Framework::Component> inputComponent =
+ std::make_shared<SurgSim::Input::InputComponent>("InputComponent");
+ view->setValue("InputComponent", inputComponent);
+
+ /// Serialize
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*view););
+
+ /// Deserialize
+ std::shared_ptr<OculusView> newView;
+ EXPECT_NO_THROW(newView = std::dynamic_pointer_cast<OculusView>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ EXPECT_NE(nullptr, newView);
+ EXPECT_NE(nullptr, newView->getInputComponent());
+ EXPECT_EQ(inputComponent->getName(), newView->getInputComponent()->getName());
+}
diff --git a/SurgSim/Devices/Oculus/VisualTest/CMakeLists.txt b/SurgSim/Devices/Oculus/VisualTest/CMakeLists.txt
new file mode 100644
index 0000000..9a81cd5
--- /dev/null
+++ b/SurgSim/Devices/Oculus/VisualTest/CMakeLists.txt
@@ -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.
+
+set(EXAMPLE_SOURCES
+ main.cpp
+)
+
+set(EXAMPLE_HEADERS
+)
+
+surgsim_add_executable(OculusVisualTest
+ "${EXAMPLE_SOURCES}" "${EXAMPLE_HEADERS}")
+
+set(LIBS
+ IdentityPoseDevice
+ OculusDevice
+ VisualTestCommon
+)
+
+target_link_libraries(OculusVisualTest ${LIBS})
+
+# Put OculusVisualTest into folder "Devices"
+set_target_properties(OculusVisualTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Oculus/VisualTest/main.cpp b/SurgSim/Devices/Oculus/VisualTest/main.cpp
new file mode 100644
index 0000000..7e69f6f
--- /dev/null
+++ b/SurgSim/Devices/Oculus/VisualTest/main.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 <memory>
+
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+#include "SurgSim/Devices/Oculus/OculusDevice.h"
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
+
+using SurgSim::Devices::IdentityPoseDevice;
+using SurgSim::Devices::OculusDevice;
+using SurgSim::Input::DeviceInterface;
+
+int main(int argc, char** argv)
+{
+ auto toolDevice = std::make_shared<OculusDevice>("OculusDevice");
+
+ // 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, "Move the Oculus 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/OpenNI/CMakeLists.txt b/SurgSim/Devices/OpenNI/CMakeLists.txt
new file mode 100644
index 0000000..b81e20a
--- /dev/null
+++ b/SurgSim/Devices/OpenNI/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.
+
+find_package(OpenNI2 REQUIRED)
+
+include_directories(SYSTEM
+ ${OPENNI2_INCLUDE_DIRS}
+)
+
+set(LIBS
+ SurgSimInput
+ ${OPENNI2_LIBRARIES}
+)
+
+set(OPENNI_DEVICE_SOURCES
+ OpenNIDevice.cpp
+ OpenNIScaffold.cpp
+)
+
+set(OPENNI_DEVICE_HEADERS
+ OpenNIDevice.h
+ OpenNIScaffold.h
+)
+
+set(DEVICE_HEADERS ${DEVICE_HEADERS} OpenNI/OpenNIDevice.h PARENT_SCOPE)
+
+surgsim_add_library(
+ OpenNIDevice
+ "${OPENNI_DEVICE_SOURCES}"
+ "${OPENNI_DEVICE_HEADERS}"
+)
+target_link_libraries(OpenNIDevice ${LIBS})
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+
+ if(GLUT_FOUND)
+ add_subdirectory(VisualTest)
+ endif(GLUT_FOUND)
+endif()
+
+set_target_properties(OpenNIDevice PROPERTIES FOLDER "Devices")
+
diff --git a/SurgSim/Devices/OpenNI/OpenNI.dox b/SurgSim/Devices/OpenNI/OpenNI.dox
new file mode 100644
index 0000000..753bc07
--- /dev/null
+++ b/SurgSim/Devices/OpenNI/OpenNI.dox
@@ -0,0 +1,18 @@
+/*!
+
+\page OpenNI OpenNI: Depth Sensing Cameras
+
+This adds support for OpenNI 2 compatible depth sensing cameras, such as the Asus Xtion Pro.
+
+Dependencies
+------------
+OpenNI 2 SDK: Prebuilt binaries are availble for Windows, and GNU/Linux on <A
+HREF="http://structure.io/openni">Structure.io</A>'s website. For Windows, add
+the 'OpenNI2/Redist/' folder to your PATH environment variable. If you used the
+default installation location, this will be:
+
+\code
+ C:/ProgramFiles (x86)/OpenNI2/Redist/
+\endcode
+
+*/
diff --git a/SurgSim/Devices/OpenNI/OpenNIDevice.cpp b/SurgSim/Devices/OpenNI/OpenNIDevice.cpp
new file mode 100644
index 0000000..8e0394b
--- /dev/null
+++ b/SurgSim/Devices/OpenNI/OpenNIDevice.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/OpenNI/OpenNIDevice.h"
+
+#include "SurgSim/Devices/OpenNI/OpenNIScaffold.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::OpenNIDevice, OpenNIDevice);
+
+OpenNIDevice::OpenNIDevice(const std::string& name) :
+ SurgSim::Input::CommonDevice(name, OpenNIScaffold::buildDeviceInputData())
+{
+}
+
+
+OpenNIDevice::~OpenNIDevice()
+{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+
+
+bool OpenNIDevice::initialize()
+{
+ SURGSIM_ASSERT(!isInitialized()) << getName() << " already initialized.";
+ auto scaffold = OpenNIScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold != nullptr);
+
+ bool initialize = false;
+ if (scaffold->registerDevice(this))
+ {
+ m_scaffold = std::move(scaffold);
+ initialize = true;
+ }
+ return initialize;
+}
+
+bool OpenNIDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized()) << getName() << " is not initialized, cannot finalize.";
+ bool success = m_scaffold->unregisterDevice(this);
+ m_scaffold.reset();
+ return success;
+}
+
+bool OpenNIDevice::isInitialized() const
+{
+ return (m_scaffold != nullptr);
+}
+
+
+}; // namespace Devices
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/OpenNI/OpenNIDevice.h b/SurgSim/Devices/OpenNI/OpenNIDevice.h
new file mode 100644
index 0000000..37b7a40
--- /dev/null
+++ b/SurgSim/Devices/OpenNI/OpenNIDevice.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_DEVICES_OPENNI_OPENNIDEVICE_H
+#define SURGSIM_DEVICES_OPENNI_OPENNIDEVICE_H
+
+#include "SurgSim/Input/CommonDevice.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+class OpenNIScaffold;
+
+SURGSIM_STATIC_REGISTRATION(OpenNIDevice);
+
+/// A class implementing the communication with one OpenNI compatible depth camera
+///
+/// \par Application input provided by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | image | "color" | Color image (RGB) of floats, each pixel value is between 0 and 1. |
+/// | image | "depth" | Depth image of floats, each pixel value is depth from the camera in meters.|
+/// | image | "depth_xyz" | Position of each pixel (x, y, z) in meters with respect to the camera. |
+///
+/// \par Application output used by the device: none.
+///
+/// \sa SurgSim::Devices::OpenNIScaffold
+class OpenNIDevice : public SurgSim::Input::CommonDevice
+{
+public:
+ /// Constructor.
+ /// \param name A unique name for the device that will be used by the application.
+ explicit OpenNIDevice(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Devices::OpenNIDevice);
+
+ /// Destructor.
+ virtual ~OpenNIDevice();
+
+ bool initialize() override;
+
+ bool isInitialized() const override;
+
+private:
+ friend class OpenNIScaffold;
+
+ bool finalize() override;
+
+ std::shared_ptr<OpenNIScaffold> m_scaffold;
+};
+
+};
+};
+
+#endif //SURGSIM_DEVICES_OPENNI_OPENNIDEVICE_H
diff --git a/SurgSim/Devices/OpenNI/OpenNIScaffold.cpp b/SurgSim/Devices/OpenNI/OpenNIScaffold.cpp
new file mode 100644
index 0000000..d210958
--- /dev/null
+++ b/SurgSim/Devices/OpenNI/OpenNIScaffold.cpp
@@ -0,0 +1,297 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/OpenNI/OpenNIScaffold.h"
+
+#include <boost/thread/locks.hpp>
+#include <boost/thread/mutex.hpp>
+#include <OpenNI.h>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/OpenNI/OpenNIDevice.h"
+#include "SurgSim/Framework/Barrier.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+struct OpenNIScaffold::DeviceData
+{
+ explicit DeviceData(OpenNIDevice* device) :
+ deviceObject(device)
+ {
+ }
+
+ /// OpenNI device
+ openni::Device camera;
+ /// OpenNI Depth Video Stream
+ openni::VideoStream depthStream;
+ /// OpenNI RGB Image Video Stream
+ openni::VideoStream colorStream;
+ /// The corresponding device object.
+ OpenNIDevice* const deviceObject;
+};
+
+struct OpenNIScaffold::StateData
+{
+ /// The list of known devices.
+ std::list<std::unique_ptr<DeviceData>> activeDevices;
+
+ /// The mutex that protects the list of known devices.
+ boost::mutex mutex;
+};
+
+OpenNIScaffold::OpenNIScaffold() :
+ SurgSim::Framework::BasicThread("OpenNI Scaffold"),
+ m_state(new StateData),
+ m_logger(SurgSim::Framework::Logger::getLogger("OpenNI"))
+{
+ setRate(100);
+}
+
+OpenNIScaffold::~OpenNIScaffold()
+{
+ openni::OpenNI::shutdown();
+}
+
+bool OpenNIScaffold::registerDevice(OpenNIDevice* device)
+{
+ bool success = true;
+ if (!isRunning())
+ {
+ std::shared_ptr<SurgSim::Framework::Barrier> barrier = std::make_shared<SurgSim::Framework::Barrier>(2);
+ start(barrier);
+ barrier->wait(true); // Wait for initialize
+ barrier->wait(true); // Wait for startup
+ success = isInitialized();
+ }
+
+ if (success)
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+ const std::string deviceName = device->getName();
+ auto sameName = [&deviceName](const std::unique_ptr<DeviceData>& info)
+ {
+ return info->deviceObject->getName() == deviceName;
+ };
+ auto found = std::find_if(m_state->activeDevices.cbegin(), m_state->activeDevices.cend(), sameName);
+
+ if (found == m_state->activeDevices.end())
+ {
+ std::unique_ptr<DeviceData> info(new DeviceData(device));
+ success = doRegisterDevice(info.get());
+ if (success)
+ {
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << ": Registered";
+ m_state->activeDevices.emplace_back(std::move(info));
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Tried to register a device when the same name, '" <<
+ device->getName() << "', is already present!";
+ success = false;
+ }
+ }
+
+ return success;
+}
+
+bool OpenNIScaffold::doRegisterDevice(DeviceData* info)
+{
+ openni::Status status;
+
+ status = info->camera.open(openni::ANY_DEVICE);
+ if (status != openni::STATUS_OK)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Could not connect to a camera: " << openni::OpenNI::getExtendedError();
+ return false;
+ }
+
+ status = info->depthStream.create(info->camera, openni::SENSOR_DEPTH);
+ if (status != openni::STATUS_OK)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Could not find depth stream: " << openni::OpenNI::getExtendedError();
+ return false;
+ }
+
+ status = info->colorStream.create(info->camera, openni::SENSOR_COLOR);
+ if (status != openni::STATUS_OK)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Could find color image stream: " << openni::OpenNI::getExtendedError();
+ return false;
+ }
+
+ status = info->depthStream.start();
+ if (status != openni::STATUS_OK || !info->depthStream.isValid())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Unable to start depth stream: " << openni::OpenNI::getExtendedError();
+ return false;
+ }
+
+ status = info->colorStream.start();
+ if (status != openni::STATUS_OK || !info->colorStream.isValid())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Unable to start color image stream: " << openni::OpenNI::getExtendedError();
+ return false;
+ }
+ return true;
+}
+
+bool OpenNIScaffold::unregisterDevice(const OpenNIDevice* device)
+{
+ bool success = true;
+ {
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ auto sameDevice = [device](const std::unique_ptr<DeviceData>& info)
+ {
+ return info->deviceObject == device;
+ };
+ auto info = std::find_if(m_state->activeDevices.begin(), m_state->activeDevices.end(), sameDevice);
+ if (info != m_state->activeDevices.end())
+ {
+ success = doUnregisterDevice((*info).get());
+ if (success)
+ {
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << ": Unregistered";
+ m_state->activeDevices.erase(info);
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Attempted to release a non-registered device named '" <<
+ device->getName() << ".";
+ success = false;
+ }
+ }
+
+ if (success && isRunning() && m_state->activeDevices.size() == 0)
+ {
+ stop();
+ }
+
+ return success;
+}
+
+bool OpenNIScaffold::doUnregisterDevice(DeviceData* info)
+{
+ info->depthStream.destroy();
+ info->colorStream.destroy();
+ info->camera.close();
+ return true;
+}
+
+bool OpenNIScaffold::doInitialize()
+{
+ openni::Status status = openni::OpenNI::initialize();
+ bool success = (status == openni::STATUS_OK);
+ SURGSIM_LOG_IF(!success, m_logger, SEVERE) << "Initialize failed: " << openni::OpenNI::getExtendedError();
+ return success;
+}
+
+bool OpenNIScaffold::doStartUp()
+{
+ return true;
+}
+
+bool OpenNIScaffold::doUpdate(double dt)
+{
+ boost::lock_guard<boost::mutex> lock(m_state->mutex);
+
+ openni::VideoFrameRef depthFrame;
+ openni::VideoFrameRef colorFrame;
+ typedef SurgSim::DataStructures::DataGroup::ImageType ImageType;
+
+ for (auto info = m_state->activeDevices.begin(); info != m_state->activeDevices.end(); ++info)
+ {
+ SurgSim::DataStructures::DataGroup& inputData = (*info)->deviceObject->getInputData();
+
+ if ((*info)->depthStream.readFrame(&depthFrame) == openni::STATUS_OK)
+ {
+ size_t width = depthFrame.getWidth();
+ size_t height = depthFrame.getHeight();
+ typedef Eigen::Map<const Eigen::Matrix<uint16_t, Eigen::Dynamic, Eigen::Dynamic>> DepthDataType;
+ DepthDataType depthData(reinterpret_cast<const uint16_t*>(depthFrame.getData()), width, height);
+
+ ImageType depth(width, height, 1);
+ depth.getAsVector() = depthData.cast<float>() * (1.0f / 1000.0f); // convert milimeters to meters
+ inputData.images().set("depth", std::move(depth));
+
+ ImageType depth_xyz(width, height, 3);
+ auto x = depth_xyz.getChannel(0);
+ auto y = depth_xyz.getChannel(1);
+ auto z = depth_xyz.getChannel(2);
+ for (size_t i = 0; i < width; i++)
+ {
+ for (size_t j = 0; j < height; j++)
+ {
+ openni::CoordinateConverter::convertDepthToWorld((*info)->depthStream, i, j, depthData(i, j),
+ &x(i, j), &y(i, j), &z(i, j));
+ }
+ }
+ depth_xyz.getAsVector() *= (1.0f / 1000.0f); // convert milimeters to meters
+ inputData.images().set("depth_xyz", std::move(depth_xyz));
+ }
+ else
+ {
+ inputData.images().reset("depth");
+ inputData.images().reset("depth_xyz");
+ }
+
+ if ((*info)->colorStream.readFrame(&colorFrame) == openni::STATUS_OK)
+ {
+ ImageType image(colorFrame.getWidth(), colorFrame.getHeight(), 3,
+ reinterpret_cast<const uint8_t*>(colorFrame.getData()));
+ image.getAsVector() *= (1.0f / 255.0f); // OpenNI returns colors between 0 and 255, scale values to 0..1
+ inputData.images().set("color", std::move(image));
+ }
+ else
+ {
+ inputData.images().reset("color");
+ }
+
+ (*info)->deviceObject->pushInput();
+ }
+ return true;
+}
+
+std::shared_ptr<OpenNIScaffold> OpenNIScaffold::getOrCreateSharedInstance()
+{
+ static auto creator = []()
+ {
+ return std::shared_ptr<OpenNIScaffold>(new OpenNIScaffold);
+ };
+ static SurgSim::Framework::SharedInstance<OpenNIScaffold> sharedInstance(creator);
+ return sharedInstance.get();
+}
+
+SurgSim::DataStructures::DataGroup OpenNIScaffold::buildDeviceInputData()
+{
+ SurgSim::DataStructures::DataGroupBuilder builder;
+ builder.addImage("color");
+ builder.addImage("depth");
+ builder.addImage("depth_xyz");
+ return builder.createData();
+}
+
+
+}; // namespace Devices
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/OpenNI/OpenNIScaffold.h b/SurgSim/Devices/OpenNI/OpenNIScaffold.h
new file mode 100644
index 0000000..96fa16d
--- /dev/null
+++ b/SurgSim/Devices/OpenNI/OpenNIScaffold.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_DEVICES_OPENNI_OPENNISCAFFOLD_H
+#define SURGSIM_DEVICES_OPENNI_OPENNISCAFFOLD_H
+
+#include <memory>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Framework/BasicThread.h"
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+class Logger;
+};
+
+namespace Devices
+{
+
+class OpenNIDevice;
+
+/// A class that manages OpenNI devices
+///
+/// \sa SurgSim::Devices::OpenNIDevice
+class OpenNIScaffold : public SurgSim::Framework::BasicThread
+{
+public:
+ /// Destructor.
+ virtual ~OpenNIScaffold();
+
+protected:
+ bool doInitialize() override;
+
+ bool doStartUp() override;
+
+ bool doUpdate(double dt) override;
+
+private:
+ /// Internal shared state data type.
+ struct StateData;
+
+ /// Interal per-device information.
+ struct DeviceData;
+
+ friend class OpenNIDevice;
+
+ /// Constructor.
+ OpenNIScaffold();
+
+ /// Gets or creates the scaffold shared by all OpenNIDevice instances.
+ /// The scaffold is managed using a SharedInstance object, so it will be destroyed when all devices are released.
+ /// \return the scaffold object.
+ static std::shared_ptr<OpenNIScaffold> getOrCreateSharedInstance();
+
+ /// Registers the specified device object.
+ /// \param device The device object to be used
+ /// \return True if the initialization succeeds, false if it fails.
+ bool registerDevice(OpenNIDevice* device);
+
+ /// Unregisters the specified device object.
+ /// \param device The device object.
+ /// \return true on success, false on failure.
+ bool unregisterDevice(const OpenNIDevice* device);
+
+ /// Do the OpenNI specific registration
+ /// \param info The device data
+ /// \return true on success, false on failure.
+ bool doRegisterDevice(DeviceData* info);
+
+ /// Do the OpenNI specific unregistration
+ /// \param info The device data
+ /// \return true on success, false on failure.
+ bool doUnregisterDevice(DeviceData* info);
+
+ /// Builds the data layout for the application input (i.e. device output).
+ static SurgSim::DataStructures::DataGroup buildDeviceInputData();
+
+ /// Internal scaffold state.
+ std::unique_ptr<StateData> m_state;
+
+ /// Logger used by the scaffold
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+};
+
+}; // namespace Devices
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_OPENNI_OPENNISCAFFOLD_H
diff --git a/SurgSim/Devices/OpenNI/UnitTests/CMakeLists.txt b/SurgSim/Devices/OpenNI/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..3b27287
--- /dev/null
+++ b/SurgSim/Devices/OpenNI/UnitTests/CMakeLists.txt
@@ -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.
+
+
+include_directories(
+ ${gtest_SOURCE_DIR}/include
+)
+
+set(UNIT_TEST_SOURCES
+ OpenNIDeviceTest.cpp
+)
+
+set(LIBS
+ OpenNIDevice
+)
+
+surgsim_add_unit_tests(OpenNIDeviceTest)
+
+set_target_properties(OpenNIDeviceTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/OpenNI/UnitTests/OpenNIDeviceTest.cpp b/SurgSim/Devices/OpenNI/UnitTests/OpenNIDeviceTest.cpp
new file mode 100644
index 0000000..03be70a
--- /dev/null
+++ b/SurgSim/Devices/OpenNI/UnitTests/OpenNIDeviceTest.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.
+
+/// \file
+/// Tests for the OpenNIDevice class.
+
+#include <boost/thread.hpp>
+#include <boost/chrono.hpp>
+#include <gtest/gtest.h>
+#include <memory>
+#include <string>
+
+#include "SurgSim/Devices/OpenNI/OpenNIDevice.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::Devices::OpenNIDevice;
+using SurgSim::Devices::OpenNIScaffold;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Testing::MockInputOutput;
+
+TEST(OpenNIDeviceTest, CreateUninitializedDevice)
+{
+ std::shared_ptr<OpenNIDevice> device = std::make_shared<OpenNIDevice>("TestOpenNI");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+}
+
+TEST(OpenNIDeviceTest, CreateAndInitializeDevice)
+{
+ std::shared_ptr<OpenNIDevice> device = std::make_shared<OpenNIDevice>("TestOpenNI");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_FALSE(device->isInitialized());
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a OpenNI device plugged in?";
+ EXPECT_TRUE(device->isInitialized());
+}
+
+TEST(OpenNIDeviceTest, Name)
+{
+ std::shared_ptr<OpenNIDevice> device = std::make_shared<OpenNIDevice>("TestOpenNI");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ EXPECT_EQ("TestOpenNI", device->getName());
+ EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a OpenNI device plugged in?";
+ EXPECT_EQ("TestOpenNI", device->getName());
+}
+
+TEST(OpenNIDeviceTest, CreateDevicesWithSameName)
+{
+ std::shared_ptr<OpenNIDevice> device1 = std::make_shared<OpenNIDevice>("OpenNI");
+ ASSERT_TRUE(device1 != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device1->initialize()) << "Initialization failed. Is a OpenNI device plugged in?";
+
+ std::shared_ptr<OpenNIDevice> device2 = std::make_shared<OpenNIDevice>("OpenNI");
+ ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
+ ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate name.";
+}
+
+TEST(OpenNIDeviceTest, InputConsumer)
+{
+ std::shared_ptr<OpenNIDevice> device = std::make_shared<OpenNIDevice>("TestOpenNI");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a OpenNI 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.
+ 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, 10);
+ EXPECT_LE(consumer->m_numTimesReceivedInput, 120);
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.images().hasData("color"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.images().hasData("depth"));
+ EXPECT_TRUE(consumer->m_lastReceivedInput.images().hasData("depth_xyz"));
+}
+
+TEST(OpenNIDeviceTest, OutputProducer)
+{
+ std::shared_ptr<OpenNIDevice> device = std::make_shared<OpenNIDevice>("TestOpenNI");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a OpenNI 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.
+ 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/OpenNI/VisualTest/CMakeLists.txt b/SurgSim/Devices/OpenNI/VisualTest/CMakeLists.txt
new file mode 100644
index 0000000..77d9669
--- /dev/null
+++ b/SurgSim/Devices/OpenNI/VisualTest/CMakeLists.txt
@@ -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_directories(
+ "${GLUT_INCLUDE_DIR}"
+)
+
+set(EXAMPLE_SOURCES
+ main.cpp
+)
+
+set(EXAMPLE_HEADERS
+)
+
+surgsim_add_executable(OpenNIVisualTest
+ "${EXAMPLE_SOURCES}"
+ "${EXAMPLE_HEADERS}"
+)
+
+set(LIBS
+ OpenNIDevice
+ SurgSimDataStructures
+ SurgSimFramework
+ SurgSimInput
+ SurgSimMath
+ VisualTestCommon
+ ${Boost_LIBRARIES}
+ ${GLUT_LIBRARIES}
+ ${OPENGL_LIBRARIES}
+)
+
+target_link_libraries(OpenNIVisualTest ${LIBS})
+
+set_target_properties(OpenNIVisualTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/OpenNI/VisualTest/main.cpp b/SurgSim/Devices/OpenNI/VisualTest/main.cpp
new file mode 100644
index 0000000..c127052
--- /dev/null
+++ b/SurgSim/Devices/OpenNI/VisualTest/main.cpp
@@ -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.
+
+#include <boost/thread.hpp>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/Image.h"
+#include "SurgSim/Devices/OpenNI/OpenNIDevice.h"
+#include "SurgSim/Framework/LockedContainer.h"
+#include "SurgSim/Input/DeviceInterface.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Testing/VisualTestCommon/GlutRenderer.h"
+
+using SurgSim::Devices::OpenNIDevice;
+using SurgSim::Input::DeviceInterface;
+using SurgSim::Math::Vector3d;
+
+
+class ImageGlutWindow : public SurgSim::Input::InputConsumerInterface
+{
+public:
+ explicit ImageGlutWindow(const std::vector<std::string>& imageNames)
+ {
+ const size_t numImages = imageNames.size();
+ const SurgSim::Math::Vector2d yRange(-1.0, 1.0);
+ SurgSim::Math::Vector2d xRange;
+ for (size_t i = 0; i < numImages; i++)
+ {
+ xRange = SurgSim::Math::Vector2d(i - numImages, i - numImages + 1) / numImages;
+ auto view = std::make_shared<GlutImage>(Eigen::AlignedBox<double, 2>(xRange, yRange));
+ GlutRenderer::addObject(view);
+ m_views.insert(std::make_pair(imageNames[i], view));
+ }
+ m_renderThread = boost::thread(boost::ref(GlutRenderer::run));
+ }
+
+ ~ImageGlutWindow()
+ {
+ if (m_renderThread.joinable())
+ {
+ m_renderThread.join();
+ }
+ }
+
+ void initializeInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override
+ {
+ for (auto view : m_views)
+ {
+ SURGSIM_ASSERT(inputData.images().hasEntry(view.first))
+ << "No image named '" << view.first << "' provided by the device: " << device;
+ }
+ }
+
+ void handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override
+ {
+ SurgSim::DataStructures::DataGroup::ImageType data;
+ for (auto view : m_views)
+ {
+ if(inputData.images().get(view.first, &data))
+ {
+ view.second->image.set(std::move(data));
+ }
+ }
+ }
+
+private:
+ boost::thread m_renderThread;
+ std::unordered_map<std::string, std::shared_ptr<GlutImage>> m_views;
+};
+
+int main(int argc, char** argv)
+{
+ std::shared_ptr<DeviceInterface> device = std::make_shared<OpenNIDevice>("OpenNIDevice");
+ if (!device->initialize())
+ {
+ std::cout << std::endl << "Could not initialize device: " << device->getName()
+ << std::endl << "--- Press Enter to quit the application! ---" << std::endl;
+ getc(stdin);
+ return 1;
+ }
+
+ std::vector<std::string> imagesToDraw;
+ imagesToDraw.push_back("color");
+ imagesToDraw.push_back("depth");
+ std::shared_ptr<ImageGlutWindow> imageWindow = std::make_shared<ImageGlutWindow>(imagesToDraw);
+ device->addInputConsumer(imageWindow);
+
+ std::cout << std::endl << "**********************************************************************" << std::endl
+ << "OpenNI Visual Test" << std::endl << std::endl
+ << "When done, press Enter to quit the application." << std::endl
+ << "**********************************************************************" << std::endl;
+ getc(stdin);
+
+ std::cout << "Exiting..." << std::endl;
+ device->removeInputConsumer(imageWindow);
+ device.reset();
+ exit(0);
+}
diff --git a/SurgSim/Devices/Phantom/CMakeLists.txt b/SurgSim/Devices/Phantom/CMakeLists.txt
index 4b28fe2..0fbbf3b 100644
--- a/SurgSim/Devices/Phantom/CMakeLists.txt
+++ b/SurgSim/Devices/Phantom/CMakeLists.txt
@@ -40,11 +40,13 @@ set(PHANTOM_DEVICE_HEADERS
PhantomScaffold.h
)
+set(DEVICE_HEADERS ${DEVICE_HEADERS} Phantom/PhantomDevice.h PARENT_SCOPE)
+
# TODO(advornik): the installation should NOT copy all the headers...
surgsim_add_library(
PhantomDevice
- "${PHANTOM_DEVICE_SOURCES}" "${PHANTOM_DEVICE_HEADERS}"
- SurgSim/Devices/Phantom
+ "${PHANTOM_DEVICE_SOURCES}"
+ "${PHANTOM_DEVICE_HEADERS}"
)
target_link_libraries(PhantomDevice ${LIBS})
diff --git a/SurgSim/Devices/Phantom/Phantom.dox b/SurgSim/Devices/Phantom/Phantom.dox
index c45a3c1..77f75cb 100644
--- a/SurgSim/Devices/Phantom/Phantom.dox
+++ b/SurgSim/Devices/Phantom/Phantom.dox
@@ -10,5 +10,6 @@ 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>
+ - The location of the hd.lib file must be added to your path, e.g., <tt>C:\\Program Files (x86)\\SensAble\\3DTouch\\lib\\win32</tt>
*/
\ No newline at end of file
diff --git a/SurgSim/Devices/Phantom/PhantomDevice.cpp b/SurgSim/Devices/Phantom/PhantomDevice.cpp
index b1ec74b..c80d19a 100644
--- a/SurgSim/Devices/Phantom/PhantomDevice.cpp
+++ b/SurgSim/Devices/Phantom/PhantomDevice.cpp
@@ -20,14 +20,16 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::PhantomDevice, PhantomDevice);
-PhantomDevice::PhantomDevice(const std::string& uniqueName, const std::string& initializationName) :
- SurgSim::Input::CommonDevice(uniqueName, PhantomScaffold::buildDeviceInputData()),
- m_initializationName(initializationName)
+PhantomDevice::PhantomDevice(const std::string& uniqueName) :
+ SurgSim::Input::CommonDevice(uniqueName, PhantomScaffold::buildDeviceInputData())
{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(PhantomDevice, std::string, InitializationName,
+ getInitializationName, setInitializationName);
}
@@ -39,6 +41,10 @@ PhantomDevice::~PhantomDevice()
}
}
+void PhantomDevice::setInitializationName(const std::string& initializationName)
+{
+ m_initializationName = initializationName;
+}
std::string PhantomDevice::getInitializationName() const
{
@@ -49,24 +55,22 @@ std::string PhantomDevice::getInitializationName() const
bool PhantomDevice::initialize()
{
SURGSIM_ASSERT(! isInitialized());
- std::shared_ptr<PhantomScaffold> scaffold = PhantomScaffold::getOrCreateSharedInstance();
+ auto scaffold = PhantomScaffold::getOrCreateSharedInstance();
SURGSIM_ASSERT(scaffold);
- if (! scaffold->registerDevice(this))
+ bool initialize = false;
+ if (scaffold->registerDevice(this))
{
- return false;
+ m_scaffold = std::move(scaffold);
+ initialize = true;
}
-
- m_scaffold = std::move(scaffold);
- SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Initialized.";
- return true;
+ return initialize;
}
bool PhantomDevice::finalize()
{
- SURGSIM_ASSERT(isInitialized());
- SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Finalizing.";
+ SURGSIM_ASSERT(isInitialized()) << getName() << " is not initialized, cannot finalize.";
bool ok = m_scaffold->unregisterDevice(this);
m_scaffold.reset();
return ok;
@@ -79,5 +83,5 @@ bool PhantomDevice::isInitialized() const
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/Phantom/PhantomDevice.h b/SurgSim/Devices/Phantom/PhantomDevice.h
index cc61915..654fd62 100644
--- a/SurgSim/Devices/Phantom/PhantomDevice.h
+++ b/SurgSim/Devices/Phantom/PhantomDevice.h
@@ -23,10 +23,11 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
class PhantomScaffold;
+SURGSIM_STATIC_REGISTRATION(PhantomDevice);
/// A class implementing the communication with a SensAble/Geomagic PHANTOM device.
///
@@ -60,34 +61,38 @@ 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);
+ explicit PhantomDevice(const std::string& uniqueName);
+
+ SURGSIM_CLASSNAME(SurgSim::Devices::PhantomDevice);
/// Destructor.
virtual ~PhantomDevice();
+ /// Sets the name used to register this device with the hardware library.
+ /// \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.
+ void setInitializationName(const std::string& initializationName);
+
/// 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;
+ bool initialize() override;
- virtual bool finalize() override;
-
- /// Check whether this device is initialized.
- bool isInitialized() const;
+ bool isInitialized() const override;
private:
friend class PhantomScaffold;
+ bool finalize() override;
+
std::shared_ptr<PhantomScaffold> m_scaffold;
std::string m_initializationName;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_PHANTOM_PHANTOMDEVICE_H
diff --git a/SurgSim/Devices/Phantom/PhantomScaffold.cpp b/SurgSim/Devices/Phantom/PhantomScaffold.cpp
index 91ab750..da03de8 100644
--- a/SurgSim/Devices/Phantom/PhantomScaffold.cpp
+++ b/SurgSim/Devices/Phantom/PhantomScaffold.cpp
@@ -41,9 +41,41 @@ using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Vector3d;
+namespace
+{
+/// 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)
+{
+ 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(SurgSim::Framework::Logger::getLogger("Devices/Phantom")) << "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);
+}
+}
+
namespace SurgSim
{
-namespace Device
+namespace Devices
{
@@ -52,20 +84,20 @@ class PhantomScaffold::Handle
public:
Handle() :
m_deviceHandle(HD_INVALID_HANDLE),
- m_scaffold(PhantomScaffold::getOrCreateSharedInstance())
+ m_logger(Framework::Logger::getLogger("Devices/Phantom"))
{
}
Handle(const std::string& deviceName, const std::string& initializationName) :
m_deviceHandle(HD_INVALID_HANDLE),
- m_scaffold(PhantomScaffold::getOrCreateSharedInstance())
+ m_logger(Framework::Logger::getLogger("Devices/Phantom"))
{
create(deviceName, initializationName);
}
~Handle()
{
- SURGSIM_ASSERT(! isValid()) << "Expected destroy() to be called before Handle object destruction.";
+ SURGSIM_ASSERT(!isValid()) << "Expected destroy() to be called before Handle object destruction.";
}
bool isValid() const
@@ -75,7 +107,7 @@ public:
bool create(const std::string& deviceName, const std::string& initializationName)
{
- SURGSIM_ASSERT(! isValid());
+ SURGSIM_ASSERT(!isValid());
HHD deviceHandle = HD_INVALID_HANDLE;
if (initializationName.length() > 0)
@@ -87,18 +119,18 @@ public:
deviceHandle = hdInitDevice(HD_DEFAULT_DEVICE);
}
- if (m_scaffold->checkForFatalError("Failed to initialize"))
+ if (checkForFatalError("Failed to initialize"))
{
// HDAPI error message already logged
- SURGSIM_LOG_INFO(m_scaffold->getLogger()) << std::endl <<
+ SURGSIM_LOG_INFO(m_logger) << 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 <<
+ SURGSIM_LOG_SEVERE(m_logger) <<
+ "Failed to initialize '" << deviceName << "'" << std::endl <<
" Error details: unknown (HDAPI returned an invalid handle)" << std::endl <<
" OpenHaptics device name: '" << initializationName << "'" << std::endl;
return false;
@@ -120,7 +152,7 @@ public:
m_deviceHandle = HD_INVALID_HANDLE;
hdDisableDevice(deviceHandle);
- m_scaffold->checkForFatalError("Couldn't disable device");
+ checkForFatalError("Couldn't disable device");
return true;
}
@@ -137,8 +169,8 @@ private:
/// The OpenHaptics device handle (or HD_INVALID_HANDLE if not valid).
HHD m_deviceHandle;
- /// The scaffold.
- std::shared_ptr<PhantomScaffold> m_scaffold;
+ /// The logger.
+ std::shared_ptr<Framework::Logger> m_logger;
};
@@ -170,7 +202,7 @@ public:
{
SURGSIM_ASSERT(! m_haveCallback);
m_callbackHandle = hdScheduleAsynchronous(run, m_scaffold.get(), HD_DEFAULT_SCHEDULER_PRIORITY);
- if (m_scaffold->checkForFatalError("Couldn't run haptic callback"))
+ if (checkForFatalError("Couldn't run haptic callback"))
{
return false;
}
@@ -182,7 +214,7 @@ public:
{
SURGSIM_ASSERT(m_haveCallback);
hdUnschedule(m_callbackHandle);
- if (m_scaffold->checkForFatalError("Couldn't stop haptic callback"))
+ if (checkForFatalError("Couldn't stop haptic callback"))
{
return false;
}
@@ -259,13 +291,10 @@ struct PhantomScaffold::StateData
{
public:
/// Initialize the state.
- StateData() : isApiInitialized(false)
+ StateData()
{
}
- /// 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;
@@ -296,15 +325,9 @@ HDCallbackCode HDCALLBACK PhantomScaffold::Callback::run(void* data)
-PhantomScaffold::PhantomScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) :
- m_logger(logger), m_state(new StateData)
+PhantomScaffold::PhantomScaffold() :
+ m_logger(Framework::Logger::getLogger("Devices/Phantom")), 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();
@@ -314,7 +337,7 @@ PhantomScaffold::PhantomScaffold(std::shared_ptr<SurgSim::Framework::Logger> log
}
}
- SURGSIM_LOG_DEBUG(m_logger) << "Phantom: Shared scaffold created.";
+ SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold created.";
}
@@ -334,28 +357,14 @@ PhantomScaffold::~PhantomScaffold()
// do anything special with each device?
m_state->activeDeviceList.clear();
}
-
- if (m_state->isApiInitialized)
- {
- finalizeSdk();
- }
}
- SURGSIM_LOG_DEBUG(m_logger) << "Phantom: Shared scaffold destroyed.";
+ SURGSIM_LOG_DEBUG(m_logger) << "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; });
@@ -394,6 +403,7 @@ bool PhantomScaffold::registerDevice(PhantomDevice* device)
return false; // message already printed
}
m_state->activeDeviceList.emplace_back(std::move(info));
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << " initialized.";
if (m_state->activeDeviceList.size() == 1)
{
@@ -418,6 +428,7 @@ bool PhantomScaffold::unregisterDevice(const PhantomDevice* const device)
savedInfo = std::move(*matching);
m_state->activeDeviceList.erase(matching);
// the iterator is now invalid but that's OK
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << " unregistered.";
}
haveOtherDevices = (m_state->activeDeviceList.size() > 0);
}
@@ -425,7 +436,7 @@ bool PhantomScaffold::unregisterDevice(const PhantomDevice* const device)
bool status = true;
if (! savedInfo)
{
- SURGSIM_LOG_WARNING(m_logger) << "Phantom: Attempted to release a non-registered device.";
+ SURGSIM_LOG_WARNING(m_logger) << "Attempted to release a non-registered device " << device->getName();
status = false;
}
else
@@ -488,6 +499,7 @@ bool PhantomScaffold::updateDevice(PhantomScaffold::DeviceData* info)
info->scaledPose.translation() = info->position * 0.001; // convert from millimeters to meters!
hdGetDoublev(HD_CURRENT_VELOCITY, info->linearVelocity.data());
+ info->linearVelocity *= 0.001;
//TODO(ryanbeasley): convert HD_CURRENT_ANGULAR_VELOCITY to a rotation vector and store in info->angularVelocity.
Eigen::Matrix<double, 4, 4, Eigen::ColMajor> transform;
@@ -590,29 +602,6 @@ void PhantomScaffold::setInputData(DeviceData* info)
(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);
@@ -687,33 +676,6 @@ bool PhantomScaffold::stopScheduler()
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;
@@ -732,13 +694,5 @@ std::shared_ptr<PhantomScaffold> PhantomScaffold::getOrCreateSharedInstance()
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 Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/Phantom/PhantomScaffold.h b/SurgSim/Devices/Phantom/PhantomScaffold.h
index a3ae215..b0d8069 100644
--- a/SurgSim/Devices/Phantom/PhantomScaffold.h
+++ b/SurgSim/Devices/Phantom/PhantomScaffold.h
@@ -23,7 +23,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
class PhantomDevice;
@@ -34,37 +34,22 @@ class PhantomDevice;
/// 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
+/// \sa SurgSim::Devices::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);
+ PhantomScaffold();
/// 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.
+ /// The scaffold is managed using a SharedInstance 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.
@@ -120,14 +105,6 @@ private:
/// \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.
@@ -150,26 +127,16 @@ private:
/// \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 Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_PHANTOM_PHANTOMSCAFFOLD_H
diff --git a/SurgSim/Devices/Phantom/UnitTests/PhantomDeviceTest.cpp b/SurgSim/Devices/Phantom/UnitTests/PhantomDeviceTest.cpp
index ab41c8d..8918c97 100644
--- a/SurgSim/Devices/Phantom/UnitTests/PhantomDeviceTest.cpp
+++ b/SurgSim/Devices/Phantom/UnitTests/PhantomDeviceTest.cpp
@@ -28,8 +28,8 @@
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Testing/MockInputOutput.h"
-using SurgSim::Device::PhantomDevice;
-using SurgSim::Device::PhantomScaffold;
+using SurgSim::Devices::PhantomDevice;
+using SurgSim::Devices::PhantomScaffold;
using SurgSim::DataStructures::DataGroup;
using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Matrix44d;
@@ -38,14 +38,15 @@ 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");
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom");
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");
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom");
+ device->setInitializationName("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?";
@@ -55,7 +56,7 @@ TEST(PhantomDeviceTest, CreateAndInitializeDevice)
TEST(PhantomDeviceTest, CreateAndInitializeDefaultDevice)
{
//PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
- std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "");
+ 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?";
@@ -65,7 +66,8 @@ TEST(PhantomDeviceTest, CreateAndInitializeDefaultDevice)
TEST(PhantomDeviceTest, Name)
{
//PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
- std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom");
+ device->setInitializationName("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?";
@@ -76,7 +78,8 @@ static void testCreateDeviceSeveralTimes(bool doSleep)
{
for (int i = 0; i < 6; ++i)
{
- std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom");
+ device->setInitializationName("Default PHANToM");
ASSERT_TRUE(device != nullptr) << "Device creation failed.";
ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
if (doSleep)
@@ -96,13 +99,15 @@ TEST(PhantomDeviceTest, CreateDeviceSeveralTimes)
TEST(PhantomDeviceTest, CreateSeveralDevices)
{
//PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
- std::shared_ptr<PhantomDevice> device1 = std::make_shared<PhantomDevice>("Phantom1", "Default PHANToM");
+ std::shared_ptr<PhantomDevice> device1 = std::make_shared<PhantomDevice>("Phantom1");
+ device1->setInitializationName("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");
+ std::shared_ptr<PhantomDevice> device2 = std::make_shared<PhantomDevice>("Phantom2");
+ device2->setInitializationName("Second PHANToM");
ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
if (! device2->initialize())
{
@@ -113,11 +118,13 @@ TEST(PhantomDeviceTest, CreateSeveralDevices)
TEST(PhantomDeviceTest, CreateDevicesWithSameName)
{
//PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
- std::shared_ptr<PhantomDevice> device1 = std::make_shared<PhantomDevice>("Phantom", "Default PHANToM");
+ std::shared_ptr<PhantomDevice> device1 = std::make_shared<PhantomDevice>("Phantom");
+ device1->setInitializationName("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");
+ std::shared_ptr<PhantomDevice> device2 = std::make_shared<PhantomDevice>("Phantom");
+ device2->setInitializationName("Second PHANToM");
ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate name.";
}
@@ -125,11 +132,13 @@ TEST(PhantomDeviceTest, CreateDevicesWithSameName)
TEST(PhantomDeviceTest, CreateDevicesWithSameInitializationName)
{
//PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
- std::shared_ptr<PhantomDevice> device1 = std::make_shared<PhantomDevice>("Phantom1", "Default PHANToM");
+ std::shared_ptr<PhantomDevice> device1 = std::make_shared<PhantomDevice>("Phantom1");
+ device1->setInitializationName("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");
+ std::shared_ptr<PhantomDevice> device2 = std::make_shared<PhantomDevice>("Phantom2");
+ device2->setInitializationName("Default PHANToM");
ASSERT_TRUE(device2 != nullptr) << "Device creation failed.";
ASSERT_FALSE(device2->initialize()) << "Initialization succeeded despite duplicate initialization name.";
}
@@ -137,7 +146,8 @@ TEST(PhantomDeviceTest, CreateDevicesWithSameInitializationName)
TEST(PhantomDeviceTest, InputConsumer)
{
//PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
- std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom");
+ device->setInitializationName("Default PHANToM");
ASSERT_TRUE(device != nullptr) << "Device creation failed.";
EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
@@ -172,7 +182,8 @@ TEST(PhantomDeviceTest, InputConsumer)
TEST(PhantomDeviceTest, OutputProducer)
{
//PhantomScaffold::setDefaultLogLevel(SurgSim::Framework::LOG_LEVEL_DEBUG);
- std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom");
+ device->setInitializationName("Default PHANToM");
ASSERT_TRUE(device != nullptr) << "Device creation failed.";
EXPECT_TRUE(device->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
@@ -197,3 +208,21 @@ TEST(PhantomDeviceTest, OutputProducer)
EXPECT_GE(producer->m_numTimesRequestedOutput, 700);
EXPECT_LE(producer->m_numTimesRequestedOutput, 1300);
}
+
+
+TEST(PhantomDeviceTest, FactoryCreation)
+{
+ std::shared_ptr<SurgSim::Input::DeviceInterface> device;
+ ASSERT_NO_THROW(device = SurgSim::Input::DeviceInterface::getFactory().create("SurgSim::Devices::PhantomDevice",
+ "TestPhantom"));
+ EXPECT_EQ("SurgSim::Devices::PhantomDevice", device->getClassName());
+}
+
+TEST(PhantomDeviceTest, AccessibleTest)
+{
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestFalcon");
+
+ std::string name = "initName";
+ EXPECT_NO_THROW(device->setValue("InitializationName", name));
+ EXPECT_EQ(name, device->getInitializationName());
+}
diff --git a/SurgSim/Devices/Phantom/UnitTests/PhantomScaffoldTest.cpp b/SurgSim/Devices/Phantom/UnitTests/PhantomScaffoldTest.cpp
index 488b726..758a65e 100644
--- a/SurgSim/Devices/Phantom/UnitTests/PhantomScaffoldTest.cpp
+++ b/SurgSim/Devices/Phantom/UnitTests/PhantomScaffoldTest.cpp
@@ -27,8 +27,8 @@
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Matrix.h"
-using SurgSim::Device::PhantomDevice;
-using SurgSim::Device::PhantomScaffold;
+using SurgSim::Devices::PhantomDevice;
+using SurgSim::Devices::PhantomScaffold;
TEST(PhantomScaffoldTest, CreateAndDestroyScaffold)
{
@@ -79,7 +79,8 @@ TEST(PhantomScaffoldTest, ScaffoldLifeCycle)
}
{
- std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom");
+ device->setInitializationName("Default PHANToM");
ASSERT_NE(nullptr, device) << "Creation failed. Is a Phantom device plugged in?";
// note: the device is NOT initialized!
{
@@ -100,7 +101,8 @@ TEST(PhantomScaffoldTest, ScaffoldLifeCycle)
}
{
- std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom");
+ device->setInitializationName("Default PHANToM");
ASSERT_NE(nullptr, device) << "Device creation failed.";
ASSERT_TRUE(device->initialize()) << "Initialization failed. Is a Phantom device plugged in?";
{
@@ -130,7 +132,8 @@ TEST(PhantomScaffoldTest, ScaffoldLifeCycle)
}
{
- std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom");
+ device->setInitializationName("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();
@@ -151,7 +154,8 @@ TEST(PhantomScaffoldTest, CreateDeviceSeveralTimes)
{
SCOPED_TRACE(i);
EXPECT_EQ(nullptr, lastScaffold.lock());
- std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom");
+ device->setInitializationName("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();
@@ -170,7 +174,8 @@ TEST(PhantomScaffoldTest, CreateDeviceSeveralTimesWithScaffoldRef)
for (int i = 0; i < 6; ++i)
{
SCOPED_TRACE(i);
- std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom", "Default PHANToM");
+ std::shared_ptr<PhantomDevice> device = std::make_shared<PhantomDevice>("TestPhantom");
+ device->setInitializationName("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();
diff --git a/SurgSim/Devices/Phantom/VisualTest/CMakeLists.txt b/SurgSim/Devices/Phantom/VisualTest/CMakeLists.txt
index f9a6d8b..fcada3b 100644
--- a/SurgSim/Devices/Phantom/VisualTest/CMakeLists.txt
+++ b/SurgSim/Devices/Phantom/VisualTest/CMakeLists.txt
@@ -14,14 +14,6 @@
# limitations under the License.
-link_directories(
- ${Boost_LIBRARY_DIRS}
-)
-
-include_directories(
- "${CMAKE_CURRENT_SOURCE_DIR}"
-)
-
set(EXAMPLE_SOURCES
main.cpp
)
@@ -29,21 +21,14 @@ set(EXAMPLE_SOURCES
set(EXAMPLE_HEADERS
)
-add_executable(PhantomVisualTest
- ${EXAMPLE_SOURCES} ${EXAMPLE_HEADERS})
-surgsim_show_ide_folders(
+surgsim_add_executable(PhantomVisualTest
"${EXAMPLE_SOURCES}" "${EXAMPLE_HEADERS}")
set(LIBS
- PhantomDevice
IdentityPoseDevice
- VisualTestCommon
+ PhantomDevice
SurgSimInput
- SurgSimFramework
- SurgSimDataStructures
- ${OPENHAPTICS_LIBRARIES}
- ${Boost_LIBRARIES}
- ${OPENGL_LIBRARIES}
+ VisualTestCommon
)
target_link_libraries(PhantomVisualTest ${LIBS})
diff --git a/SurgSim/Devices/Phantom/VisualTest/main.cpp b/SurgSim/Devices/Phantom/VisualTest/main.cpp
index cf11f78..8b3934a 100644
--- a/SurgSim/Devices/Phantom/VisualTest/main.cpp
+++ b/SurgSim/Devices/Phantom/VisualTest/main.cpp
@@ -22,13 +22,14 @@
#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
using SurgSim::Input::DeviceInterface;
-using SurgSim::Device::PhantomDevice;
-using SurgSim::Device::IdentityPoseDevice;
+using SurgSim::Devices::PhantomDevice;
+using SurgSim::Devices::IdentityPoseDevice;
int main(int argc, char** argv)
{
- std::shared_ptr<PhantomDevice> toolDevice = std::make_shared<PhantomDevice>("PhantomDevice", "Default PHANToM");
+ std::shared_ptr<PhantomDevice> toolDevice = std::make_shared<PhantomDevice>("PhantomDevice");
+ toolDevice->setInitializationName("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.
diff --git a/SurgSim/Devices/ReplayPoseDevice/CMakeLists.txt b/SurgSim/Devices/ReplayPoseDevice/CMakeLists.txt
new file mode 100644
index 0000000..65bba5f
--- /dev/null
+++ b/SurgSim/Devices/ReplayPoseDevice/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}")
+
+set(REPLAY_POSE_DEVICE_SOURCES
+ ReplayPoseDevice.cpp
+ ReplayPoseScaffold.cpp
+)
+
+set(REPLAY_POSE_DEVICE_HEADERS
+ ReplayPoseDevice.h
+ ReplayPoseScaffold.h
+)
+
+set(DEVICE_HEADERS ${DEVICE_HEADERS} ReplayPoseDevice/ReplayPoseDevice.h PARENT_SCOPE)
+
+surgsim_add_library(
+ ReplayPoseDevice
+ "${REPLAY_POSE_DEVICE_SOURCES}"
+ "${REPLAY_POSE_DEVICE_HEADERS}"
+)
+
+set(LIBS
+ SurgSimDataStructures
+ SurgSimFramework
+ SurgSimInput
+ SurgSimMath
+)
+
+target_link_libraries(ReplayPoseDevice ${LIBS}
+)
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+endif()
+
+# Put ReplayPoseDevice into folder "Devices"
+set_target_properties(ReplayPoseDevice PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/ReplayPoseDevice/ReplayPoseDevice.cpp b/SurgSim/Devices/ReplayPoseDevice/ReplayPoseDevice.cpp
new file mode 100644
index 0000000..8ab9a96
--- /dev/null
+++ b/SurgSim/Devices/ReplayPoseDevice/ReplayPoseDevice.cpp
@@ -0,0 +1,97 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/ReplayPoseDevice/ReplayPoseDevice.h"
+
+#include "SurgSim/Devices/ReplayPoseDevice/ReplayPoseScaffold.h"
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::ReplayPoseDevice, ReplayPoseDevice);
+
+ReplayPoseDevice::ReplayPoseDevice(const std::string& uniqueName) :
+ SurgSim::Input::CommonDevice(uniqueName, ReplayPoseScaffold::buildDeviceInputData()),
+ m_fileName("ReplayPoseDevice.txt"),
+ m_rate(1000.0)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(ReplayPoseDevice, std::string, FileName, getFileName, setFileName);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(ReplayPoseDevice, double, Rate, getRate, setRate);
+}
+
+ReplayPoseDevice::~ReplayPoseDevice()
+{
+ if (isInitialized())
+ {
+ finalize();
+ }
+}
+
+const std::string ReplayPoseDevice::getFileName() const
+{
+ return m_fileName;
+}
+
+void ReplayPoseDevice::setFileName(const std::string& fileName)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "The filename can only be set before initialization";
+ m_fileName = fileName;
+}
+
+double ReplayPoseDevice::getRate() const
+{
+ return m_rate;
+}
+
+void ReplayPoseDevice::setRate(double rate)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "The rate can only be set before initialization";
+ m_rate = rate;
+}
+
+bool ReplayPoseDevice::initialize()
+{
+ SURGSIM_ASSERT(!isInitialized()) << getName() << " already initialized.";
+ std::shared_ptr<ReplayPoseScaffold> scaffold = ReplayPoseScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold);
+
+ if (!scaffold->registerDevice(this))
+ {
+ return false;
+ }
+ m_scaffold = std::move(scaffold);
+
+ return true;
+}
+
+
+bool ReplayPoseDevice::finalize()
+{
+ SURGSIM_ASSERT(isInitialized()) << getName() << " is not initialized, cannot finalize.";
+ bool ok = m_scaffold->unregisterDevice();
+ m_scaffold.reset();
+ return ok;
+}
+
+bool ReplayPoseDevice::isInitialized() const
+{
+ return (m_scaffold != nullptr);
+}
+
+}; // namespace Devices
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/ReplayPoseDevice/ReplayPoseDevice.h b/SurgSim/Devices/ReplayPoseDevice/ReplayPoseDevice.h
new file mode 100644
index 0000000..05e72c6
--- /dev/null
+++ b/SurgSim/Devices/ReplayPoseDevice/ReplayPoseDevice.h
@@ -0,0 +1,90 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_REPLAYPOSEDEVICE_REPLAYPOSEDEVICE_H
+#define SURGSIM_DEVICES_REPLAYPOSEDEVICE_REPLAYPOSEDEVICE_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Input/CommonDevice.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+class ReplayPoseScaffold;
+
+SURGSIM_STATIC_REGISTRATION(ReplayPoseDevice);
+
+/// A class implementing the replay pose device, which is a pretend device that replays a recorded motion from a file
+///
+/// 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.
+///
+/// \par Application input provided by the device:
+/// | type | name | |
+/// | ---- | ---- | --- |
+/// | pose | "pose" | %Device pose (units are meters). |
+///
+/// \sa SurgSim::Input::CommonDevice
+class ReplayPoseDevice : public SurgSim::Input::CommonDevice
+{
+public:
+ /// Constructor.
+ /// \param uniqueName A unique name for the device that will be used by the application.
+ explicit ReplayPoseDevice(const std::string& uniqueName);
+
+ /// \return The filename from which the input poses are read (default is 'ReplayPoseDevice.txt')
+ const std::string getFileName() const;
+
+ /// \param fileName from which the input poses will be read (default is 'ReplayPoseDevice.txt')
+ void setFileName(const std::string& fileName);
+
+ /// \return The rate (in Hz) at which the device is running (1KHz is the default)
+ double getRate() const;
+
+ /// \param rate The rate (in Hz) at which the device will run (1KHz is the default)
+ /// \exception SurgSim::Framework::AssertionFailure if the method is called after initialization
+ void setRate(double rate);
+
+ SURGSIM_CLASSNAME(SurgSim::Devices::ReplayPoseDevice);
+
+ virtual ~ReplayPoseDevice();
+
+ bool initialize() override;
+
+ bool isInitialized() const override;
+
+private:
+ friend class ReplayPoseScaffold;
+
+ bool finalize() override;
+
+ std::shared_ptr<ReplayPoseScaffold> m_scaffold;
+
+ /// The filename to read the input transform from
+ std::string m_fileName;
+
+ /// The rate to run the device at (i.e. at which rate the information is populated).
+ /// This is independent of the record being replayed, the motion will be real-time (interpolation may occur).
+ double m_rate;
+};
+
+}; // namespace Devices
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_REPLAYPOSEDEVICE_REPLAYPOSEDEVICE_H
diff --git a/SurgSim/Devices/ReplayPoseDevice/ReplayPoseScaffold.cpp b/SurgSim/Devices/ReplayPoseDevice/ReplayPoseScaffold.cpp
new file mode 100644
index 0000000..467de25
--- /dev/null
+++ b/SurgSim/Devices/ReplayPoseDevice/ReplayPoseScaffold.cpp
@@ -0,0 +1,281 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/ReplayPoseDevice/ReplayPoseScaffold.h"
+
+#include <Eigen/Core>
+
+#include <fstream>
+
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Devices/ReplayPoseDevice/ReplayPoseDevice.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SharedInstance.h"
+
+namespace
+{
+/// Missing istream "operator >>" for Eigen matrix type
+/// http://eigen.tuxfamily.org/bz/show_bug.cgi?id=622
+template<typename Derived>
+std::istream& operator >>(std::istream& s, Eigen::MatrixBase<Derived>& m)
+{
+ for (int i = 0; i < m.rows(); ++i)
+ {
+ for (int j = 0; j < m.cols(); j++)
+ {
+ s >> m(i, j);
+ }
+ }
+ return s;
+}
+}
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+ReplayPoseScaffold::ReplayPoseScaffold() :
+ Framework::BasicThread("ReplayPoseScaffold"),
+ m_logger(Framework::Logger::getLogger("Devices/ReplayPoseScaffold"))
+{
+ SURGSIM_LOG_DEBUG(m_logger) << "ReplayPose scaffold created.";
+}
+
+ReplayPoseScaffold::~ReplayPoseScaffold()
+{
+ if (m_device != nullptr)
+ {
+ unregisterDevice();
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "ReplayPose scaffold destroyed.";
+}
+
+std::shared_ptr<ReplayPoseScaffold> ReplayPoseScaffold::getOrCreateSharedInstance()
+{
+ static Framework::SharedInstance<ReplayPoseScaffold> sharedInstance;
+ return sharedInstance.get();
+}
+
+bool ReplayPoseScaffold::doInitialize()
+{
+ return true;
+}
+
+bool ReplayPoseScaffold::doStartUp()
+{
+ return true;
+}
+
+bool ReplayPoseScaffold::doUpdate(double dt)
+{
+ boost::unique_lock<boost::mutex> scopedLock(m_deviceLock);
+
+ SURGSIM_ASSERT(m_device != nullptr) << "DeviceData not properly allocated";
+
+ return updateDevice(m_device.get());
+}
+
+struct ReplayPoseScaffold::DeviceData
+{
+ /// Constructor
+ /// \param device Device to be managed by this scaffold
+ explicit DeviceData(ReplayPoseDevice* device) :
+ deviceObject(device),
+ m_timestamp(0),
+ m_index(0),
+ m_fileLoaded(false)
+ {
+ m_fileLoaded = loadFile(deviceObject->getFileName());
+ m_timer.start();
+ }
+
+ /// Retrieve the pose for the given time stamp "m_timestamp"
+ /// \return The requested pose, Identity if none could be loaded
+ Math::RigidTransform3d getPose()
+ {
+ if (m_motion.size() == 0)
+ {
+ return Math::RigidTransform3d::Identity();
+ }
+
+ if (m_motion.size() == 1 || (m_index == 0 && m_timestamp <= m_motion[0].first))
+ {
+ return m_motion[0].second;
+ }
+
+ while (m_motion.size() > m_index && m_timestamp > m_motion[m_index].first)
+ {
+ m_index++;
+ }
+
+ // Requesting a timestamp over the higher limit recorded in the file
+ if (m_motion.size() <= m_index)
+ {
+ return m_motion[m_motion.size() - 1].second;
+ }
+
+ // Are we requesting exactly the timestamp we just indexed
+ if (m_timestamp == m_motion[m_index].first)
+ {
+ return m_motion[m_index].second;
+ }
+
+ // Otherwise, we need to interpolate between the index and the previous index
+ double range = m_motion[m_index].first - m_motion[m_index - 1].first;
+ double time = (m_timestamp - m_motion[m_index - 1].first) / range;
+ return Math::interpolate(m_motion[m_index - 1].second, m_motion[m_index].second, time);
+ }
+
+ /// Device object managed by this scaffold.
+ ReplayPoseDevice* const deviceObject;
+
+ /// Timer to keep track of the time stamp and keep the replay in sync with the recording
+ Framework::Timer m_timer;
+
+ /// Time stamp for the device (which time stamp are we reproducing?)
+ double m_timestamp;
+
+ /// Series of poses through time loaded from the input file
+ std::vector<std::pair<double, Math::RigidTransform3d>> m_motion;
+
+ /// Index of the latest pose used
+ size_t m_index;
+
+ /// Valid file loaded successfully
+ bool m_fileLoaded;
+
+private:
+ // Prevent copy construction and copy assignment. (VS2012 does not support "= delete" yet.)
+ DeviceData(const DeviceData&) /*= delete*/;
+ DeviceData& operator=(const DeviceData&) /*= delete*/;
+
+ /// \param fileName the file to be open and loaded
+ /// \return True if the file was loaded successfully, False otherwise
+ bool loadFile(const std::string& fileName)
+ {
+ auto logger = Framework::Logger::getLogger("Devices/ReplayPoseScaffold");
+
+ std::ifstream inputFile;
+ bool result = true;
+
+ inputFile.open(fileName, std::ios::in);
+
+ if (!inputFile.is_open())
+ {
+ SURGSIM_LOG_WARNING(logger) << "Could not find or open the file " << fileName <<
+ "; Replay will use Identity pose";
+ result = false;
+ }
+ else
+ {
+ while (!inputFile.eof())
+ {
+ double timeStamp;
+ Math::RigidTransform3d pose;
+ inputFile >> timeStamp >> pose.matrix();
+ m_motion.emplace_back(timeStamp, pose);
+ }
+ SURGSIM_LOG_INFO(logger) << "Loaded " << m_motion.size() << " timestamps";
+ if (m_motion.size() >= 1)
+ {
+ SURGSIM_LOG_INFO(logger) << "The loaded timestamps cover a range of " <<
+ m_motion[m_motion.size() - 1].first - m_motion[0].first << " second(s)";
+ }
+ SURGSIM_LOG_IF(m_motion.size() == 0, logger, WARNING) <<
+ "No poses could be properly loaded, Identity pose will be used";
+
+ inputFile.close();
+ }
+
+ return result;
+ }
+};
+
+bool ReplayPoseScaffold::registerDevice(ReplayPoseDevice* device)
+{
+ boost::unique_lock<boost::mutex> scopedLock(m_deviceLock);
+ SURGSIM_ASSERT(m_device == nullptr) << "Can't register two ReplayPoseDevice.";
+
+ m_device.reset(new ReplayPoseScaffold::DeviceData(device));
+ if (m_device == nullptr)
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "Failed to create a DeviceData";
+ return false;
+ }
+ if (!m_device->m_fileLoaded)
+ {
+ SURGSIM_LOG_CRITICAL(m_logger) << "Failed to load the file to replay";
+ return false;
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << "Registered device " << device->getName();
+
+ bool success = true;
+ if (!isRunning())
+ {
+ std::shared_ptr<Framework::Barrier> barrier = std::make_shared<Framework::Barrier>(2);
+ start(barrier);
+ barrier->wait(true); // Wait for initialize
+ barrier->wait(true); // Wait for startup
+ success = isInitialized();
+ }
+
+ if (success)
+ {
+ setRate(device->getRate());
+ }
+
+ return success;
+}
+
+bool ReplayPoseScaffold::unregisterDevice()
+{
+
+ if (isRunning())
+ {
+ stop();
+ }
+
+ // #threadsafety After unregistering, another thread could be in the process of registering.
+ boost::unique_lock<boost::mutex> scopedLock(m_deviceLock);
+ m_device.reset();
+ SURGSIM_LOG_DEBUG(m_logger) << "Unregistered device";
+ return true;
+}
+
+bool ReplayPoseScaffold::updateDevice(ReplayPoseScaffold::DeviceData* info)
+{
+ DataStructures::DataGroup& inputData = m_device->deviceObject->getInputData();
+
+ info->m_timer.markFrame();
+ info->m_timestamp += info->m_timer.getLastFramePeriod();
+
+ Math::RigidTransform3d pose = info->getPose();
+ inputData.poses().set(DataStructures::Names::POSE, pose);
+
+ m_device->deviceObject->pushInput();
+
+ return true;
+}
+
+DataStructures::DataGroup ReplayPoseScaffold::buildDeviceInputData()
+{
+ DataStructures::DataGroupBuilder builder;
+ builder.addPose(DataStructures::Names::POSE);
+ return builder.createData();
+}
+
+}; // namespace Devices
+}; // namespace SurgSim
diff --git a/SurgSim/Devices/ReplayPoseDevice/ReplayPoseScaffold.h b/SurgSim/Devices/ReplayPoseDevice/ReplayPoseScaffold.h
new file mode 100644
index 0000000..6d439be
--- /dev/null
+++ b/SurgSim/Devices/ReplayPoseDevice/ReplayPoseScaffold.h
@@ -0,0 +1,91 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_REPLAYPOSEDEVICE_REPLAYPOSESCAFFOLD_H
+#define SURGSIM_DEVICES_REPLAYPOSEDEVICE_REPLAYPOSESCAFFOLD_H
+
+#include <boost/thread/mutex.hpp>
+#include <memory>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Framework/BasicThread.h"
+#include "SurgSim/Framework/Logger.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+namespace SurgSim
+{
+namespace Devices
+{
+
+class ReplayPoseDevice;
+
+class ReplayPoseScaffold : public SurgSim::Framework::BasicThread
+{
+ friend class ReplayPoseDevice;
+
+public:
+ /// Constructor.
+ ReplayPoseScaffold();
+
+ /// Destructor.
+ ~ReplayPoseScaffold();
+
+ /// Gets or creates the scaffold shared by all RawMultiAxisDevice instances.
+ /// The scaffold is managed using a SharedInstance object, so it will be destroyed when all devices are released.
+ /// \return the scaffold object.
+ static std::shared_ptr<ReplayPoseScaffold> getOrCreateSharedInstance();
+
+protected:
+ bool doInitialize() override;
+ bool doStartUp() override;
+ bool doUpdate(double dt) override;
+
+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(ReplayPoseDevice* 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 info The information to update the device from
+ /// \return True on success.
+ bool updateDevice(ReplayPoseScaffold::DeviceData* info);
+
+ /// Builds the data layout for the application input (i.e. device output).
+ static DataStructures::DataGroup buildDeviceInputData();
+
+ /// Logger used by the scaffold and all devices.
+ std::shared_ptr<Framework::Logger> m_logger;
+
+ /// The ReplayPose device locking mechanism
+ boost::mutex m_deviceLock;
+
+ /// The ReplayPose device managed by this scaffold
+ std::unique_ptr<DeviceData> m_device;
+};
+
+}; // namespace Devices
+}; // namespace SurgSim
+
+#endif // SURGSIM_DEVICES_REPLAYPOSEDEVICE_REPLAYPOSESCAFFOLD_H
diff --git a/SurgSim/Devices/ReplayPoseDevice/UnitTests/CMakeLists.txt b/SurgSim/Devices/ReplayPoseDevice/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..749bf94
--- /dev/null
+++ b/SurgSim/Devices/ReplayPoseDevice/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
+ ReplayPoseDeviceTest.cpp
+)
+
+set(LIBS
+ ReplayPoseDevice
+)
+
+surgsim_add_unit_tests(ReplayPoseDeviceTest)
+
+set_target_properties(ReplayPoseDeviceTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/ReplayPoseDevice/UnitTests/ReplayPoseDeviceTest.cpp b/SurgSim/Devices/ReplayPoseDevice/UnitTests/ReplayPoseDeviceTest.cpp
new file mode 100644
index 0000000..693d64b
--- /dev/null
+++ b/SurgSim/Devices/ReplayPoseDevice/UnitTests/ReplayPoseDeviceTest.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 ReplayPoseDevice class.
+
+#include <memory>
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include <boost/thread/thread.hpp> // This is to access boost::this_thread namespace
+
+#include "SurgSim/Devices/ReplayPoseDevice/ReplayPoseDevice.h"
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Framework/Timer.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Testing/MockInputOutput.h"
+
+using SurgSim::Devices::ReplayPoseDevice;
+using SurgSim::DataStructures::DataGroup;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Matrix44d;
+using SurgSim::Testing::MockInputOutput;
+
+namespace
+{
+void createFakeRecord(const std::string& fileName, double rate)
+{
+ SurgSim::Framework::Timer timer;
+ timer.setMaxNumberOfFrames(1);
+ timer.start();
+
+ std::ofstream f(fileName, std::ios::out | std::ios::trunc);
+
+ timer.endFrame();
+ double cumulativeTime = timer.getLastFramePeriod();
+ double deltaTime = 1.0 / rate;
+
+ SurgSim::Math::Quaterniond q(Eigen::AngleAxisd(2.0123, SurgSim::Math::Vector3d(1, 2, 3).normalized()));
+ SurgSim::Math::Vector3d t(0.01, -0.04, 0.0035);
+ SurgSim::Math::RigidTransform3d poseStart = SurgSim::Math::makeRigidTranslation(SurgSim::Math::Vector3d::Zero());
+ SurgSim::Math::RigidTransform3d poseEnd = SurgSim::Math::makeRigidTransform(q, t);
+
+ do
+ {
+ auto pose = SurgSim::Math::interpolate(poseStart, poseEnd, cumulativeTime);
+ f << cumulativeTime << std::endl << pose.matrix() << std::endl;
+ cumulativeTime += deltaTime;
+ } while (cumulativeTime < 1.0);
+ f << "1.0" << std::endl << poseEnd.matrix() << std::endl;
+
+ f.close();
+}
+
+
+
+bool exist(const std::string& fileName)
+{
+ std::ifstream f(fileName.c_str());
+ bool result = f.good();
+ f.close();
+ return result;
+}
+
+void clearFakeRecord(const std::string& fileName)
+{
+ if (exist(fileName))
+ {
+ std::remove(fileName.c_str());
+ }
+}
+}
+
+TEST(ReplayPoseDeviceTest, Name)
+{
+ auto device = std::make_shared<ReplayPoseDevice>("FakeReplayDevice");
+ EXPECT_EQ("FakeReplayDevice", device->getName());
+}
+
+TEST(ReplayPoseDeviceTest, Filename)
+{
+ std::string fileName("FakeRecord.txt");
+ createFakeRecord(fileName, 30);
+
+ auto device = std::make_shared<ReplayPoseDevice>("FakeReplayDevice");
+ EXPECT_EQ(0, device->getFileName().compare("ReplayPoseDevice.txt"));
+ EXPECT_NO_THROW(device->setFileName(fileName));
+ EXPECT_EQ(0, device->getFileName().compare(fileName));
+
+ EXPECT_TRUE(device->initialize());
+
+ clearFakeRecord(fileName);
+ EXPECT_THROW(device->setFileName(fileName), SurgSim::Framework::AssertionFailure);
+}
+
+TEST(ReplayPoseDeviceTest, Initialize)
+{
+ std::string fileName("FakeRecord.txt");
+ clearFakeRecord(fileName);
+
+ {
+ SCOPED_TRACE("Missing filename");
+ auto device = std::make_shared<ReplayPoseDevice>("FakeReplayDevice");
+ device->setFileName(fileName);
+ EXPECT_FALSE(device->initialize()); // Missing the setFilename call, no file open
+ EXPECT_FALSE(device->isInitialized());
+ }
+ {
+ SCOPED_TRACE("Success");
+ createFakeRecord(fileName, 30);
+
+ auto device = std::make_shared<ReplayPoseDevice>("FakeReplayDevice");
+ device->setFileName(fileName);
+ EXPECT_TRUE(device->initialize());
+ EXPECT_TRUE(device->isInitialized());
+
+ clearFakeRecord(fileName);
+ }
+}
+
+TEST(ReplayPoseDeviceTest, Factory)
+{
+ std::shared_ptr<SurgSim::Input::DeviceInterface> device;
+ ASSERT_NO_THROW(device = SurgSim::Input::DeviceInterface::getFactory().create(
+ "SurgSim::Devices::ReplayPoseDevice", "Device"));
+ EXPECT_NE(nullptr, device);
+}
+
+namespace
+{
+void AddInputConsumerRecordXHzReplayYHz(double rateRecord, double rateReplay)
+{
+ std::string fileName("FakeRecord.txt");
+
+ createFakeRecord(fileName, rateRecord);
+
+ std::shared_ptr<ReplayPoseDevice> device = std::make_shared<ReplayPoseDevice>("MyReplayPoseDevice");
+ ASSERT_TRUE(device != nullptr) << "Device creation failed.";
+ device->setRate(rateReplay);
+ device->setFileName("FakeRecord.txt");
+ ASSERT_TRUE(device->initialize()) << "Initialization failed.";
+
+ 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.
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
+
+ // Check the number of invocations.
+ EXPECT_EQ(1, consumer->m_numTimesInitializedInput);
+ EXPECT_LE(consumer->m_numTimesReceivedInput, rateReplay * 1.3); // rateReplay@ 1000Hz => higher limit@ 1300Hz
+
+ EXPECT_TRUE(consumer->m_lastReceivedInput.poses().hasData(SurgSim::DataStructures::Names::POSE));
+
+ // Check the replay pose passed 1.0 second
+ SurgSim::Math::RigidTransform3d pose;
+ boost::this_thread::sleep_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(50));
+ ASSERT_TRUE(consumer->m_lastReceivedInput.poses().get(SurgSim::DataStructures::Names::POSE, &pose));
+ SurgSim::Math::Quaterniond q(Eigen::AngleAxisd(2.0123, SurgSim::Math::Vector3d(1, 2, 3).normalized()));
+ SurgSim::Math::Vector3d t(0.01, -0.04, 0.0035);
+ SurgSim::Math::RigidTransform3d poseEnd = SurgSim::Math::makeRigidTransform(q, t);
+ EXPECT_NEAR(0, (pose.matrix() - poseEnd.matrix()).norm(), 1e-6);
+
+ EXPECT_TRUE(device->removeInputConsumer(consumer));
+ // Removing the same input consumer again should fail.
+ EXPECT_FALSE(device->removeInputConsumer(consumer));
+
+ clearFakeRecord(fileName);
+}
+}
+
+TEST(ReplayPoseDeviceTest, AddInputConsumerRecord60HzReplay60Hz)
+{
+ AddInputConsumerRecordXHzReplayYHz(60.0, 60.0);
+}
+
+TEST(ReplayPoseDeviceTest, AddInputConsumerRecord60HzReplay1000Hz)
+{
+ AddInputConsumerRecordXHzReplayYHz(60.0, 1000.0);
+}
+
+TEST(ReplayPoseDeviceTest, AddInputConsumerRecord1000HzReplay60Hz)
+{
+ AddInputConsumerRecordXHzReplayYHz(1000.0, 60.0);
+}
+
+TEST(ReplayPoseDeviceTest, AddInputConsumerRecord1000HzReplay1000Hz)
+{
+ AddInputConsumerRecordXHzReplayYHz(1000.0, 1000.0);
+}
diff --git a/SurgSim/Devices/Sixense/CMakeLists.txt b/SurgSim/Devices/Sixense/CMakeLists.txt
index be79c7d..0c4cc97 100644
--- a/SurgSim/Devices/Sixense/CMakeLists.txt
+++ b/SurgSim/Devices/Sixense/CMakeLists.txt
@@ -13,18 +13,14 @@
# 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(
+ ${SIXENSESDK_INCLUDE_DIR}
)
-include_directories(
- "${CMAKE_CURRENT_SOURCE_DIR}"
- "${SIXENSE_SDK_INCLUDE_DIR}"
+set(LIBS
+ ${SIXENSESDK_LIBRARIES}
)
set(SIXENSE_DEVICE_SOURCES
@@ -39,11 +35,13 @@ set(SIXENSE_DEVICE_HEADERS
SixenseThread.h
)
+set(DEVICE_HEADERS ${DEVICE_HEADERS} Sixense/SixenseDevice.h PARENT_SCOPE)
+
# TODO(advornik): the installation should NOT copy all the headers...
surgsim_add_library(
SixenseDevice
- "${SIXENSE_DEVICE_SOURCES}" "${SIXENSE_DEVICE_HEADERS}"
- SurgSim/Devices/Sixense
+ "${SIXENSE_DEVICE_SOURCES}"
+ "${SIXENSE_DEVICE_HEADERS}"
)
target_link_libraries(SixenseDevice ${LIBS})
diff --git a/SurgSim/Devices/Sixense/Sixense.dox b/SurgSim/Devices/Sixense/Sixense.dox
index 2a18a5b..17e28bb 100644
--- a/SurgSim/Devices/Sixense/Sixense.dox
+++ b/SurgSim/Devices/Sixense/Sixense.dox
@@ -6,6 +6,9 @@ The Razer Hydra (by Sixense Entertainment and Razer USA) is a device that tracks
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
+- An environment variable named SIXENSESDK_PATH or SIXENSE_ROOT must be set to the directory containing <tt>(include\\)sixense.h</tt>
+- Executables that incorporate this device must have access to the appropriate libraries at runtime, either by adding the appropriate
+ folder(s) to the path environment variable, or copying the libraries into the folder with the executable.
+- Sixense's SDK is distributed/built under Visual Studio 2010 or 2013 (no source code available). To run applications under debug mode with
+ Sixense's SDK, MSVCP100d.dll is required, which is only distributed in Visual Studio 2010.
+*/
diff --git a/SurgSim/Devices/Sixense/SixenseDevice.cpp b/SurgSim/Devices/Sixense/SixenseDevice.cpp
index 6dcea1d..6c0de2f 100644
--- a/SurgSim/Devices/Sixense/SixenseDevice.cpp
+++ b/SurgSim/Devices/Sixense/SixenseDevice.cpp
@@ -17,14 +17,14 @@
#include "SurgSim/Devices/Sixense/SixenseScaffold.h"
#include "SurgSim/Framework/Log.h"
-#include "SurgSim/Framework/Assert.h"
namespace SurgSim
{
-namespace Device
+namespace Devices
{
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::SixenseDevice, SixenseDevice);
SixenseDevice::SixenseDevice(const std::string& uniqueName) :
SurgSim::Input::CommonDevice(uniqueName, SixenseScaffold::buildDeviceInputData())
@@ -43,35 +43,32 @@ SixenseDevice::~SixenseDevice()
bool SixenseDevice::initialize()
{
- SURGSIM_ASSERT(! isInitialized());
- std::shared_ptr<SixenseScaffold> scaffold = SixenseScaffold::getOrCreateSharedInstance();
- SURGSIM_ASSERT(scaffold);
+ SURGSIM_ASSERT(!isInitialized()) << getName() << " already initialized.";
+ auto scaffold = SixenseScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold != nullptr);
- if (! scaffold->registerDevice(this))
+ bool initialize = false;
+ if (scaffold->registerDevice(this))
{
- return false;
+ m_scaffold = std::move(scaffold);
+ initialize = true;
}
-
- m_scaffold = std::move(scaffold);
- SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Initialized.";
- return true;
+ return initialize;
}
bool SixenseDevice::finalize()
{
- SURGSIM_ASSERT(isInitialized());
- SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Finalizing.";
+ SURGSIM_ASSERT(isInitialized()) << getName() << " is not initialized, cannot finalize.";
bool ok = m_scaffold->unregisterDevice(this);
m_scaffold.reset();
return ok;
}
-
bool SixenseDevice::isInitialized() const
{
return (m_scaffold != nullptr);
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/Sixense/SixenseDevice.h b/SurgSim/Devices/Sixense/SixenseDevice.h
index 2f4e697..07835a6 100644
--- a/SurgSim/Devices/Sixense/SixenseDevice.h
+++ b/SurgSim/Devices/Sixense/SixenseDevice.h
@@ -23,10 +23,11 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
class SixenseScaffold;
+SURGSIM_STATIC_REGISTRATION(SixenseDevice);
/// A class implementing the communication with one Sixense controller, for example one of the two on the Razer Hydra.
///
@@ -57,23 +58,24 @@ public:
/// \param uniqueName A unique name for the device that will be used by the application.
explicit SixenseDevice(const std::string& uniqueName);
+ SURGSIM_CLASSNAME(SurgSim::Devices::SixenseDevice);
+
/// Destructor.
virtual ~SixenseDevice();
- virtual bool initialize() override;
-
- virtual bool finalize() override;
+ bool initialize() override;
- /// Check wheter this device is initialized.
- bool isInitialized() const;
+ bool isInitialized() const override;
private:
friend class SixenseScaffold;
+ bool finalize() override;
+
std::shared_ptr<SixenseScaffold> m_scaffold;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_SIXENSE_SIXENSEDEVICE_H
diff --git a/SurgSim/Devices/Sixense/SixenseScaffold.cpp b/SurgSim/Devices/Sixense/SixenseScaffold.cpp
index 5cf6676..dfec188 100644
--- a/SurgSim/Devices/Sixense/SixenseScaffold.cpp
+++ b/SurgSim/Devices/Sixense/SixenseScaffold.cpp
@@ -45,7 +45,7 @@ using SurgSim::Math::RigidTransform3d;
namespace SurgSim
{
-namespace Device
+namespace Devices
{
@@ -100,15 +100,10 @@ private:
};
-SixenseScaffold::SixenseScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) :
- m_logger(logger), m_state(new StateData)
+SixenseScaffold::SixenseScaffold() :
+ m_logger(Framework::Logger::getLogger("Devices/Sixense")), 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.";
+ SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold created.";
}
@@ -205,6 +200,7 @@ bool SixenseScaffold::registerDevice(SixenseDevice* device)
return false;
}
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << " initialized.";
return true;
}
@@ -221,13 +217,11 @@ bool SixenseScaffold::unregisterDevice(const SixenseDevice* const device)
m_state->activeDeviceList.erase(matching);
// the iterator is now invalid but that's OK
found = true;
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << " finalized.";
}
}
- if (! found)
- {
- SURGSIM_LOG_WARNING(m_logger) << "Sixense/Hydra: Attempted to release a non-registered device.";
- }
+ SURGSIM_LOG_IF(!found, m_logger, SEVERE) << "Attempted to release a non-registered device " << device->getName();
return found;
}
@@ -480,17 +474,10 @@ std::shared_ptr<SixenseScaffold> SixenseScaffold::getOrCreateSharedInstance()
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 Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/Sixense/SixenseScaffold.h b/SurgSim/Devices/Sixense/SixenseScaffold.h
index 17765d1..7a490ba 100644
--- a/SurgSim/Devices/Sixense/SixenseScaffold.h
+++ b/SurgSim/Devices/Sixense/SixenseScaffold.h
@@ -23,7 +23,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
class SixenseDevice;
@@ -31,30 +31,18 @@ class SixenseThread;
/// A class that manages Sixense devices, such as the Razer Hydra.
///
-/// \sa SurgSim::Device::SixenseDevice
+/// \sa SurgSim::Devices::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.
+ /// The scaffold is managed using a SharedInstance 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;
@@ -66,9 +54,7 @@ private:
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);
+ SixenseScaffold();
/// Registers the specified device object.
/// If successful, the device object will become connected to an unused controller.
@@ -140,15 +126,13 @@ private:
/// 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 Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_SIXENSE_SIXENSESCAFFOLD_H
diff --git a/SurgSim/Devices/Sixense/SixenseThread.cpp b/SurgSim/Devices/Sixense/SixenseThread.cpp
index be61d3f..dfc31a1 100644
--- a/SurgSim/Devices/Sixense/SixenseThread.cpp
+++ b/SurgSim/Devices/Sixense/SixenseThread.cpp
@@ -19,7 +19,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
SixenseThread::~SixenseThread()
@@ -42,5 +42,5 @@ bool SixenseThread::doStartUp()
return true;
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/Sixense/SixenseThread.h b/SurgSim/Devices/Sixense/SixenseThread.h
index 3379478..da80f40 100644
--- a/SurgSim/Devices/Sixense/SixenseThread.h
+++ b/SurgSim/Devices/Sixense/SixenseThread.h
@@ -23,13 +23,13 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
class SixenseScaffold;
/// A class implementing the thread context for sampling Sixense devices.
-/// \sa SurgSim::Device::SixenseScaffold
+/// \sa SurgSim::Devices::SixenseScaffold
class SixenseThread : public SurgSim::Framework::BasicThread
{
public:
@@ -43,15 +43,15 @@ public:
virtual ~SixenseThread();
protected:
- virtual bool doInitialize() override;
- virtual bool doStartUp() override;
- virtual bool doUpdate(double dt) override;
+ bool doInitialize() override;
+ bool doStartUp() override;
+ bool doUpdate(double dt) override;
private:
SixenseScaffold* m_scaffold;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_SIXENSE_SIXENSETHREAD_H
diff --git a/SurgSim/Devices/Sixense/UnitTests/CMakeLists.txt b/SurgSim/Devices/Sixense/UnitTests/CMakeLists.txt
index 5fbe7f9..383682f 100644
--- a/SurgSim/Devices/Sixense/UnitTests/CMakeLists.txt
+++ b/SurgSim/Devices/Sixense/UnitTests/CMakeLists.txt
@@ -13,7 +13,6 @@
# See the License for the specific language governing permissions and
# limitations under the License.
-
include_directories(
${gtest_SOURCE_DIR}/include
)
@@ -25,23 +24,23 @@ set(UNIT_TEST_SOURCES
set(LIBS
SixenseDevice
- SurgSimInput
- SurgSimFramework
SurgSimDataStructures
- ${Boost_LIBRARIES}
- ${SIXENSE_SDK_LIBRARIES}
+ SurgSimMath
+ SurgSimTesting
)
-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)
+
+# Copy the device DLLs into the build directory
+surgsim_copy_to_target_directory_for_release(SixenseDeviceTest
+ ${SIXENSESDK_sixense_SHARED_RELEASE}
+ ${SIXENSESDK_sixense_utils_SHARED_RELEASE}
)
-surgsim_add_unit_tests(SixenseDeviceTest)
+surgsim_copy_to_target_directory_for_debug(SixenseDeviceTest
+ ${SIXENSESDK_sixense_SHARED_DEBUG}
+ ${SIXENSESDK_sixense_utils_SHARED_DEBUG}
+ ${SIXENSESDK_DeviceDLL_SHARED_DEBUG} # crazy debug-only dependency
+)
set_target_properties(SixenseDeviceTest PROPERTIES FOLDER "Devices")
diff --git a/SurgSim/Devices/Sixense/UnitTests/SixenseDeviceTest.cpp b/SurgSim/Devices/Sixense/UnitTests/SixenseDeviceTest.cpp
index 53d098c..d333cd2 100644
--- a/SurgSim/Devices/Sixense/UnitTests/SixenseDeviceTest.cpp
+++ b/SurgSim/Devices/Sixense/UnitTests/SixenseDeviceTest.cpp
@@ -28,8 +28,8 @@
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Testing/MockInputOutput.h"
-using SurgSim::Device::SixenseDevice;
-using SurgSim::Device::SixenseScaffold;
+using SurgSim::Devices::SixenseDevice;
+using SurgSim::Devices::SixenseScaffold;
using SurgSim::DataStructures::DataGroup;
using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Matrix44d;
diff --git a/SurgSim/Devices/Sixense/UnitTests/SixenseScaffoldTest.cpp b/SurgSim/Devices/Sixense/UnitTests/SixenseScaffoldTest.cpp
index 7d85345..9764cf7 100644
--- a/SurgSim/Devices/Sixense/UnitTests/SixenseScaffoldTest.cpp
+++ b/SurgSim/Devices/Sixense/UnitTests/SixenseScaffoldTest.cpp
@@ -27,8 +27,8 @@
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Matrix.h"
-using SurgSim::Device::SixenseDevice;
-using SurgSim::Device::SixenseScaffold;
+using SurgSim::Devices::SixenseDevice;
+using SurgSim::Devices::SixenseScaffold;
TEST(SixenseScaffoldTest, CreateAndDestroyScaffold)
{
diff --git a/SurgSim/Devices/Sixense/VisualTest/CMakeLists.txt b/SurgSim/Devices/Sixense/VisualTest/CMakeLists.txt
index 7508268..0d87882 100644
--- a/SurgSim/Devices/Sixense/VisualTest/CMakeLists.txt
+++ b/SurgSim/Devices/Sixense/VisualTest/CMakeLists.txt
@@ -13,15 +13,6 @@
# 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
)
@@ -29,22 +20,16 @@ set(EXAMPLE_SOURCES
set(EXAMPLE_HEADERS
)
-add_executable(SixenseVisualTest
- ${EXAMPLE_SOURCES} ${EXAMPLE_HEADERS})
-
-surgsim_show_ide_folders(
- "${EXAMPLE_SOURCES}" "${EXAMPLE_HEADERS}")
+surgsim_add_executable(SixenseVisualTest
+ "${EXAMPLE_SOURCES}"
+ "${EXAMPLE_HEADERS}"
+)
set(LIBS
- SixenseDevice
IdentityPoseDevice
- VisualTestCommon
+ SixenseDevice
SurgSimInput
- SurgSimFramework
- SurgSimDataStructures
- ${Boost_LIBRARIES}
- ${SIXENSE_SDK_LIBRARIES}
- ${OPENGL_LIBRARIES}
+ VisualTestCommon
)
target_link_libraries(SixenseVisualTest ${LIBS})
@@ -52,14 +37,14 @@ 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}
+ ${SIXENSESDK_sixense_SHARED_RELEASE}
+ ${SIXENSESDK_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
+ ${SIXENSESDK_sixense_SHARED_DEBUG}
+ ${SIXENSESDK_sixense_utils_SHARED_DEBUG}
+ ${SIXENSESDK_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
index b7700ac..b20ff0f 100644
--- a/SurgSim/Devices/Sixense/VisualTest/main.cpp
+++ b/SurgSim/Devices/Sixense/VisualTest/main.cpp
@@ -22,8 +22,8 @@
#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
using SurgSim::Input::DeviceInterface;
-using SurgSim::Device::SixenseDevice;
-using SurgSim::Device::IdentityPoseDevice;
+using SurgSim::Devices::SixenseDevice;
+using SurgSim::Devices::IdentityPoseDevice;
int main(int argc, char** argv)
diff --git a/SurgSim/Devices/TrackIR/CMakeLists.txt b/SurgSim/Devices/TrackIR/CMakeLists.txt
index c03da61..0829676 100644
--- a/SurgSim/Devices/TrackIR/CMakeLists.txt
+++ b/SurgSim/Devices/TrackIR/CMakeLists.txt
@@ -21,6 +21,10 @@ include_directories(
"${OPTITRACK_INCLUDE_DIR}"
)
+set(LIBS
+ ${OPTITRACK_LIBRARY}
+)
+
set(TRACKIR_DEVICE_SOURCES
TrackIRDevice.cpp
TrackIRThread.cpp
@@ -32,6 +36,8 @@ set(TRACKIR_DEVICE_HEADERS
TrackIRThread.h
)
+set(DEVICE_HEADERS ${DEVICE_HEADERS} TrackIR/TrackIRDevice.h PARENT_SCOPE)
+
set(TRACKIR_DEVICE_LINUX_SOURCES
linux/TrackIRScaffold.cpp
)
@@ -48,10 +54,12 @@ endif()
surgsim_add_library(
TrackIRDevice
- "${TRACKIR_DEVICE_SOURCES}" "${TRACKIR_DEVICE_HEADERS}"
- SurgSim/Devices/TrackIR
+ "${TRACKIR_DEVICE_SOURCES}"
+ "${TRACKIR_DEVICE_HEADERS}"
)
+target_link_libraries(TrackIRDevice ${LIBS})
+
if(BUILD_TESTING)
# The unit tests will be built whenever the library is built.
add_subdirectory(UnitTests)
diff --git a/SurgSim/Devices/TrackIR/TrackIRDevice.cpp b/SurgSim/Devices/TrackIR/TrackIRDevice.cpp
index ebff529..ed49305 100644
--- a/SurgSim/Devices/TrackIR/TrackIRDevice.cpp
+++ b/SurgSim/Devices/TrackIR/TrackIRDevice.cpp
@@ -20,11 +20,15 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
+SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::TrackIRDevice, TrackIRDevice);
+
TrackIRDevice::TrackIRDevice(const std::string& uniqueName) :
- SurgSim::Input::CommonDevice(uniqueName, TrackIRScaffold::buildDeviceInputData())
+ SurgSim::Input::CommonDevice(uniqueName, TrackIRScaffold::buildDeviceInputData()),
+ m_positionScale(1.0),
+ m_orientationScale(1.0)
{
}
@@ -36,27 +40,24 @@ TrackIRDevice::~TrackIRDevice()
}
}
-
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.";
+ auto scaffold = TrackIRScaffold::getOrCreateSharedInstance();
+ SURGSIM_ASSERT(scaffold != nullptr) << "TrackIRDevice::initialize(): Failed to obtain a TrackIR scaffold.";
- if (!scaffold->registerDevice(this))
+ bool initialize = false;
+ if (scaffold->registerDevice(this))
{
- return false;
+ m_scaffold = std::move(scaffold);
+ initialize = true;
}
-
- m_scaffold = std::move(scaffold);
- SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Initialized.";
- return true;
+ return initialize;
}
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;
@@ -108,5 +109,5 @@ double TrackIRDevice::defaultOrientationScale()
return 1.0;
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/TrackIR/TrackIRDevice.h b/SurgSim/Devices/TrackIR/TrackIRDevice.h
index b620f94..eee9e1e 100644
--- a/SurgSim/Devices/TrackIR/TrackIRDevice.h
+++ b/SurgSim/Devices/TrackIR/TrackIRDevice.h
@@ -23,12 +23,16 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
class TrackIRScaffold;
+SURGSIM_STATIC_REGISTRATION(TrackIRDevice);
+
/// A class implementing the communication with Natural Point TrackIR camera.
-///
+/// Z is the direction that the camera faces.
+/// Y is in the direction of the camera's up.
+/// X is the direction to the camera's left (making a right-hand coordinate system).
/// \par Application input provided by the device:
/// | type | name | |
/// | ---- | ---- | --- |
@@ -44,19 +48,14 @@ public:
/// \param uniqueName A unique name for the device.
explicit TrackIRDevice(const std::string& uniqueName);
+ SURGSIM_CLASSNAME(SurgSim::Devices::TrackIRDevice);
+
/// 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;
+ bool initialize() override;
- /// Check whether this device is initialized.
- /// \return True if this device is initialized; false otherwise.
- bool isInitialized() const;
+ bool isInitialized() const override;
/// Sets the position scale for this device.
/// The position scale controls how much the pose changes for a given device translation.
@@ -75,6 +74,8 @@ public:
private:
friend class TrackIRScaffold;
+ bool finalize() override;
+
// Returns the default position scale
static double defaultPositionScale();
// Returns the default rotation scale
@@ -89,7 +90,7 @@ private:
std::shared_ptr<TrackIRScaffold> m_scaffold;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_TRACKIR_TRACKIRDEVICE_H
diff --git a/SurgSim/Devices/TrackIR/TrackIRScaffold.h b/SurgSim/Devices/TrackIR/TrackIRScaffold.h
index 33a1128..c8fb6ef 100644
--- a/SurgSim/Devices/TrackIR/TrackIRScaffold.h
+++ b/SurgSim/Devices/TrackIR/TrackIRScaffold.h
@@ -28,38 +28,27 @@ namespace DataStructures
class DataGroup;
}
-namespace Device
+namespace Devices
{
class TrackIRDevice;
/// A class that manages Natural Point TRACKIR devices.
///
-/// \sa SurgSim::Device::TrackIRDevice
+/// \sa SurgSim::Devices::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);
+ TrackIRScaffold();
/// 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.
+ /// The scaffold is managed using a SharedInstance 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;
@@ -134,14 +123,11 @@ private:
/// 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 Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_TRACKIR_TRACKIRSCAFFOLD_H
diff --git a/SurgSim/Devices/TrackIR/TrackIRThread.cpp b/SurgSim/Devices/TrackIR/TrackIRThread.cpp
index 559fee8..687c103 100644
--- a/SurgSim/Devices/TrackIR/TrackIRThread.cpp
+++ b/SurgSim/Devices/TrackIR/TrackIRThread.cpp
@@ -19,7 +19,7 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
TrackIRThread::TrackIRThread(TrackIRScaffold* scaffold, TrackIRScaffold::DeviceData* deviceData) :
@@ -27,6 +27,7 @@ TrackIRThread::TrackIRThread(TrackIRScaffold* scaffold, TrackIRScaffold::DeviceD
m_scaffold(scaffold),
m_deviceData(deviceData)
{
+ setRate(100.0);
}
TrackIRThread::~TrackIRThread()
@@ -48,5 +49,5 @@ bool TrackIRThread::doUpdate(double dt)
return m_scaffold->runInputFrame(m_deviceData);
}
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/TrackIR/TrackIRThread.h b/SurgSim/Devices/TrackIR/TrackIRThread.h
index 7e3f011..6d13a51 100644
--- a/SurgSim/Devices/TrackIR/TrackIRThread.h
+++ b/SurgSim/Devices/TrackIR/TrackIRThread.h
@@ -21,11 +21,11 @@
namespace SurgSim
{
-namespace Device
+namespace Devices
{
/// A class implementing the thread context for sampling TrackIR devices.
-/// \sa SurgSim::Device::TrackIRScaffold
+/// \sa SurgSim::Devices::TrackIRScaffold
class TrackIRThread : public SurgSim::Framework::BasicThread
{
public:
@@ -42,14 +42,14 @@ public:
protected:
/// Initialize this thread.
/// \return True on success, false otherwise.
- virtual bool doInitialize() override;
+ bool doInitialize() override;
/// Start up this thread.
/// \return True on success, false otherwise.
- virtual bool doStartUp() override;
+ 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;
+ bool doUpdate(double dt) override;
private:
// Pointer to the scaffold which will be updated by this thread.
@@ -58,7 +58,7 @@ private:
TrackIRScaffold::DeviceData* m_deviceData;
};
-}; // namespace Device
+}; // namespace Devices
}; // namespace SurgSim
#endif // SURGSIM_DEVICES_TRACKIR_TRACKIRTHREAD_H
diff --git a/SurgSim/Devices/TrackIR/UnitTests/TrackIRDeviceTest.cpp b/SurgSim/Devices/TrackIR/UnitTests/TrackIRDeviceTest.cpp
index b49818d..acc2a24 100644
--- a/SurgSim/Devices/TrackIR/UnitTests/TrackIRDeviceTest.cpp
+++ b/SurgSim/Devices/TrackIR/UnitTests/TrackIRDeviceTest.cpp
@@ -29,7 +29,7 @@
#include "SurgSim/Input/InputConsumerInterface.h"
#include "SurgSim/Testing/MockInputOutput.h"
-using SurgSim::Device::TrackIRDevice;
+using SurgSim::Devices::TrackIRDevice;
using SurgSim::DataStructures::DataGroup;
using SurgSim::Input::InputConsumerInterface;
using SurgSim::Testing::MockInputOutput;
@@ -48,20 +48,6 @@ TEST(TrackIRDeviceTest, CreateAndInitializeDevice)
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");
@@ -96,14 +82,14 @@ TEST(TrackIRDeviceTest, InputConsumer)
// 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.)
+ // (The thread to poll data out of TrackIR is running at default 100Hz.)
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_GE(consumer->m_numTimesReceivedInput, 50);
+ EXPECT_LE(consumer->m_numTimesReceivedInput, 120);
EXPECT_TRUE(consumer->m_lastReceivedInput.poses().isValid());
}
diff --git a/SurgSim/Devices/TrackIR/UnitTests/TrackIRScaffoldTest.cpp b/SurgSim/Devices/TrackIR/UnitTests/TrackIRScaffoldTest.cpp
index c3def6c..012bcc3 100644
--- a/SurgSim/Devices/TrackIR/UnitTests/TrackIRScaffoldTest.cpp
+++ b/SurgSim/Devices/TrackIR/UnitTests/TrackIRScaffoldTest.cpp
@@ -23,8 +23,8 @@
#include "SurgSim/Devices/TrackIR/TrackIRDevice.h"
#include "SurgSim/Devices/TrackIR/TrackIRScaffold.h"
-using SurgSim::Device::TrackIRDevice;
-using SurgSim::Device::TrackIRScaffold;
+using SurgSim::Devices::TrackIRDevice;
+using SurgSim::Devices::TrackIRScaffold;
TEST(TrackIRScaffoldTest, CreateAndDestroyScaffold)
{
@@ -155,22 +155,6 @@ TEST(TrackIRScaffoldTest, CreateDeviceSeveralTimes)
}
}
-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;
diff --git a/SurgSim/Devices/TrackIR/VisualTest/CMakeLists.txt b/SurgSim/Devices/TrackIR/VisualTest/CMakeLists.txt
index af4617c..f739e44 100644
--- a/SurgSim/Devices/TrackIR/VisualTest/CMakeLists.txt
+++ b/SurgSim/Devices/TrackIR/VisualTest/CMakeLists.txt
@@ -20,19 +20,14 @@ set(EXAMPLE_SOURCES
set(EXAMPLE_HEADERS
)
-add_executable(TrackIRVisualTest
- ${EXAMPLE_SOURCES} ${EXAMPLE_HEADERS})
-surgsim_show_ide_folders(
+surgsim_add_executable(TrackIRVisualTest
"${EXAMPLE_SOURCES}" "${EXAMPLE_HEADERS}")
set(LIBS
IdentityPoseDevice
+ SurgSimInput
TrackIRDevice
VisualTestCommon
- SurgSimDataStructures
- SurgSimFramework
- SurgSimInput
- ${OPENGL_LIBRARIES}
${OPTITRACK_LIBRARY}
)
diff --git a/SurgSim/Devices/TrackIR/VisualTest/main.cpp b/SurgSim/Devices/TrackIR/VisualTest/main.cpp
index aaecef1..7746f42 100644
--- a/SurgSim/Devices/TrackIR/VisualTest/main.cpp
+++ b/SurgSim/Devices/TrackIR/VisualTest/main.cpp
@@ -22,8 +22,8 @@
#include "SurgSim/Testing/VisualTestCommon/ToolSquareTest.h"
using SurgSim::Input::DeviceInterface;
-using SurgSim::Device::TrackIRDevice;
-using SurgSim::Device::IdentityPoseDevice;
+using SurgSim::Devices::TrackIRDevice;
+using SurgSim::Devices::IdentityPoseDevice;
int main(int argc, char** argv)
diff --git a/SurgSim/Devices/TrackIR/linux/TrackIRScaffold.cpp b/SurgSim/Devices/TrackIR/linux/TrackIRScaffold.cpp
index e323305..bf2b2f3 100644
--- a/SurgSim/Devices/TrackIR/linux/TrackIRScaffold.cpp
+++ b/SurgSim/Devices/TrackIR/linux/TrackIRScaffold.cpp
@@ -43,7 +43,7 @@ using SurgSim::Math::Vector3d;
namespace SurgSim
{
-namespace Device
+namespace Devices
{
struct TrackIRScaffold::DeviceData
@@ -59,9 +59,9 @@ struct TrackIRScaffold::DeviceData
}
/// The corresponding device object.
- SurgSim::Device::TrackIRDevice* const deviceObject;
+ SurgSim::Devices::TrackIRDevice* const deviceObject;
/// Processing thread.
- std::unique_ptr<SurgSim::Device::TrackIRThread> thread;
+ std::unique_ptr<SurgSim::Devices::TrackIRThread> thread;
/// Scale factor for the position axes; stored locally before the device is initialized.
double positionScale;
@@ -101,16 +101,11 @@ private:
};
-TrackIRScaffold::TrackIRScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) :
- m_logger(logger),
+TrackIRScaffold::TrackIRScaffold() :
+ m_logger(Framework::Logger::getLogger("Devices/TrackIR")),
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.";
+ SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold created.";
}
@@ -145,11 +140,6 @@ TrackIRScaffold::~TrackIRScaffold()
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)
{
@@ -192,6 +182,7 @@ bool TrackIRScaffold::registerDevice(TrackIRDevice* device)
startCamera(info.get());
m_state->activeDeviceList.emplace_back(std::move(info));
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << " initialized.";
}
return m_state->isApiInitialized;
@@ -216,13 +207,11 @@ bool TrackIRScaffold::unregisterDevice(const TrackIRDevice* const device)
m_state->activeDeviceList.erase(matching);
// the iterator is now invalid but that's OK
found = true;
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << " unregistered.";
}
}
- if (!found)
- {
- SURGSIM_LOG_WARNING(m_logger) << "TrackIR: Attempted to release a non-registered device.";
- }
+ SURGSIM_LOG_IF(!found, m_logger, SEVERE) << "Attempted to release a non-registered device " << device->getName();
return found;
}
@@ -388,12 +377,5 @@ std::shared_ptr<TrackIRScaffold> TrackIRScaffold::getOrCreateSharedInstance()
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 Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/TrackIR/win32/TrackIRScaffold.cpp b/SurgSim/Devices/TrackIR/win32/TrackIRScaffold.cpp
index b5f6d4a..cb40820 100644
--- a/SurgSim/Devices/TrackIR/win32/TrackIRScaffold.cpp
+++ b/SurgSim/Devices/TrackIR/win32/TrackIRScaffold.cpp
@@ -43,7 +43,7 @@ using SurgSim::Math::Vector3d;
namespace SurgSim
{
-namespace Device
+namespace Devices
{
struct TrackIRScaffold::DeviceData
@@ -107,9 +107,9 @@ struct TrackIRScaffold::DeviceData
CameraLibrary::cModuleVectorProcessing* vectorProcessor;
/// The corresponding device object.
- SurgSim::Device::TrackIRDevice* const deviceObject;
+ SurgSim::Devices::TrackIRDevice* const deviceObject;
/// Processing thread.
- std::unique_ptr<SurgSim::Device::TrackIRThread> thread;
+ std::unique_ptr<SurgSim::Devices::TrackIRThread> thread;
/// The mutex that protects the externally modifiable parameters.
boost::mutex parametersMutex;
@@ -144,16 +144,11 @@ private:
};
-TrackIRScaffold::TrackIRScaffold(std::shared_ptr<SurgSim::Framework::Logger> logger) :
- m_logger(logger),
+TrackIRScaffold::TrackIRScaffold() :
+ m_logger(Framework::Logger::getLogger("Devices/TrackIR")),
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.";
+ SURGSIM_LOG_DEBUG(m_logger) << "Shared scaffold created.";
}
@@ -188,12 +183,6 @@ TrackIRScaffold::~TrackIRScaffold()
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);
@@ -240,6 +229,7 @@ bool TrackIRScaffold::registerDevice(TrackIRDevice* device)
startCamera(info.get());
m_state->activeDeviceList.emplace_back(std::move(info));
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << " initialized.";
}
else
{
@@ -270,13 +260,11 @@ bool TrackIRScaffold::unregisterDevice(const TrackIRDevice* const device)
m_state->activeDeviceList.erase(matching);
// the iterator is now invalid but that's OK
found = true;
+ SURGSIM_LOG_INFO(m_logger) << "Device " << device->getName() << " unregistered.";
}
}
- if (!found)
- {
- SURGSIM_LOG_WARNING(m_logger) << "TrackIR: Attempted to release a non-registered device.";
- }
+ SURGSIM_LOG_IF(!found, m_logger, SEVERE) << "Attempted to release a non-registered device " << device->getName();
return found;
}
@@ -362,12 +350,12 @@ bool TrackIRScaffold::updateDevice(TrackIRScaffold::DeviceData* info)
// 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()));
+ // The angles returned from CameraSDK are Euler angles (y-x'-z'' intrinsic rotations): X=pitch, Y=yaw, Z=roll.
+ // OSS use right handed coordinate system (X:Right, Y:Up, Z:Outward) and right hand rule for rotations.
+ Matrix33d rotationX = makeRotationMatrix(pitch * 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;
+ Matrix33d rotationZ = makeRotationMatrix(roll * M_PI / 180.0, Vector3d(Vector3d::UnitZ()));
+ Matrix33d orientation = rotationY * rotationX * rotationZ;
RigidTransform3d pose;
pose.linear() = orientation;
@@ -462,12 +450,5 @@ std::shared_ptr<TrackIRScaffold> TrackIRScaffold::getOrCreateSharedInstance()
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 Devices
}; // namespace SurgSim
diff --git a/SurgSim/Devices/UnitTests/CMakeLists.txt b/SurgSim/Devices/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..f2a8f9e
--- /dev/null
+++ b/SurgSim/Devices/UnitTests/CMakeLists.txt
@@ -0,0 +1,43 @@
+# 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
+ DeviceUtilitiesTests.cpp
+)
+
+set(UNIT_TEST_HEADERS
+)
+
+set(LIBS
+ IdentityPoseDevice
+ SurgSimDevices
+ SurgSimInput
+)
+
+surgsim_add_unit_tests(SurgSimDevicesTest)
+
+set_target_properties(SurgSimDevicesTest PROPERTIES FOLDER "Devices")
+
+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"
+)
diff --git a/SurgSim/Devices/UnitTests/Data/noInitialize.yaml b/SurgSim/Devices/UnitTests/Data/noInitialize.yaml
new file mode 100644
index 0000000..35f1f7c
--- /dev/null
+++ b/SurgSim/Devices/UnitTests/Data/noInitialize.yaml
@@ -0,0 +1,2 @@
+- MockDeviceNoInitialize:
+ Name: Device
diff --git a/SurgSim/Devices/UnitTests/Data/noName.yaml b/SurgSim/Devices/UnitTests/Data/noName.yaml
new file mode 100644
index 0000000..d47c279
--- /dev/null
+++ b/SurgSim/Devices/UnitTests/Data/noName.yaml
@@ -0,0 +1,2 @@
+- SurgSim::Devices::IdentityPoseDevice:
+ NoName: Device
\ No newline at end of file
diff --git a/SurgSim/Devices/UnitTests/Data/notMap.yaml b/SurgSim/Devices/UnitTests/Data/notMap.yaml
new file mode 100644
index 0000000..da3289a
--- /dev/null
+++ b/SurgSim/Devices/UnitTests/Data/notMap.yaml
@@ -0,0 +1 @@
+- SurgSim::Devices::NovintDevice
diff --git a/SurgSim/Devices/UnitTests/Data/notRegistered.yaml b/SurgSim/Devices/UnitTests/Data/notRegistered.yaml
new file mode 100644
index 0000000..1380d85
--- /dev/null
+++ b/SurgSim/Devices/UnitTests/Data/notRegistered.yaml
@@ -0,0 +1,2 @@
+- SurgSim::Devices::NotADevice:
+ Name: Device
diff --git a/SurgSim/Devices/UnitTests/Data/notSequence.yaml b/SurgSim/Devices/UnitTests/Data/notSequence.yaml
new file mode 100644
index 0000000..90db57a
--- /dev/null
+++ b/SurgSim/Devices/UnitTests/Data/notSequence.yaml
@@ -0,0 +1 @@
+SurgSim::Devices::InputPoseDevice:
diff --git a/SurgSim/Devices/UnitTests/Data/success.yaml b/SurgSim/Devices/UnitTests/Data/success.yaml
new file mode 100644
index 0000000..faab945
--- /dev/null
+++ b/SurgSim/Devices/UnitTests/Data/success.yaml
@@ -0,0 +1,4 @@
+- SurgSim::Devices::IdentityPoseDevice:
+ Name: Device1
+- SurgSim::Devices::IdentityPoseDevice:
+ Name: Device2
diff --git a/SurgSim/Devices/UnitTests/DeviceUtilitiesTests.cpp b/SurgSim/Devices/UnitTests/DeviceUtilitiesTests.cpp
new file mode 100644
index 0000000..e3df527
--- /dev/null
+++ b/SurgSim/Devices/UnitTests/DeviceUtilitiesTests.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.
+
+/// \file
+/// Tests for the DeviceUtilities.
+
+#include <memory>
+#include <string>
+#include <gtest/gtest.h>
+
+#include "SurgSim/Devices/DeviceUtilities.h"
+#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Input/DeviceInterface.h"
+
+namespace
+{
+class MockDeviceNoInitialize : public SurgSim::Devices::IdentityPoseDevice
+{
+public:
+ explicit MockDeviceNoInitialize(const std::string& name) : SurgSim::Devices::IdentityPoseDevice(name)
+ {
+ }
+
+ std::string getClassName() const override
+ {
+ return "MockDeviceNoInitialize";
+ }
+
+ bool initialize() override
+ {
+ return false;
+ }
+};
+}
+
+TEST(DeviceUtilitiesTests, CreateDevice)
+{
+ const std::string name = "name";
+ std::vector<std::string> types;
+ EXPECT_EQ(nullptr, SurgSim::Devices::createDevice(types, name));
+
+ types.push_back("DoNotHave");
+ EXPECT_EQ(nullptr, SurgSim::Devices::createDevice(types, name));
+
+ types.push_back("SurgSim::Devices::IdentityPoseDevice");
+ auto device = SurgSim::Devices::createDevice(types, name);
+ ASSERT_NE(nullptr, device);
+ EXPECT_NE(nullptr, std::dynamic_pointer_cast<SurgSim::Devices::IdentityPoseDevice>(device));
+ EXPECT_EQ(name, device->getName());
+}
+
+TEST(DeviceUtilitiesTests, LoadDevice)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ std::shared_ptr<SurgSim::Input::DeviceInterface> device;
+
+ EXPECT_ANY_THROW(device = SurgSim::Devices::loadDevice("noFile.yaml"));
+ ASSERT_EQ(nullptr, device);
+
+ EXPECT_NO_THROW(device = SurgSim::Devices::loadDevice("notSequence.yaml"));
+ ASSERT_EQ(nullptr, device);
+
+ EXPECT_NO_THROW(device = SurgSim::Devices::loadDevice("notMap.yaml"));
+ ASSERT_EQ(nullptr, device);
+
+ EXPECT_NO_THROW(device = SurgSim::Devices::loadDevice("notRegistered.yaml"));
+ ASSERT_EQ(nullptr, device);
+
+ EXPECT_NO_THROW(device = SurgSim::Devices::loadDevice("noName.yaml"));
+ ASSERT_EQ(nullptr, device);
+
+ SurgSim::Input::DeviceInterface::getFactory().registerClass<MockDeviceNoInitialize>("MockDeviceNoInitialize");
+ EXPECT_NO_THROW(device = SurgSim::Devices::loadDevice("noInitialize.yaml"));
+ ASSERT_EQ(nullptr, device);
+
+ EXPECT_NO_THROW(device = SurgSim::Devices::loadDevice("success.yaml"));
+ ASSERT_NE(nullptr, device);
+ EXPECT_EQ("Device1", device->getName());
+}
\ No newline at end of file
diff --git a/SurgSim/Blocks/UnitTests/config.txt.in b/SurgSim/Devices/UnitTests/config.txt.in
similarity index 100%
copy from SurgSim/Blocks/UnitTests/config.txt.in
copy to SurgSim/Devices/UnitTests/config.txt.in
diff --git a/SurgSim/Devices/devices.dox b/SurgSim/Devices/devices.dox
index f4ec6ac..ac8e6a8 100644
--- a/SurgSim/Devices/devices.dox
+++ b/SurgSim/Devices/devices.dox
@@ -6,10 +6,14 @@ The 'Devices' namespace contains classes that add physical devices (e.g., mice,
Devices with Dependencies:
- \subpage LabJack
+- \subpage Leap
- \subpage MultiAxis
+- \subpage Nimble
- \subpage Novint
+- \subpage Oculus
+- \subpage OpenNI
- \subpage Phantom
- \subpage Sixense
- \subpage TrackIR
-*/
\ No newline at end of file
+*/
diff --git a/SurgSim/Framework/Accessible.cpp b/SurgSim/Framework/Accessible.cpp
index 02d377a..1e7e89c 100644
--- a/SurgSim/Framework/Accessible.cpp
+++ b/SurgSim/Framework/Accessible.cpp
@@ -13,7 +13,9 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include <algorithm>
#include "SurgSim/Framework/Accessible.h"
+#include "SurgSim/Framework/Log.h"
#include "SurgSim/Math/Matrix.h"
namespace SurgSim
@@ -41,7 +43,7 @@ boost::any Accessible::getValue(const std::string& name) const
}
else
{
- SURGSIM_FAILURE() << "Can't get property: " << name << "." << ((functors == std::end(m_functors)) ?
+ SURGSIM_FAILURE() << "Can't get property: " << name << ". " << ((functors == std::end(m_functors)) ?
"Property not found." : "No getter defined for property.");
return boost::any();
}
@@ -63,7 +65,7 @@ void Accessible::setValue(const std::string& name, const boost::any& value)
}
else
{
- SURGSIM_FAILURE() << "Can't set property: " << name << "." << ((functors == std::end(m_functors)) ?
+ SURGSIM_FAILURE() << "Can't set property: " << name << ". " << ((functors == std::end(m_functors)) ?
"Property not found." : "No setter defined for property.");
}
}
@@ -72,14 +74,12 @@ void Accessible::setValue(const std::string& name, const boost::any& value)
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;
}
@@ -122,6 +122,13 @@ void Accessible::setSerializable(const std::string& name, EncoderType encoder, D
m_functors[name].decoder = decoder;
}
+void Accessible::setDecoder(const std::string& name, DecoderType decoder)
+{
+ SURGSIM_ASSERT(decoder != nullptr) << "Decoder functor can't be nullptr";
+
+ m_functors[name].decoder = decoder;
+}
+
YAML::Node Accessible::encode() const
{
YAML::Node result;
@@ -136,26 +143,47 @@ YAML::Node Accessible::encode() const
return result;
}
-void Accessible::decode(const YAML::Node& node)
+void Accessible::decode(const YAML::Node& node, const std::vector<std::string>& ignoredProperties)
{
- SURGSIM_ASSERT(node.IsMap()) << "Node to decode accessible has to be map.";
- for (auto functors = m_functors.cbegin(); functors != m_functors.cend(); ++functors)
+ SURGSIM_LOG_DEBUG(SurgSim::Framework::Logger::getLogger("Framework/Accessible")) <<
+ "Decoding node: \n" << node;
+ SURGSIM_ASSERT(node.IsMap()) << "Node to be decoded has to be map.";
+
+ for (auto data = node.begin(); data != node.end(); ++data)
{
- auto decoder = functors->second.decoder;
- if (decoder != nullptr)
+ std::string name = data->first.as<std::string>();
+ if (std::find(std::begin(ignoredProperties), std::end(ignoredProperties), name) != std::end(ignoredProperties))
+ {
+ continue;
+ }
+
+ auto functors = m_functors.find(name);
+ if (functors == std::end(m_functors) || !functors->second.decoder)
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getLogger("Framework/Accessible"))
+ << "Can't find property with name " << name << " in the accessible, while trying to set " <<
+ data->second;
+ }
+ else
{
- YAML::Node temporary = node[functors->first];
+ YAML::Node temporary = data->second;
if (!temporary.IsNull() && temporary.IsDefined())
{
try
{
- decoder(&temporary);
+ functors->second.decoder(&temporary);
}
catch (std::exception e)
{
- SURGSIM_FAILURE() << e.what();
+ SURGSIM_FAILURE() << e.what() << " for value " << std::endl << temporary;
}
}
+ else
+ {
+ SURGSIM_LOG_INFO(SurgSim::Framework::Logger::getLogger("Framework/Accessible"))
+ << "Found property with name " << name << " in the accessible."
+ << " But it seems no value is specified for this property in the YAML file.";
+ }
}
}
}
@@ -168,6 +196,8 @@ void Accessible::forwardProperty(const std::string& name, const Accessible& targ
{
functors.getter = found->second.getter;
functors.setter = found->second.setter;
+ functors.encoder = found->second.encoder;
+ functors.decoder = found->second.decoder;
m_functors[name] = std::move(functors);
}
else
@@ -195,6 +225,20 @@ SurgSim::Math::Matrix44f convert(boost::any val)
return floatResult;
}
+template<>
+std::string convert(boost::any val)
+{
+ std::string result;
+ try
+ {
+ result = std::string(boost::any_cast<const char*>(val));
+ }
+ catch (boost::bad_any_cast&)
+ {
+ result = std::string(boost::any_cast<std::string>(val));
+ }
+ return result;
+}
}; // Framework
}; // SurgSim
diff --git a/SurgSim/Framework/Accessible.h b/SurgSim/Framework/Accessible.h
index b929ef7..3b2ca0d 100644
--- a/SurgSim/Framework/Accessible.h
+++ b/SurgSim/Framework/Accessible.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,11 +16,12 @@
#ifndef SURGSIM_FRAMEWORK_ACCESSIBLE_H
#define SURGSIM_FRAMEWORK_ACCESSIBLE_H
-#include <string>
+#include <boost/any.hpp>
+#include <boost/preprocessor.hpp>
+#include <functional>
#include <memory>
+#include <string>
#include <unordered_map>
-#include <functional>
-#include <boost/any.hpp>
#include <yaml-cpp/yaml.h>
#include "SurgSim/Math/Matrix.h"
@@ -49,8 +50,6 @@ public:
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.
@@ -66,7 +65,6 @@ public:
/// \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
@@ -125,7 +123,7 @@ public:
/// \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
+ /// Sets the functions used to convert data from and to a YAML::Node. Will throw an 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.
@@ -133,6 +131,15 @@ public:
/// in the instance.
void setSerializable(const std::string& name, EncoderType encoder, DecoderType decoder);
+ /// Sets the functions used to convert data from a YAML::Node.
+ /// This leaves the encoder (class -> YAML) conversion empty, this can be used to let the user decide how to
+ /// model the data in the data file, inside the class this should all result in one member to be created/changed.
+ /// \param name The name of the property.
+ /// \param decoder The function to be used to read the property from the node and set it
+ /// in the instance.
+ void setDecoder(const std::string& name, DecoderType decoder);
+
+
/// Encode this Accessible to a YAML::Node
/// \return The encoded version of this instance.
YAML::Node encode() const;
@@ -140,9 +147,10 @@ public:
/// 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);
+ /// \param node The node that carries the data to be decoded, properties with names that don't
+ /// match up with properties in the Accessible will be reported.
+ /// \param ignoredProperties Properties that will be ignored.
+ void decode(const YAML::Node& node, const std::vector<std::string>& ignoredProperties = std::vector<std::string>());
private:
@@ -155,6 +163,7 @@ private:
/// Private struct to keep the map under control
struct Functors
{
+ Functors() : getter(nullptr), setter(nullptr), encoder(nullptr), decoder(nullptr) {}
GetterType getter;
SetterType setter;
EncoderType encoder;
@@ -192,6 +201,12 @@ T convert(boost::any val);
template <>
SurgSim::Math::Matrix44f convert(boost::any val);
+/// Specialization for convert<T>() to correctly cast const char* to std::string
+/// \param val The value to be converted, should be a const char*
+/// \return A std::string
+template <>
+std::string 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.
@@ -215,9 +230,72 @@ SurgSim::Math::Matrix44f convert(boost::any val);
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)))
+/// A macro to register a setter that can be used from YAML, and as a writeable property
+/// use this to provide alternatives to more complicated values, e.g. setModelFilename() vs generate and set Model
+/// Enables the alternative use of the model file instead of the actual mesh object
+#define SURGSIM_ADD_SETTER(class, type, property, setter) \
+ {\
+ setDecoder(#property, std::bind((void(class::*)(const type&))&class::setter, this,\
+ std::bind(&YAML::Node::as<type>,std::placeholders::_1))); \
+ setSetter(#property, std::bind((void(class::*)(const type&))&class::setter, this,\
+ std::bind(SurgSim::Framework::convert<type>,std::placeholders::_1)));\
+ }
+
+
}; // Framework
}; // SurgSim
+#define SURGSIM_ENUM_TOSTRING(r, data, elem) \
+ case data::elem: \
+ result = BOOST_PP_STRINGIZE(elem); \
+ break;
+
+#define SURGSIM_ENUM_FROMSTRING(r, data, elem) \
+ if (value == BOOST_PP_STRINGIZE(elem)) \
+ { \
+ rhs = data::elem; \
+ return true; \
+ }
+
+/// Required type of enums used by SURGSIM_SERIALIZABLE_ENUM
+#define SURGSIM_ENUM_TYPE int8_t
+
+/// A macro to create an enum that can be easily serialized
+/// During serialization, the enum will be converted to its string based named,
+/// back to an enum during deserialization. The enum must already be forward
+/// declared in its namespace before calling this macro in the global namespace.
+#define SURGSIM_SERIALIZABLE_ENUM(name, enumerators) \
+ enum name : SURGSIM_ENUM_TYPE\
+ { \
+ BOOST_PP_SEQ_ENUM(enumerators) \
+ }; \
+ namespace YAML \
+ { \
+ template <> \
+ struct convert<name> \
+ { \
+ static Node encode(const name& rhs) \
+ { \
+ Node result; \
+ switch (rhs) \
+ { \
+ BOOST_PP_SEQ_FOR_EACH(SURGSIM_ENUM_TOSTRING, name, enumerators) \
+ default: \
+ SURGSIM_FAILURE() << "Can not find enum value in " << #name << ": " << rhs; \
+ } \
+ return result; \
+ } \
+ static bool decode(const Node& node, name& rhs) \
+ { \
+ std::string value = node.as<std::string>(); \
+ std::transform(value.begin(), value.end(), value.begin(), ::toupper); \
+ BOOST_PP_SEQ_FOR_EACH(SURGSIM_ENUM_FROMSTRING, name, enumerators) \
+ SURGSIM_FAILURE() << "Unknown " << #name << ": " << value; \
+ return false; \
+ } \
+ }; \
+ }
+
#include "SurgSim/Framework/Accessible-inl.h"
-#endif
\ No newline at end of file
+#endif
diff --git a/SurgSim/Framework/ApplicationData.cpp b/SurgSim/Framework/ApplicationData.cpp
index 1c5f289..fe725c1 100644
--- a/SurgSim/Framework/ApplicationData.cpp
+++ b/SurgSim/Framework/ApplicationData.cpp
@@ -67,14 +67,21 @@ std::string ApplicationData::findFile(const std::string& fileName) const
}
path file(fileName);
- for (auto it = m_paths.cbegin(); it != m_paths.cend(); ++it)
+ if (file.is_absolute() && boost::filesystem::exists(file))
+ {
+ result = file.make_preferred().string();
+ }
+ else
{
- path filePath(*it);
- filePath /= fileName;
- if (boost::filesystem::exists(filePath))
+ for (auto it = m_paths.cbegin(); it != m_paths.cend(); ++it)
{
- result = filePath.make_preferred().string();
- break;
+ path filePath(*it);
+ filePath /= fileName;
+ if (boost::filesystem::exists(filePath))
+ {
+ result = filePath.make_preferred().string();
+ break;
+ }
}
}
return result;
diff --git a/SurgSim/Framework/Asset.cpp b/SurgSim/Framework/Asset.cpp
index 5a7afcb..472d4b4 100644
--- a/SurgSim/Framework/Asset.cpp
+++ b/SurgSim/Framework/Asset.cpp
@@ -27,6 +27,12 @@ namespace Framework
Asset::Asset() : m_fileName()
{
+ serializeFileName(this);
+}
+
+Asset::Asset(const Asset& rhs)
+{
+ serializeFileName(this);
}
Asset::~Asset()
@@ -54,20 +60,20 @@ std::string Asset::getFileName() const
return m_fileName;
}
-void Asset::serializeFileName(SurgSim::Framework::Accessible* accesible)
+void Asset::serializeFileName(SurgSim::Framework::Accessible* accessible)
{
// 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)));
+ accessible->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)));
+ accessible->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
diff --git a/SurgSim/Framework/Asset.h b/SurgSim/Framework/Asset.h
index 815826e..db6a81b 100644
--- a/SurgSim/Framework/Asset.h
+++ b/SurgSim/Framework/Asset.h
@@ -17,6 +17,8 @@
#define SURGSIM_FRAMEWORK_ASSET_H
#include <string>
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Framework/Accessible.h"
namespace SurgSim
{
@@ -30,13 +32,20 @@ class AssetTest;
/// 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
+/// Components that take assets as parameters should, in addition to their general `set<Asset>()` and `get<Asset>()`
+/// functions, implement a helper function `load<Asset>()`, that creates and loads the specified asset.
+/// Additionally, a special YAML parameter (`<Asset>FileName`) should be implemented, that can be used to specify
+/// the asset, without having to specify the whole asset in serialized form.
+class Asset : virtual public Accessible, public FactoryBase<Asset>
{
friend AssetTest;
public:
/// Constructor
Asset();
+ /// Copy Constructor
+ Asset(const Asset& rhs);
+
/// Destructor
virtual ~Asset();
@@ -57,6 +66,10 @@ public:
/// \return Name of the file loaded by this class.
std::string getFileName() const;
+ /// Support serialization with a classname
+ /// \return the name of this class
+ virtual std::string getClassName() const = 0;
+
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.
@@ -64,12 +77,12 @@ protected:
/// \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.
+private:
+ /// Wrap the registration calls for the filename property, which is more complicated due to the overloaded
+ /// function call load()
/// \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;
};
@@ -77,4 +90,4 @@ private:
} // namespace Framework
} // namespace SurgSim
-#endif // SURGSIM_FRAMEWORK_ASSET_H
\ No newline at end of file
+#endif // SURGSIM_FRAMEWORK_ASSET_H
diff --git a/SurgSim/Framework/BasicSceneElement.h b/SurgSim/Framework/BasicSceneElement.h
index 060bfba..717e12e 100644
--- a/SurgSim/Framework/BasicSceneElement.h
+++ b/SurgSim/Framework/BasicSceneElement.h
@@ -40,7 +40,7 @@ public:
protected:
/// Initializes the scene element
/// \return True if succeeds, false if fails
- virtual bool doInitialize() override;
+ bool doInitialize() override;
};
diff --git a/SurgSim/Framework/BasicThread.cpp b/SurgSim/Framework/BasicThread.cpp
index 73a067b..961337f 100644
--- a/SurgSim/Framework/BasicThread.cpp
+++ b/SurgSim/Framework/BasicThread.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -29,6 +29,7 @@ namespace Framework
{
BasicThread::BasicThread(const std::string& name) :
+ m_logger(Logger::getLogger(name)),
m_name(name),
m_period(1.0 / 30),
m_isIdle(false),
@@ -37,6 +38,11 @@ BasicThread::BasicThread(const std::string& name) :
m_stopExecution(false),
m_isSynchronous(false)
{
+ // The maximum number of frames in the timer is set to 1,000,000
+ // + If the timer is reset every second, that is enough frame to measure real rates up to 1MHz
+ // + If the timer is reset every minute, that is enough frame to measure real rates up to 16.66KHz
+ // + If the timer is reset every hour, that is enough frame to measure real rates up to 277.77Hz
+ m_timer.setMaxNumberOfFrames(1000000);
}
#ifdef _MSC_VER
@@ -80,6 +86,8 @@ bool BasicThread::startUp()
void BasicThread::start(std::shared_ptr<Barrier> startupBarrier, bool isSynchronized)
{
+ boost::unique_lock<boost::mutex> lock(m_mutexStartStop);
+
m_startupBarrier = startupBarrier;
m_stopExecution = false;
m_isRunning = false;
@@ -87,6 +95,7 @@ void BasicThread::start(std::shared_ptr<Barrier> startupBarrier, bool isSynchron
// Start the thread with a reference to this
// prevents making a copy
+ m_isRunning = true;
m_thisThread = boost::thread(boost::ref(*this));
}
@@ -100,30 +109,37 @@ void BasicThread::operator()()
bool success = executeInitialization();
if (! success)
{
+ m_isRunning = false;
return;
}
- boost::chrono::duration<double> frameTime(0.0);
+
+ size_t numUpdates = 0;
+ boost::chrono::duration<double> totalFrameTime(0.0);
boost::chrono::duration<double> sleepTime(0.0);
+ boost::chrono::duration<double> totalSleepTime(0.0);
Clock::time_point start;
- m_isRunning = true;
+ m_timer.start();
while (m_isRunning && !m_stopExecution)
{
+ start = Clock::now();
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_timer.beginFrame();
m_isRunning = doUpdate(m_period.count());
+ m_timer.endFrame();
+ }
+
+ // Check for frameTime being > desired update period report error, adjust ...
+ sleepTime = m_period - (Clock::now() - start);
+ if (sleepTime.count() > 0.0)
+ {
+ totalSleepTime += sleepTime;
+ SurgSim::Framework::sleep_until(start + m_period);
}
- frameTime = Clock::now() - start;
}
else
{
@@ -132,9 +148,13 @@ void BasicThread::operator()()
// all the threads that are waiting to indefinitely wait as there is one less thread on the barrier
// #threadsafety
bool success = waitForBarrier(true);
+ totalSleepTime += Clock::now() - start;
+
if (success && !m_isIdle)
{
+ m_timer.beginFrame();
m_isRunning = doUpdate(m_period.count());
+ m_timer.endFrame();
}
if (! success || !m_isRunning)
{
@@ -142,6 +162,23 @@ void BasicThread::operator()()
m_isSynchronous = false;
}
}
+ totalFrameTime += Clock::now() - start;
+ numUpdates++;
+
+ if (m_logger->getThreshold() <= SURGSIM_LOG_LEVEL(INFO))
+ {
+ if (totalFrameTime.count() > 5.0)
+ {
+ SURGSIM_LOG_INFO(m_logger) << std::setprecision(4)
+ << "Rate: " << numUpdates / totalFrameTime.count() << "Hz / "
+ << 1.0 / m_period.count() << "Hz, "
+ << "Average doUpdate: " << (totalFrameTime.count() - totalSleepTime.count()) / numUpdates << "s, "
+ << "Sleep: " << 100.0 * totalSleepTime.count() / totalFrameTime.count() << "%";
+ totalFrameTime = boost::chrono::duration<double>::zero();
+ totalSleepTime = boost::chrono::duration<double>::zero();
+ numUpdates = 0;
+ }
+ }
}
doBeforeStop();
@@ -152,14 +189,15 @@ void BasicThread::operator()()
void BasicThread::stop()
{
+ boost::unique_lock<boost::mutex> lock(m_mutexStartStop);
+
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.";
+ SURGSIM_LOG_INFO(m_logger) << "Thread is detached, cannot wait for it to stop.";
}
else
{
@@ -168,8 +206,7 @@ void BasicThread::stop()
}
else
{
- SURGSIM_LOG_INFO(Logger::getDefaultLogger()) << "Thread " << getName() <<
- " is in synchronouse mode, stop with a barrier->wait(false).";
+ SURGSIM_LOG_INFO(m_logger) << "Thread is in synchronouse mode, stop with a barrier->wait(false).";
}
}
@@ -194,7 +231,7 @@ bool BasicThread::executeInitialization()
success = initialize();
SURGSIM_ASSERT(success) << "Initialization has failed for thread " << getName();
- SURGSIM_LOG_INFO(Logger::getDefaultLogger()) << "Initialization has succeeded for thread " << getName();
+ SURGSIM_LOG_INFO(m_logger) << "Initialization has succeeded for thread";
// 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
@@ -208,7 +245,7 @@ bool BasicThread::executeInitialization()
success = startUp();
SURGSIM_ASSERT(success) << "Startup has failed for thread " << getName();
- SURGSIM_LOG_INFO(Logger::getDefaultLogger()) << "Startup has succeeded for thread " << getName();
+ SURGSIM_LOG_INFO(m_logger) << "Startup has succeeded for thread " << getName();
// Waits for all the threads to startup and then proceeds
success = waitForBarrier(success);
@@ -239,6 +276,21 @@ bool BasicThread::isSynchronous()
return m_isSynchronous;
}
+double BasicThread::getCpuTime() const
+{
+ return m_timer.getCumulativeTime();
+}
+
+size_t BasicThread::getUpdateCount() const
+{
+ return m_timer.getCurrentNumberOfFrames();
+}
+
+void BasicThread::resetCpuTimeAndUpdateCount()
+{
+ m_timer.start();
+}
+
bool BasicThread::doUpdate(double dt)
{
return true;
diff --git a/SurgSim/Framework/BasicThread.h b/SurgSim/Framework/BasicThread.h
index a3124de..221349b 100644
--- a/SurgSim/Framework/BasicThread.h
+++ b/SurgSim/Framework/BasicThread.h
@@ -23,6 +23,7 @@
#include <boost/chrono.hpp>
#include "SurgSim/Framework/Barrier.h"
+#include "SurgSim/Framework/Timer.h"
namespace SurgSim
{
@@ -118,8 +119,23 @@ public:
/// \return true if synchronized, false if not.
bool isSynchronous();
+ /// \return the cumulated cpu time taken to run all update since last reset or thread creation
+ /// \note Only the latest 1,000,000 frames since last reset are cumulated, so if the timer is never reset,
+ /// \note the Cpu time will not increase past that limit.
+ double getCpuTime() const;
+
+ /// \return the number of updates done since last reset or thread creation
+ /// \note The update count since last reset has a limit of 1,000,000.
+ size_t getUpdateCount() const;
+
+ /// Reset the cpu time and the update count to 0
+ void resetCpuTimeAndUpdateCount();
+
protected:
+ /// Timer to measure the actual time taken to doUpdate
+ Timer m_timer;
+
/// Trigger the initialization of this object, this will be called before all other threads doStartup()
/// are called
/// \return true on success
@@ -135,6 +151,8 @@ protected:
virtual bool executeInitialization();
+ /// Logger for this thread
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
private:
std::string m_name;
@@ -143,6 +161,9 @@ private:
boost::chrono::duration<double> m_period;
std::shared_ptr<Barrier> m_startupBarrier;
+ // Protects the start and stop functions so on can only execute once the other is done
+ boost::mutex m_mutexStartStop;
+
bool m_isIdle;
bool m_isInitialized;
bool m_isRunning;
diff --git a/SurgSim/Framework/BehaviorManager.cpp b/SurgSim/Framework/BehaviorManager.cpp
index 0e3b6aa..f1f21a9 100644
--- a/SurgSim/Framework/BehaviorManager.cpp
+++ b/SurgSim/Framework/BehaviorManager.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -55,11 +55,8 @@ bool BehaviorManager::executeRemovals(const std::shared_ptr<SurgSim::Framework::
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);
+ processComponents();
return true;
}
diff --git a/SurgSim/Framework/BehaviorManager.h b/SurgSim/Framework/BehaviorManager.h
index e74cae6..7299156 100644
--- a/SurgSim/Framework/BehaviorManager.h
+++ b/SurgSim/Framework/BehaviorManager.h
@@ -35,16 +35,16 @@ public:
BehaviorManager();
~BehaviorManager();
- virtual int getType() const override;
+ 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;
+ bool executeAdditions(const std::shared_ptr<Component>& component) override;
+ bool executeRemovals(const std::shared_ptr<Component>& component) override;
private:
- virtual bool doInitialize() override;
- virtual bool doStartUp() override;
- virtual bool doUpdate(double dt) override;
+ bool doInitialize() override;
+ bool doStartUp() override;
+ bool doUpdate(double dt) override;
};
diff --git a/SurgSim/Framework/CMakeLists.txt b/SurgSim/Framework/CMakeLists.txt
index f6c2311..62854ef 100644
--- a/SurgSim/Framework/CMakeLists.txt
+++ b/SurgSim/Framework/CMakeLists.txt
@@ -33,8 +33,10 @@ set(SURGSIM_FRAMEWORK_SOURCES
PoseComponent.cpp
Representation.cpp
Runtime.cpp
+ SamplingMetricBase.cpp
Scene.cpp
SceneElement.cpp
+ ThreadPool.cpp
Timer.cpp
TransferPropertiesBehavior.cpp
)
@@ -53,6 +55,7 @@ set(SURGSIM_FRAMEWORK_HEADERS
BehaviorManager.h
Clock.h
Component.h
+ Component-inl.h
ComponentManager.h
ComponentManager-inl.h
FrameworkConvert.h
@@ -72,20 +75,23 @@ set(SURGSIM_FRAMEWORK_HEADERS
Representation.h
ReuseFactory.h
Runtime.h
+ SamplingMetricBase.h
Scene.h
SceneElement.h
SceneElement-inl.h
SharedInstance.h
SharedInstance-inl.h
+ ThreadPool.h
+ ThreadPool-inl.h
Timer.h
TransferPropertiesBehavior.h
)
+surgsim_create_library_header(Framework.h "${SURGSIM_FRAMEWORK_HEADERS}")
surgsim_add_library(
SurgSimFramework
"${SURGSIM_FRAMEWORK_SOURCES}"
"${SURGSIM_FRAMEWORK_HEADERS}"
- "SurgSim/Framework"
)
SET(LIBS
@@ -94,7 +100,6 @@ SET(LIBS
)
target_link_libraries(SurgSimFramework ${LIBS})
-add_dependencies(SurgSimFramework yaml-cpp)
if(BUILD_TESTING)
add_subdirectory(UnitTests)
diff --git a/SurgSim/Framework/Clock.h b/SurgSim/Framework/Clock.h
index 7395a85..3fac860 100644
--- a/SurgSim/Framework/Clock.h
+++ b/SurgSim/Framework/Clock.h
@@ -17,6 +17,7 @@
#define SURGSIM_FRAMEWORK_CLOCK_H
#include <boost/chrono.hpp>
+#include <boost/thread.hpp>
/// \file
/// Place for a simple wrapper around boost
@@ -27,10 +28,32 @@ namespace SurgSim
namespace Framework
{
- /// Wraps around the actual clock we are using.
- typedef boost::chrono::system_clock Clock;
+/// Wraps around the actual clock we are using.
+typedef boost::chrono::high_resolution_clock Clock;
+
+/// A more accurate sleep_until that accounts for scheduler errors
+/// \tparam C Clock type
+/// \tparam D Duration type
+/// \param time The time point in absolute time to sleep until
+template <class C, class D>
+void sleep_until(const boost::chrono::time_point<C, D>& time)
+{
+ // 2ms gives good results on windows and linux
+ static const boost::chrono::duration<double> schedulerError(0.002);
+
+ boost::chrono::time_point<C, D> earlierTime = time - schedulerError;
+ if (earlierTime > C::now())
+ {
+ boost::this_thread::sleep_until(earlierTime);
+ }
+
+ while (C::now() < time)
+ {
+ boost::this_thread::yield();
+ }
+}
}; // Framework
}; // SurgSim
-#endif
\ No newline at end of file
+#endif
diff --git a/SurgSim/Framework/Component-inl.h b/SurgSim/Framework/Component-inl.h
new file mode 100644
index 0000000..920efbb
--- /dev/null
+++ b/SurgSim/Framework/Component-inl.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_FRAMEWORK_COMPONENT_INL_H
+#define SURGSIM_FRAMEWORK_COMPONENT_INL_H
+
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+template <class Target, class Source>
+std::shared_ptr<Target> checkAndConvert(std::shared_ptr<Source> incoming, const std::string& expectedTypeName)
+{
+ SURGSIM_ASSERT(incoming != nullptr) << "Incoming pointer can't be nullptr";
+ auto result = std::dynamic_pointer_cast<Target>(incoming);
+ SURGSIM_ASSERT(result != nullptr)
+ << "Expected " << expectedTypeName << " but received " << incoming->getClassName() << " which cannot "
+ << "be converted, in component " << incoming->getFullName() << ".";
+ return result;
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Framework/Component.cpp b/SurgSim/Framework/Component.cpp
index 4709579..43d0d50 100644
--- a/SurgSim/Framework/Component.cpp
+++ b/SurgSim/Framework/Component.cpp
@@ -53,6 +53,17 @@ std::string Component::getName() const
return m_name;
}
+std::string Component::getFullName() const
+{
+ std::string name;
+ const auto& sceneElement = getSceneElement();
+ if (sceneElement != nullptr)
+ {
+ name = sceneElement->getName() + "/";
+ }
+ return name + m_name;
+}
+
void Component::setName(const std::string& name)
{
m_name = name;
@@ -91,6 +102,17 @@ bool Component::wakeUp()
return m_isAwake;
}
+void Component::retire()
+{
+ doRetire();
+ m_isAwake = false;
+}
+
+void Component::doRetire()
+{
+ return;
+}
+
void Component::setScene(std::weak_ptr<Scene> scene)
{
m_scene = scene;
@@ -138,12 +160,6 @@ 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" <<
@@ -187,6 +203,5 @@ bool Component::isLocalActive() const
return m_isLocalActive;
}
-
}; // namespace Framework
}; // namespace SurgSim
diff --git a/SurgSim/Framework/Component.h b/SurgSim/Framework/Component.h
index 0c56760..5cb545a 100644
--- a/SurgSim/Framework/Component.h
+++ b/SurgSim/Framework/Component.h
@@ -16,9 +16,8 @@
#ifndef SURGSIM_FRAMEWORK_COMPONENT_H
#define SURGSIM_FRAMEWORK_COMPONENT_H
-
-#include <string>
#include <memory>
+#include <string>
#include <boost/uuid/uuid.hpp>
@@ -40,7 +39,10 @@ class SceneElement;
/// 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>
+class Component :
+ public Accessible,
+ public std::enable_shared_from_this<Component>,
+ public FactoryBase1<Component, std::string>
{
public:
/// Constructor
@@ -53,6 +55,10 @@ public:
/// \return Name of this component.
std::string getName() const;
+ /// Gets a string containing the name of the Component and (if it has one) its SceneElement.
+ /// \return Name of Component and SceneElement.
+ std::string getFullName() const;
+
/// Sets the name of component.
/// \param name The name of this component.
void setName(const std::string& name);
@@ -79,6 +85,10 @@ public:
/// \return True if this component is woken up successfully; otherwise, false.
bool wakeUp();
+ /// Retire this component, this will be called when the component is removed from the ComponentManager that is
+ /// responsible for handling this component. This gives the component a chance to get rid of all shared objects
+ void retire();
+
/// Sets the scene.
/// \param scene The scene for this component
void setScene(std::weak_ptr<Scene> scene);
@@ -110,11 +120,6 @@ public:
/// \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();
@@ -127,6 +132,10 @@ public:
/// \return True if component is woken up successfully; otherwise, false.
virtual bool doWakeUp() = 0;
+ /// Interface to be implemented by derived classes
+ /// Has a default implementation, does nothing
+ virtual void doRetire();
+
/// \return True if this component is active and its SceneElement (if any) is also active;
/// Otherwise, false.
bool isActive() const;
@@ -148,7 +157,10 @@ protected:
/// \return The PoseComponent
virtual std::shared_ptr<const PoseComponent> getPoseComponent() const;
+
private:
+
+
/// Name of this component
std::string m_name;
@@ -181,7 +193,22 @@ private:
};
+/// The function tries to convert the Source type to the Target type it will throw if Target is not a subclass
+/// of Source.
+/// \tparam Target type that is used as the target type for the conversion, can usually be deduced
+/// \tparam Source type that is the type of the incoming parameter, target needs to be a subclass of Source for the
+/// function to succeed
+/// \param incoming pointer to an instance of Source that is supposed to be converted to a pointer to Target
+/// \param expectedTypeName a name to be used in the error message if the conversion fails, use the full
+/// namespace name of Source here.
+/// \throws if
+/// \return pointer of type Target if Target is a subclass of Source, throws otherwise.
+template <class Target, class Source>
+std::shared_ptr<Target> checkAndConvert(std::shared_ptr<Source> incoming, const std::string& expectedTypeName);
+
}; // namespace Framework
}; // namespace SurgSim
+#include "SurgSim/Framework/Component-inl.h"
+
#endif // SURGSIM_FRAMEWORK_COMPONENT_H
diff --git a/SurgSim/Framework/ComponentManager-inl.h b/SurgSim/Framework/ComponentManager-inl.h
index 6d68aab..677dbb4 100644
--- a/SurgSim/Framework/ComponentManager-inl.h
+++ b/SurgSim/Framework/ComponentManager-inl.h
@@ -28,7 +28,7 @@ namespace Framework
/// \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)
+ 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";
@@ -38,13 +38,12 @@ std::shared_ptr<T> ComponentManager::tryAddComponent(std::shared_ptr<SurgSim::Fr
auto found = std::find(container->cbegin(), container->cend(), typedComponent);
if (found == container->cend())
{
- SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Added component " << component->getName();
+ SURGSIM_LOG_INFO(m_logger) << "Added component " << component->getFullName();
container->push_back(typedComponent);
}
else
{
- SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " component " << component->getName() <<
- " already added to " << getName();
+ SURGSIM_LOG_INFO(m_logger) << "Component " << component->getFullName() << " already added to " << getName();
typedComponent = nullptr;
}
}
@@ -53,7 +52,7 @@ std::shared_ptr<T> ComponentManager::tryAddComponent(std::shared_ptr<SurgSim::Fr
template<class T>
bool ComponentManager::tryRemoveComponent(std::shared_ptr<SurgSim::Framework::Component> component,
- std::vector<std::shared_ptr<T>>* container)
+ std::vector<std::shared_ptr<T>>* container)
{
SURGSIM_ASSERT(container != nullptr) << "Trying to use a component container that is null";
bool result = false;
@@ -69,13 +68,25 @@ bool ComponentManager::tryRemoveComponent(std::shared_ptr<SurgSim::Framework::Co
}
else
{
- SURGSIM_LOG_INFO(m_logger) << SURGSIM_CURRENT_FUNCTION << " Unable to remove component " <<
- typedComponent->getName() << ". Not found.";
+ SURGSIM_LOG_INFO(m_logger)
+ << SURGSIM_CURRENT_FUNCTION << " Unable to remove component "
+ << typedComponent->getName() << ". Not found.";
}
}
return result;
};
+template<class T>
+void ComponentManager::retireComponents(const std::vector<std::shared_ptr<T>>& container)
+{
+ static_assert(std::is_base_of<Component, T>::value == true, "Class has to be of type component");
+ for (const auto& component : container)
+ {
+ component->retire();
+ }
+}
+
+
}; // namespace Framework
}; // namespace SurgSim
diff --git a/SurgSim/Framework/ComponentManager.cpp b/SurgSim/Framework/ComponentManager.cpp
index 41c6da4..165be73 100644
--- a/SurgSim/Framework/ComponentManager.cpp
+++ b/SurgSim/Framework/ComponentManager.cpp
@@ -26,12 +26,13 @@ namespace Framework
{
ComponentManager::ComponentManager(const std::string& name /*= "Unknown Component Manager"*/) :
- BasicThread(name), m_logger(Logger::getLogger(name))
+ BasicThread(name)
{
}
ComponentManager::~ComponentManager()
{
+ SURGSIM_LOG_INFO(m_logger) << getName() << " destruction";
}
void ComponentManager::setRuntime(std::shared_ptr<Runtime> val)
@@ -50,6 +51,7 @@ bool ComponentManager::enqueueRemoveComponent(const std::shared_ptr<Component>&
{
boost::lock_guard<boost::mutex> lock(m_componentMutex);
m_componentRemovals.push_back(component);
+ m_elementCache.push_back(component->getSceneElement());
return true;
}
@@ -64,9 +66,10 @@ void ComponentManager::processComponents()
// 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<SceneElement>> inflightElements;
std::vector<std::shared_ptr<Component>> actualAdditions;
- copyScheduledComponents(&inflightAdditions, &inflightRemovals);
+ copyScheduledComponents(&inflightAdditions, &inflightRemovals, &inflightElements);
actualAdditions.reserve(inflightAdditions.size());
if (!inflightAdditions.empty())
@@ -85,7 +88,7 @@ void ComponentManager::processBehaviors(const double dt)
{
auto it = std::begin(m_behaviors);
auto endIt = std::end(m_behaviors);
- for ( ; it != endIt; ++it)
+ for (; it != endIt; ++it)
{
if ((*it)->isActive())
{
@@ -101,7 +104,7 @@ bool ComponentManager::executeInitialization()
// Call BasicThread initialize to do the initialize and startup call
bool success = BasicThread::executeInitialization();
- if (! success )
+ if (! success)
{
return success;
}
@@ -109,9 +112,10 @@ bool ComponentManager::executeInitialization()
// 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<SceneElement>> inflightElements;
std::vector<std::shared_ptr<Component>> actualAdditions;
- copyScheduledComponents(&inflightAdditions, &inflightRemovals);
+ copyScheduledComponents(&inflightAdditions, &inflightRemovals, &inflightElements);
actualAdditions.reserve(inflightAdditions.size());
if (! inflightAdditions.empty())
@@ -142,26 +146,31 @@ bool ComponentManager::executeInitialization()
void ComponentManager::copyScheduledComponents(
std::vector<std::shared_ptr<Component>>* inflightAdditions,
- std::vector<std::shared_ptr<Component>>* inflightRemovals
-)
+ std::vector<std::shared_ptr<Component>>* inflightRemovals,
+ std::vector<std::shared_ptr<SceneElement>>* inflightElements)
{
// 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);
+ std::swap(m_componentAdditions, *inflightAdditions);
m_componentAdditions.clear();
- *inflightRemovals = std::move(m_componentRemovals);
+ std::swap(m_componentRemovals, *inflightRemovals);
m_componentRemovals.clear();
+
+ std::swap(m_elementCache, *inflightElements);
+ m_elementCache.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)
+ for (auto it = beginIt; it != endIt; ++it)
{
- tryRemoveComponent(*it, &m_behaviors);
- executeRemovals(*it);
+ if (tryRemoveComponent(*it, &m_behaviors) || executeRemovals(*it))
+ {
+ (*it)->retire();
+ }
}
}
@@ -171,7 +180,7 @@ void ComponentManager::addComponents(
std::vector<std::shared_ptr<Component>>* actualAdditions)
{
// Add All Components to the internal storage
- for(auto it = beginIt; it != endIt; ++it)
+ for (auto it = beginIt; it != endIt; ++it)
{
std::shared_ptr<Behavior> behavior = std::dynamic_pointer_cast<Behavior>(*it);
if (behavior != nullptr && behavior->getTargetManagerType() == getType())
@@ -189,22 +198,30 @@ void ComponentManager::addComponents(
}
void ComponentManager::wakeUpComponents(const std::vector<std::shared_ptr<Component>>::const_iterator& beginIt,
- const std::vector<std::shared_ptr<Component>>::const_iterator& endIt)
+ const std::vector<std::shared_ptr<Component>>::const_iterator& endIt)
{
- for(auto it = beginIt; it != endIt; ++it)
+ for (auto it = beginIt; it != endIt; ++it)
{
- if ( (*it)->isInitialized() && !(*it)->isAwake())
+ if ((*it)->isInitialized() && !(*it)->isAwake())
{
- if ( !(*it)->wakeUp())
+ 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!";
+ getName() << ". Component was not added to the manager!";
executeRemovals(*it);
}
}
}
}
+void ComponentManager::doBeforeStop()
+{
+ for (const auto& behavior : m_behaviors)
+ {
+ behavior->retire();
+ }
+}
+
/// Returns this manager's logger
std::shared_ptr<SurgSim::Framework::Logger> ComponentManager::getLogger() const
{
diff --git a/SurgSim/Framework/ComponentManager.h b/SurgSim/Framework/ComponentManager.h
index fc9f59d..f058963 100644
--- a/SurgSim/Framework/ComponentManager.h
+++ b/SurgSim/Framework/ComponentManager.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -95,6 +95,7 @@ protected:
/// 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);
@@ -107,7 +108,8 @@ protected:
/// 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);
+ std::vector<std::shared_ptr<Component>>* inflightRemovals,
+ std::vector<std::shared_ptr<SceneElement>>* inflightElements);
/// Returns this manager's logger
std::shared_ptr<SurgSim::Framework::Logger> getLogger() const;
@@ -120,16 +122,19 @@ protected:
/// Contain components scheduled to be added/removed
std::vector<std::shared_ptr<Component>> m_componentAdditions;
std::vector<std::shared_ptr<Component>> m_componentRemovals;
+ std::vector<std::shared_ptr<SceneElement>> m_elementCache;
///@}
- /// 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;
+ void doBeforeStop() override;
+
+ template<class T>
+ void retireComponents(const std::vector<std::shared_ptr<T>>& container);
+
private:
/// Adds a component.
/// \param component The component to be added.
@@ -145,7 +150,7 @@ private:
/// 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;
+ bool executeInitialization() override;
/// Delegates to doRemoveComponent to remove all the components in the indicated array.
/// \param beginIt The begin iterator.
diff --git a/SurgSim/Framework/FrameworkConvert-inl.h b/SurgSim/Framework/FrameworkConvert-inl.h
index b86ce8a..fec5183 100644
--- a/SurgSim/Framework/FrameworkConvert-inl.h
+++ b/SurgSim/Framework/FrameworkConvert-inl.h
@@ -32,7 +32,7 @@ YAML::Node YAML::convert<std::shared_ptr<T>>::encode(
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<T> >::type& rhs) //NOLINT
{
std::shared_ptr<SurgSim::Framework::Component> temporary;
bool success = YAML::convert<std::shared_ptr<SurgSim::Framework::Component>>::decode(node, temporary);
diff --git a/SurgSim/Framework/FrameworkConvert.cpp b/SurgSim/Framework/FrameworkConvert.cpp
index bd976dc..ed7f79b 100644
--- a/SurgSim/Framework/FrameworkConvert.cpp
+++ b/SurgSim/Framework/FrameworkConvert.cpp
@@ -18,9 +18,13 @@
#include "SurgSim/Framework/BasicSceneElement.h"
#include "SurgSim/Framework/Component.h"
#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/Asset.h"
#include <boost/uuid/uuid_io.hpp>
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/ApplicationData.h"
+
namespace
{
const std::string NamePropertyName = "Name";
@@ -45,7 +49,7 @@ Node convert<std::shared_ptr<SurgSim::Framework::Component>>::encode(
bool convert<std::shared_ptr<SurgSim::Framework::Component>>::decode(
const Node& node,
- std::shared_ptr<SurgSim::Framework::Component>& rhs)
+ std::shared_ptr<SurgSim::Framework::Component>& rhs) //NOLINT
{
bool result = false;
@@ -54,44 +58,50 @@ bool convert<std::shared_ptr<SurgSim::Framework::Component>>::decode(
return false;
}
- Node data = node.begin()->second;
+ SurgSim::Framework::Component::FactoryType& factory = SurgSim::Framework::Component::getFactory();
std::string className = node.begin()->first.as<std::string>();
+ SURGSIM_ASSERT(factory.isRegistered(className)) << "Class " << className << " is not registered in the factory.";
- if (data.IsMap() &&
- data[IdPropertyName].IsDefined() &&
- data[NamePropertyName].IsDefined())
+ Node data = node.begin()->second;
+ if (data.IsMap() && data[NamePropertyName].IsDefined())
{
+ std::string name = data[NamePropertyName].as<std::string>();
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
+ if (data[IdPropertyName].IsDefined())
{
- SurgSim::Framework::Component::FactoryType& factory =
- SurgSim::Framework::Component::getFactory();
-
- if (factory.isRegistered(className))
+ std::string id = data[IdPropertyName].as<std::string>();
+ RegistryType& registry = getRegistry();
+ auto sharedComponent = registry.find(id);
+ if (sharedComponent != registry.end())
{
- rhs = factory.create(className, data[NamePropertyName].as<std::string>());
- getRegistry()[id] = rhs;
+ SURGSIM_ASSERT(name == sharedComponent->second->getName() &&
+ className == sharedComponent->second->getClassName())
+ << "The current node:" << std::endl << node << std::endl << "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." << std::endl
+ << "The original component is a:" << std::endl
+ << sharedComponent->second->getClassName() << std::endl
+ << sharedComponent->second->getName();
+ rhs = sharedComponent->second;
}
else
{
- SURGSIM_FAILURE() << "Class " << className << " is not registered in the factory.";
+ rhs = factory.create(className, name);
+ getRegistry()[id] = rhs;
}
}
+ else
+ {
+ rhs = factory.create(className, name);
+ }
}
- rhs->decode(data);
+
+ std::vector<std::string> ignoredProperties;
+ ignoredProperties.push_back(NamePropertyName);
+ ignoredProperties.push_back(IdPropertyName);
+
+ rhs->decode(data, ignoredProperties);
result = true;
}
return result;
@@ -126,7 +136,7 @@ Node convert<std::shared_ptr<SurgSim::Framework::SceneElement>>::encode(
bool convert<std::shared_ptr<SurgSim::Framework::SceneElement>>::decode(
const Node& node,
- std::shared_ptr<SurgSim::Framework::SceneElement>& rhs)
+ std::shared_ptr<SurgSim::Framework::SceneElement>& rhs) //NOLINT
{
if (rhs == nullptr)
{
@@ -136,6 +146,60 @@ bool convert<std::shared_ptr<SurgSim::Framework::SceneElement>>::decode(
return rhs->decode(node);
}
+bool YAML::convert<std::vector<std::shared_ptr<SurgSim::Framework::SceneElement>>>::decode(
+ const Node& node,
+ std::vector<std::shared_ptr<SurgSim::Framework::SceneElement>>& rhs, // NOLINT
+ std::vector<std::string>* stack)
+{
+ std::unique_ptr<std::vector<std::string>> localStack;
+ if (node.IsSequence())
+ {
+ if (stack == nullptr)
+ {
+ localStack.reset(new std::vector<std::string>());
+ stack = localStack.get();
+ }
+ for (auto element = node.begin(); element != node.end(); ++element)
+ {
+ if (element->IsMap() && element->begin()->first.as<std::string>() == "INCLUDE")
+ {
+
+ auto file = element->begin()->second.as<std::string>();
+ auto data = SurgSim::Framework::Runtime::getApplicationData();
+ SURGSIM_ASSERT(data->tryFindFile(file, &file))
+ << "Could not find include file " << file;
+
+ SURGSIM_ASSERT(std::find(stack->cbegin(), stack->cend(), file) == stack->cend())
+ << "Found inclusion loop File: " << file << " included from " << stack->back()
+ << " is already included.";
+
+ auto included = YAML::LoadFile(file);
+ stack->push_back(file);
+ YAML::convert<std::vector<std::shared_ptr<SurgSim::Framework::SceneElement>>>::decode(
+ included, rhs, stack);
+ stack->pop_back();
+ }
+ else
+ {
+ rhs.push_back(element->as<std::shared_ptr<SurgSim::Framework::SceneElement>>());
+ }
+ }
+ }
+ else if (node.IsMap())
+ {
+ rhs.push_back(node.as<std::shared_ptr<SurgSim::Framework::SceneElement>>());
+ }
+ else
+ {
+ SURGSIM_FAILURE()
+ << "Trying to decode std::vector<std::shared_ptr<SceneElement>> but the received node is neither "
+ << "a sequence nor a map";
+ return false;
+ }
+ return true;
+}
+
+
Node convert<SurgSim::Framework::SceneElement>::encode(
const SurgSim::Framework::SceneElement& rhs)
{
@@ -151,7 +215,7 @@ Node convert<std::shared_ptr<SurgSim::Framework::Scene>>::encode(
bool convert<std::shared_ptr<SurgSim::Framework::Scene>>::decode(
const Node& node,
- std::shared_ptr<SurgSim::Framework::Scene>& rhs)
+ std::shared_ptr<SurgSim::Framework::Scene>& rhs) //NOLINT
{
bool result = false;
if (rhs != nullptr)
@@ -161,4 +225,46 @@ bool convert<std::shared_ptr<SurgSim::Framework::Scene>>::decode(
return result;
}
-}
\ No newline at end of file
+YAML::Node YAML::convert<std::shared_ptr<SurgSim::Framework::Asset>>::encode(
+ const std::shared_ptr<SurgSim::Framework::Asset> rhs)
+{
+ YAML::Node node;
+ node[rhs->getClassName()] = rhs->encode();
+ return node;
+}
+
+bool YAML::convert<std::shared_ptr<SurgSim::Framework::Asset>>::decode(
+ const Node& node, std::shared_ptr<SurgSim::Framework::Asset>& rhs) //NOLINT
+{
+ bool result = false;
+
+ if (node.IsMap())
+ {
+ if (nullptr == rhs)
+ {
+ std::string className = node.begin()->first.as<std::string>();
+ auto& factory = SurgSim::Framework::Asset::getFactory();
+
+ if (factory.isRegistered(className))
+ {
+ rhs = factory.create(className);
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Class " << className << " is not registered in the Asset factory.";
+ }
+ }
+
+ Node data = node.begin()->second;
+ if (data.IsMap())
+ {
+ rhs->decode(data);
+ }
+
+ result = true;
+ }
+
+ return result;
+}
+
+}
diff --git a/SurgSim/Framework/FrameworkConvert.h b/SurgSim/Framework/FrameworkConvert.h
index d56ddb5..d0f7540 100644
--- a/SurgSim/Framework/FrameworkConvert.h
+++ b/SurgSim/Framework/FrameworkConvert.h
@@ -25,6 +25,7 @@ namespace SurgSim
{
namespace Framework
{
+class Asset;
class Component;
class SceneElement;
class Scene;
@@ -34,9 +35,9 @@ class Scene;
namespace YAML
{
-/// Specializatio of YAML::convert for std::shared_ptr, this is used to redirect the serialization of a derived class
+/// Specialization 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
+/// Component serialization specialization, currently each redirection has to be implemented separately, the re is
/// probably a way to do this automatically.
/// \tparam T class that should be converted from a shared ptr
template <class T>
@@ -44,13 +45,14 @@ 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);
+ 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);
+ std::shared_ptr<T>>::type& rhs); //NOLINT
};
+
/// 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.
@@ -62,12 +64,10 @@ struct convert<std::shared_ptr<T>>
/// 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> >
+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:
+ static bool decode(const Node& node, std::shared_ptr<SurgSim::Framework::Component>& rhs); //NOLINT
typedef std::unordered_map<std::string, std::shared_ptr<SurgSim::Framework::Component>> RegistryType;
@@ -75,9 +75,6 @@ private:
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.
@@ -91,10 +88,20 @@ 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);
+ static bool decode(const Node& node, std::shared_ptr<SurgSim::Framework::SceneElement>& rhs); //NOLINT
};
template<>
+struct convert <std::vector<std::shared_ptr<SurgSim::Framework::SceneElement>>>
+{
+
+ static bool decode(const Node& node,
+ std::vector<std::shared_ptr<SurgSim::Framework::SceneElement>>& rhs, //NOLINT
+ std::vector<std::string>* stack = nullptr);
+};
+
+
+template<>
struct convert<SurgSim::Framework::SceneElement>
{
static Node encode(const SurgSim::Framework::SceneElement& rhs);
@@ -104,14 +111,19 @@ 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);
+ static bool decode(const Node& node, std::shared_ptr<SurgSim::Framework::Scene>& rhs); //NOLINT
};
-
+template<>
+struct convert<std::shared_ptr<SurgSim::Framework::Asset>>
+{
+ static Node encode(const std::shared_ptr<SurgSim::Framework::Asset> rhs);
+ static bool decode(const Node& node, std::shared_ptr<SurgSim::Framework::Asset>& rhs); //NOLINT
+};
};
#include "SurgSim/Framework/FrameworkConvert-inl.h"
-#endif // SURGSIM_FRAMEWORK_FRAMEWORKCONVERT_H
\ No newline at end of file
+#endif // SURGSIM_FRAMEWORK_FRAMEWORKCONVERT_H
diff --git a/SurgSim/Framework/LogMessageBase.cpp b/SurgSim/Framework/LogMessageBase.cpp
index 8782288..6849206 100644
--- a/SurgSim/Framework/LogMessageBase.cpp
+++ b/SurgSim/Framework/LogMessageBase.cpp
@@ -26,7 +26,7 @@ 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"};
+ static std::string levelNames[5] = {"DEBUG ", "INFO ", "WARNING ", "SEVERE ", "CRITICAL"};
std::time_t timeStamp;
std::time(&timeStamp);
::tm tm;
@@ -42,14 +42,14 @@ LogMessageBase::LogMessageBase(Logger* logger, int 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 << " ";
+ 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) <<
+ levelName << " " <<
+ m_logger->getName() << " ";
}
}; // namespace Framework
diff --git a/SurgSim/Framework/LogOutput.cpp b/SurgSim/Framework/LogOutput.cpp
index 167f6de..ef7b1fc 100644
--- a/SurgSim/Framework/LogOutput.cpp
+++ b/SurgSim/Framework/LogOutput.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -14,13 +14,14 @@
// 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>
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Logger.h"
+
+
namespace SurgSim
{
namespace Framework
@@ -37,11 +38,6 @@ FileOutput::FileOutput(const std::string& filename) :
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()) <<
@@ -58,15 +54,6 @@ 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;
diff --git a/SurgSim/Framework/LogOutput.h b/SurgSim/Framework/LogOutput.h
index 0d31fba..3d9185a 100644
--- a/SurgSim/Framework/LogOutput.h
+++ b/SurgSim/Framework/LogOutput.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,8 +16,8 @@
#ifndef SURGSIM_FRAMEWORK_LOGOUTPUT_H
#define SURGSIM_FRAMEWORK_LOGOUTPUT_H
-#include <fstream>
#include <boost/thread/mutex.hpp>
+#include <fstream>
namespace SurgSim
{
@@ -58,11 +58,9 @@ public:
/// \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;
+ bool writeMessage(const std::string& message) override;
private:
std::string m_filename;
@@ -80,12 +78,11 @@ public:
/// \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;
+ bool writeMessage(const std::string& message) override;
private:
std::ostream& m_stream;
diff --git a/SurgSim/Framework/LoggerManager.cpp b/SurgSim/Framework/LoggerManager.cpp
index ecb2a2b..7c1cdf2 100644
--- a/SurgSim/Framework/LoggerManager.cpp
+++ b/SurgSim/Framework/LoggerManager.cpp
@@ -63,6 +63,7 @@ void LoggerManager::setThreshold(int threshold)
void LoggerManager::setThreshold(const std::string& path, int threshold)
{
boost::lock_guard<boost::mutex> lock(m_mutex);
+ m_thresholds.push_back(std::make_pair(path, threshold));
for (auto it = m_loggers.cbegin(); it != m_loggers.cend(); ++it)
{
if (boost::istarts_with(it->first, path))
@@ -96,6 +97,14 @@ std::shared_ptr<Logger> LoggerManager::getLogger(const std::string& name)
{
result = std::make_shared<Logger>(Logger(name, m_defaultOutput));
result->setThreshold(m_globalThreshold);
+
+ for (const auto& subgroupThreshold : m_thresholds)
+ {
+ if (boost::istarts_with(name, subgroupThreshold.first))
+ {
+ result->setThreshold(subgroupThreshold.second);
+ }
+ }
m_loggers[name] = result;
}
return result;
diff --git a/SurgSim/Framework/LoggerManager.h b/SurgSim/Framework/LoggerManager.h
index 188b46c..0bcc0d9 100644
--- a/SurgSim/Framework/LoggerManager.h
+++ b/SurgSim/Framework/LoggerManager.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,8 +16,9 @@
#ifndef SURGSIM_FRAMEWORK_LOGGERMANAGER_H
#define SURGSIM_FRAMEWORK_LOGGERMANAGER_H
-#include <unordered_map>
#include <boost/thread/mutex.hpp>
+#include <unordered_map>
+#include <vector>
namespace SurgSim
{
@@ -73,6 +74,9 @@ private:
/// Keep track of all the loggers
std::unordered_map<std::string, std::shared_ptr<Logger>> m_loggers;
+ /// Keep track of subgroup thresholds.
+ std::vector<std::pair<std::string, int>> m_thresholds;
+
/// Use for default output of the logger
std::shared_ptr<LogOutput> m_defaultOutput;
@@ -90,4 +94,4 @@ private:
}; // Framework
}; // SurgSim
-#endif //SURGSIM_FRAMEWORK_LOGGERMANAGER_H
\ No newline at end of file
+#endif //SURGSIM_FRAMEWORK_LOGGERMANAGER_H
diff --git a/SurgSim/Framework/Macros.h b/SurgSim/Framework/Macros.h
index 3edf7e7..f621ec4 100644
--- a/SurgSim/Framework/Macros.h
+++ b/SurgSim/Framework/Macros.h
@@ -21,11 +21,11 @@
#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
+/// Macro to tell GCC this is a used variable, and not to optimize it out
#ifdef __GNUC__
-#define SURGSIM_DO_PRAGMA(x) _Pragma (#x)
+#define SURGSIM_USED_VARIABLE(x) x __attribute__((used))
#else
-#define SURGSIM_DO_PRAGMA(x)
+#define SURGSIM_USED_VARIABLE(x) x
#endif
///@{
diff --git a/SurgSim/Framework/ObjectFactory.h b/SurgSim/Framework/ObjectFactory.h
index 3343159..c2739a7 100644
--- a/SurgSim/Framework/ObjectFactory.h
+++ b/SurgSim/Framework/ObjectFactory.h
@@ -105,7 +105,7 @@ public:
private:
- typedef boost::function<std::shared_ptr<Base>(Parameter1)> Constructor;
+ typedef boost::function<std::shared_ptr<Base>(const Parameter1&)> Constructor;
/// All the constructors.
std::map<std::string, Constructor> m_constructors;
@@ -114,6 +114,42 @@ private:
mutable boost::mutex m_mutex;
};
+
+/// CRTP Base class to implement Object Factory functionality on a base class, use this rather than writing
+/// your own functions to return the factory
+/// \tparam T base class of the generated objects
+template <class T>
+class FactoryBase
+{
+public:
+ typedef ObjectFactory<T> FactoryType;
+
+ /// \return a reference to the factory
+ static FactoryType& getFactory()
+ {
+ static FactoryType factory;
+ return factory;
+ }
+};
+
+/// CRTP Base class to implement Object Factory functionality on a base class, use this rather than writing
+/// your own functions to return the factory
+/// \tparam T base class of the generated objects
+/// \tparam P constructor parameter for object generation
+template <class T, class P>
+class FactoryBase1
+{
+public:
+ typedef ObjectFactory1<T, P> FactoryType;
+
+ /// \return a reference to the factory
+ static FactoryType& getFactory()
+ {
+ static FactoryType factory;
+ return factory;
+ }
+};
+
};
};
@@ -126,8 +162,8 @@ private:
/// '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); \
+ SURGSIM_USED_VARIABLE(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.
@@ -143,10 +179,8 @@ private:
/// 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)
+ SURGSIM_USED_VARIABLE(static bool SURGSIM_CONCATENATE(ClassName, IsRegistered)) = \
+ SURGSIM_CONCATENATE(ClassName, Registered);
-#endif // SURGSIM_FRAMEWORK_OBJECTFACTORY_H
\ No newline at end of file
+#endif // SURGSIM_FRAMEWORK_OBJECTFACTORY_H
diff --git a/SurgSim/Framework/PoseComponent.h b/SurgSim/Framework/PoseComponent.h
index eee5076..4a5d5bc 100644
--- a/SurgSim/Framework/PoseComponent.h
+++ b/SurgSim/Framework/PoseComponent.h
@@ -53,17 +53,17 @@ protected:
/// A PoseComponent cannot have a PoseComponent, so this will
/// return nullptr.
/// \return The PoseComponent
- virtual std::shared_ptr<PoseComponent> getPoseComponent() override;
+ 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;
+ std::shared_ptr<const PoseComponent> getPoseComponent() const override;
private:
- virtual bool doInitialize() override;
- virtual bool doWakeUp() override;
+ bool doInitialize() override;
+ bool doWakeUp() override;
SurgSim::Math::RigidTransform3d m_pose;
};
diff --git a/SurgSim/Framework/Representation.cpp b/SurgSim/Framework/Representation.cpp
index 3b31ebd..3420715 100644
--- a/SurgSim/Framework/Representation.cpp
+++ b/SurgSim/Framework/Representation.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -13,22 +13,23 @@
// 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/PoseComponent.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())
+ Component(m_name), m_localPose(Math::RigidTransform3d::Identity())
{
- SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, SurgSim::Math::RigidTransform3d, LocalPose, getLocalPose,
- setLocalPose);
- SURGSIM_ADD_RO_PROPERTY(Representation, SurgSim::Math::RigidTransform3d, Pose, getPose);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, Math::RigidTransform3d, LocalPose, getLocalPose, setLocalPose);
+ SURGSIM_ADD_RO_PROPERTY(Representation, Math::RigidTransform3d, Pose, getPose);
}
Representation::~Representation()
@@ -52,9 +53,10 @@ void Representation::setLocalPose(const SurgSim::Math::RigidTransform3d& pose)
SurgSim::Math::RigidTransform3d Representation::getPose() const
{
- if (getSceneElement() != nullptr)
+ auto element = getSceneElement();
+ if (element != nullptr)
{
- return getSceneElement()->getPose() * getLocalPose();
+ return element->getPose() * getLocalPose();
}
else
{
diff --git a/SurgSim/Framework/Representation.h b/SurgSim/Framework/Representation.h
index 485a193..02672dc 100644
--- a/SurgSim/Framework/Representation.h
+++ b/SurgSim/Framework/Representation.h
@@ -55,8 +55,8 @@ private:
/// Local Pose of the Representation with respect to the SceneElement
SurgSim::Math::RigidTransform3d m_localPose;
- virtual bool doInitialize() override;
- virtual bool doWakeUp() override;
+ bool doInitialize() override;
+ bool doWakeUp() override;
};
}; // namespace Framework
diff --git a/SurgSim/Framework/Runtime.cpp b/SurgSim/Framework/Runtime.cpp
index 5fc90c9..43e71e6 100644
--- a/SurgSim/Framework/Runtime.cpp
+++ b/SurgSim/Framework/Runtime.cpp
@@ -13,7 +13,6 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-
#include <boost/thread/thread.hpp>
#include <boost/thread/locks.hpp>
@@ -23,8 +22,11 @@
#include "SurgSim/Framework/Barrier.h"
#include "SurgSim/Framework/ComponentManager.h"
#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/ThreadPool.h"
+#include "SurgSim/Framework/Timer.h"
namespace SurgSim
{
@@ -35,14 +37,16 @@ std::shared_ptr<ApplicationData> Runtime::m_applicationData;
Runtime::Runtime() :
m_isRunning(false),
- m_isPaused(false)
+ m_isPaused(false),
+ m_isStopped(false)
{
initSearchPaths("");
}
Runtime::Runtime(const std::string& configFilePath) :
m_isRunning(false),
- m_isPaused(false)
+ m_isPaused(false),
+ m_isStopped(false)
{
initSearchPaths(configFilePath);
}
@@ -64,6 +68,17 @@ void Runtime::addManager(std::shared_ptr<ComponentManager> manager)
}
}
+
+std::vector<std::weak_ptr<ComponentManager>> Runtime::getManagers() const
+{
+ std::vector<std::weak_ptr<ComponentManager>> result(m_managers.size());
+
+ std::copy(m_managers.begin(), m_managers.end(), result.begin());
+
+ return result;
+}
+
+
std::shared_ptr<Scene> Runtime::getScene()
{
if (m_scene == nullptr)
@@ -135,6 +150,8 @@ bool Runtime::start(bool paused)
{
auto logger = Logger::getDefaultLogger();
+ SURGSIM_ASSERT(m_isStopped == false) << "This runtime has already been stopped, it cannot be started again.";
+
// 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
@@ -173,6 +190,11 @@ bool Runtime::start(bool paused)
bool Runtime::stop()
{
+ if (m_isStopped == true)
+ {
+ return false;
+ }
+
if (isPaused())
{
resume();
@@ -180,11 +202,29 @@ bool Runtime::stop()
m_isRunning = false;
+ // Suspend updates on all threads
std::vector<std::shared_ptr<ComponentManager>>::iterator it;
for (it = m_managers.begin(); it != m_managers.end(); ++it)
{
+ (*it)->setIdle(true);
+ }
+
+ // Give all threads time to run through update
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+
+ // Now stop all threads
+ for (it = m_managers.begin(); it != m_managers.end(); ++it)
+ {
+ SurgSim::Framework::Timer timer;
+ timer.start();
(*it)->stop();
+ timer.endFrame();
+ SURGSIM_LOG_INFO(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Killing : " << (*it)->getName() << " took " << timer.getCumulativeTime() << "s";
}
+
+ m_isStopped = true;
+
return true;
}
@@ -285,6 +325,12 @@ std::shared_ptr<const ApplicationData> Runtime::getApplicationData()
return m_applicationData;
}
+std::shared_ptr<ThreadPool> Runtime::getThreadPool()
+{
+ static auto threadPool = std::make_shared<ThreadPool>();
+ return threadPool;
+}
+
void Runtime::addComponent(const std::shared_ptr<Component>& component)
{
if (m_isRunning)
@@ -298,6 +344,8 @@ void Runtime::addComponent(const std::shared_ptr<Component>& component)
void Runtime::removeComponent(const std::shared_ptr<Component>& component)
{
+ SURGSIM_ASSERT(component != nullptr)
+ << "Trying to remove a nullptr !";
if (m_isRunning)
{
for (auto it = std::begin(m_managers); it != std::end(m_managers); ++it)
@@ -307,5 +355,141 @@ void Runtime::removeComponent(const std::shared_ptr<Component>& component)
}
}
+void Runtime::loadScene(const std::string& fileName)
+{
+ YAML::Node node;
+ boost::lock_guard<boost::mutex> lock(m_sceneHandling);
+ if (tryLoadNode(fileName, &node))
+ {
+ YAML::convert<std::shared_ptr<SurgSim::Framework::Component>>::getRegistry().clear();
+ m_scene = std::make_shared<Scene>(getSharedPtr());
+ m_scene->decode(node);
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Loading of the Scene failed.";
+ }
+
+}
+
+void Runtime::addSceneElements(const std::string& fileName)
+{
+ SURGSIM_LOG_DEBUG(Logger::getLogger("Runtime")) << "Adding scene elements from " << fileName;
+ YAML::Node node;
+ boost::lock_guard<boost::mutex> lock(m_sceneHandling);
+
+ if (tryLoadNode(fileName, &node))
+ {
+ std::vector<std::shared_ptr<SceneElement>> elements;
+ if (tryConvertElements(fileName, node, &elements))
+ {
+ getScene()->addSceneElements(elements);
+ }
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Could not add scene elements from the YAML file: " << fileName;
+ }
+ SURGSIM_LOG_DEBUG(Logger::getLogger("Runtime")) << "Done adding scene elements from " << fileName;
+}
+
+std::vector<std::shared_ptr<SceneElement>> Runtime::duplicateSceneElements(const std::string& fileName)
+{
+ YAML::Node node;
+ YAML::convert<std::shared_ptr<SurgSim::Framework::Component>>::RegistryType registry;
+ std::vector<std::shared_ptr<SceneElement>> result;
+
+ boost::lock_guard<boost::mutex> lock(m_sceneHandling);
+
+ // Use a temporary registry
+ std::swap(YAML::convert<std::shared_ptr<SurgSim::Framework::Component>>::getRegistry(), registry);
+
+ bool success = false;
+ if (tryLoadNode(fileName, &node) && tryConvertElements(fileName, node, &result))
+ {
+ success = true;
+ }
+
+ // restore the original registry
+ std::swap(YAML::convert<std::shared_ptr<SurgSim::Framework::Component>>::getRegistry(), registry);
+
+ if (!success)
+ {
+ SURGSIM_FAILURE() << "Could not clone from the YAML file: " << fileName;
+ }
+
+ return result;
+}
+
+bool tryLoadNode(const std::string& fileName, YAML::Node* node)
+{
+ bool result = false;
+ std::string path;
+ SURGSIM_ASSERT(node != nullptr) << "Can't load node into nullptr.";
+ if (Runtime::getApplicationData()->tryFindFile(fileName, &path))
+ {
+ try
+ {
+ *node = YAML::LoadFile(path);
+ result = true;
+ }
+ catch (YAML::ParserException e)
+ {
+ SURGSIM_LOG_SEVERE(Logger::getLogger("Runtime")) << "Could not parse YAML File at " << path
+ << " due to " << e.msg << " at line " << e.mark.line << " column " << e.mark.column;
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(Logger::getLogger("Runtime")) << "Could not find file " << fileName;
+ }
+ return result;
+}
+
+bool Runtime::tryConvertElements(const std::string& filename, const YAML::Node& node,
+ std::vector<std::shared_ptr<SceneElement>>* elements)
+{
+ bool result = false;
+ if (node.IsSequence())
+ {
+ try
+ {
+ *elements = node.as<std::vector<std::shared_ptr<SceneElement>>>();
+ result = true;
+ }
+ catch (YAML::Exception e)
+ {
+ SURGSIM_LOG_SEVERE(Logger::getLogger("Runtime"))
+ << "File " << filename << " YAML conversion failed with exception: " + e.msg;
+ }
+ catch (SurgSim::Framework::AssertionFailure e)
+ {
+ SURGSIM_LOG_CRITICAL(Logger::getLogger("Runtime"))
+ << "File " << filename << " conversion failed.";
+ throw e;
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(Logger::getLogger("Runtime"))
+ << "File " << filename << " not a YAML sequence, can't load scene elements.";
+ }
+ return result;
+}
+
+void Runtime::saveScene(const std::string& fileName) const
+{
+ std::ofstream out(fileName);
+ if (out.good())
+ {
+ out << m_scene->encode();
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(Logger::getLogger("Runtime"))
+ << "Failed to open " << fileName << ". Cannot save the scene.";
+ }
+}
+
}; // namespace Framework
}; // namespace SurgSim
diff --git a/SurgSim/Framework/Runtime.h b/SurgSim/Framework/Runtime.h
index 15fe277..7867767 100644
--- a/SurgSim/Framework/Runtime.h
+++ b/SurgSim/Framework/Runtime.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,12 +16,17 @@
#ifndef SURGSIM_FRAMEWORK_RUNTIME_H
#define SURGSIM_FRAMEWORK_RUNTIME_H
-#include <vector>
+#include <boost/thread/mutex.hpp>
+#include <map>
#include <memory>
#include <string>
-#include <map>
+#include <vector>
-#include <boost/thread/mutex.hpp>
+
+namespace YAML
+{
+class Node;
+}
namespace SurgSim
{
@@ -35,6 +40,7 @@ class Component;
class Logger;
class Scene;
class SceneElement;
+class ThreadPool;
/// 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
@@ -59,6 +65,13 @@ public:
/// Add a worker thread, this should probably only be possible if the system is not running
void addManager(std::shared_ptr<ComponentManager> thread);
+ /// \return All the managers from the runtime
+ std::vector<std::weak_ptr<ComponentManager>> getManagers() const;
+
+ /// \return The first manager of type T that is found nullptr otherwise
+ template <class T>
+ std::shared_ptr<T> getManager() const;
+
/// \return The scene to be used for this runtime. Use this for any kind of scene manipulation.
std::shared_ptr<Scene> getScene();
@@ -111,6 +124,10 @@ public:
/// \return The application data.
static std::shared_ptr<const ApplicationData> getApplicationData();
+ /// Gets the thread pool for the runtime.
+ /// \return The thread pool.
+ static std::shared_ptr<ThreadPool> getThreadPool();
+
/// Adds a component.
/// \param component The component.
void addComponent(const std::shared_ptr<Component>& component);
@@ -119,6 +136,46 @@ public:
/// \param component The component.
void removeComponent(const std::shared_ptr<Component>& component);
+ /// Loads the scene from the given file, clears all the elements in the scene, the old scene will be
+ /// overwritten.
+ /// \param fileName the filename of the scene to be loaded, needs to be found
+ /// \throws If the file cannot be found or is an invalid YAML file
+ void loadScene(const std::string& fileName);
+
+ /// Adds the scene elements from the file to the current scene
+ /// The file format should be just a list of sceneElements
+ /// \code
+ /// - SurgSim::Framework::BasicSceneElement:
+ /// Name: element1
+ /// IsActive: true
+ /// Components:
+ /// - MockComponent:
+ /// Name: component1
+ /// Id: 792faa40-459b-40cf-981d-560a8f2bd1801
+ /// - SurgSim::Framework::BasicSceneElement:
+ /// Name: element2
+ /// IsActive: true
+ /// Components:
+ /// - MockComponent:
+ /// Name: component2
+ /// Id: 1de26315-82a7-46b2-ae38-324d25009629
+ /// \endcode
+ /// \param fileName the filename of the scene to be loaded, needs to be found
+ /// \throws If the file cannot be found or is an invalid file
+ void addSceneElements(const std::string& fileName);
+
+ /// Loads and duplicates the scene elements from the file, the elements will not have common ids
+ /// with any other cloned elements, this lets you repeatedly load a set of elements to replicate this
+ /// set. The format is a list of scene elements \sa addSceneElements().
+ /// \param fileName the filename of the scene to be loaded, needs to be found
+ /// \throws if loading failed
+ /// \return a vector of scene elements with the loaded elements
+ std::vector<std::shared_ptr<SceneElement>> duplicateSceneElements(const std::string& fileName);
+
+ /// Write out the whole scene as a file
+ /// \param fileName the name of the scene-file if no path is given, uses the current path of the executable
+ void saveScene(const std::string& fileName) const;
+
private:
/// Preprocess scene elements. This is called during the startup sequence
@@ -135,21 +192,53 @@ private:
/// "." will be used as default path.
void initSearchPaths(const std::string& configFilePath);
+ /// Convert nodes to vector of elements
+ /// \param fileName the original filename for error reporting
+ /// \param node the node to be converted
+ /// \param [out] elements the pointer for the results
+ /// \return true if the conversion was successful
+ bool tryConvertElements(const std::string& fileName, const YAML::Node& node,
+ std::vector<std::shared_ptr<SceneElement>>* elements);
+
/// 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::vector<std::shared_ptr<ComponentManager>> m_managers;
std::shared_ptr<Scene> m_scene;
static std::shared_ptr<ApplicationData> m_applicationData;
- boost::mutex m_mutex;
+ boost::mutex m_sceneHandling;
std::shared_ptr<Barrier> m_barrier;
bool m_isPaused;
+
+ bool m_isStopped;
};
+template <class T>
+std::shared_ptr<T>
+SurgSim::Framework::Runtime::getManager() const
+{
+ std::shared_ptr<T> result;
+ for (auto& manager : m_managers)
+ {
+ result = std::dynamic_pointer_cast<T>(manager);
+ if (result != nullptr)
+ {
+ return result;
+ }
+ }
+ return result;
+}
+
+/// Perform a YAML load operation
+/// \param fileName the filename of the scene to be loaded, needs to be found
+/// \param [out] node pointer to the nodes structure to receive the newly loaded nodes
+/// \return true if the loading succeeded
+bool tryLoadNode(const std::string& fileName, YAML::Node* node);
+
+
}; // namespace Framework
}; // namespace SurgSim
diff --git a/SurgSim/Framework/SamplingMetricBase.cpp b/SurgSim/Framework/SamplingMetricBase.cpp
new file mode 100644
index 0000000..4cec78b
--- /dev/null
+++ b/SurgSim/Framework/SamplingMetricBase.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 "SamplingMetricBase.h"
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Accessible.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+SamplingMetricBase::SamplingMetricBase(const std::string& name) :
+ SurgSim::Framework::Behavior(name),
+ m_logger(SurgSim::Framework::Logger::getLogger(name)),
+ m_targetManagerType(SurgSim::Framework::MANAGER_TYPE_BEHAVIOR),
+ m_elapsedTime(0.0),
+ m_maxNumberOfMeasurements(54000)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SurgSim::Framework::SamplingMetricBase, size_t, MaxNumberOfMeasurements,
+ getMaxNumberOfMeasurements, setMaxNumberOfMeasurements);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SurgSim::Framework::SamplingMetricBase, int, TargetManagerType,
+ getTargetManagerType, setTargetManagerType);
+}
+
+bool SamplingMetricBase::doWakeUp()
+{
+ return true;
+}
+
+bool SamplingMetricBase::doInitialize()
+{
+ m_elapsedTime = 0.0;
+ m_measurementValues.clear();
+ return true;
+}
+
+bool SamplingMetricBase::canMeasure(double dt)
+{
+ return true;
+}
+
+SamplingMetricBase::MeasurementsType SamplingMetricBase::getMeasurementValues()
+{
+ return m_measurementValues;
+}
+
+int SamplingMetricBase::getTargetManagerType() const
+{
+ return m_targetManagerType;
+}
+
+void SamplingMetricBase::setTargetManagerType(int targetManagerType)
+{
+ m_targetManagerType = targetManagerType;
+}
+
+void SamplingMetricBase::setMaxNumberOfMeasurements(size_t maxNumberOfMeasurements)
+{
+ m_maxNumberOfMeasurements = (maxNumberOfMeasurements > 0) ? maxNumberOfMeasurements : 1;
+ if (m_measurementValues.size() > m_maxNumberOfMeasurements)
+ {
+ m_measurementValues.erase(std::begin(m_measurementValues),
+ std::begin(m_measurementValues) +
+ m_measurementValues.size() -
+ m_maxNumberOfMeasurements);
+ }
+}
+
+size_t SamplingMetricBase::getMaxNumberOfMeasurements() const
+{
+ return m_maxNumberOfMeasurements;
+}
+
+size_t SamplingMetricBase::getCurrentNumberOfMeasurements() const
+{
+ return m_measurementValues.size();
+}
+
+double SamplingMetricBase::getElapsedTime() const
+{
+ return m_elapsedTime;
+}
+
+void SamplingMetricBase::update(double dt)
+{
+ m_elapsedTime += dt;
+ if (canMeasure(dt))
+ {
+ MeasurementEntryType newEntry;
+ newEntry.first = m_elapsedTime;
+ newEntry.second = performMeasurement(dt);
+ m_measurementValues.push_back(newEntry);
+ if (m_measurementValues.size() > m_maxNumberOfMeasurements)
+ {
+ m_measurementValues.pop_front();
+ }
+
+ }
+}
+}
+}
+
+
diff --git a/SurgSim/Framework/SamplingMetricBase.h b/SurgSim/Framework/SamplingMetricBase.h
new file mode 100644
index 0000000..032e25f
--- /dev/null
+++ b/SurgSim/Framework/SamplingMetricBase.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_FRAMEWORK_SAMPLINGMETRICBASE_H
+#define SURGSIM_FRAMEWORK_SAMPLINGMETRICBASE_H
+
+#include <deque>
+
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+class Logger;
+};
+};
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// SamplingMetricBase provides a base class to support metric development. New
+/// metrics should derive from the base class and redefine canMeasure() and
+/// takeMeasurement() ensure the system is ready to be measured and to perform
+/// the measurement respectively.
+///
+/// The nominal setting provides for 30 minutes of continuous sampling at 30 hertz.
+/// After that time, old measurements will be discarded as new measurements are made
+/// so as to stay within the same memory footprint.
+class SamplingMetricBase : public SurgSim::Framework::Behavior
+{
+public:
+ /// Constructor for the class.
+ /// \param name is the name given to the behavior.
+ explicit SamplingMetricBase(const std::string& name);
+
+ /// Type of the individual entries in the measurement data structure. The first field of the
+ /// pair holds the elapsed simulation time (accumulation of the dt value) since the last successful
+ /// measurement and the second field of the pair holds the measurement value obtained at that time.
+ typedef std::pair<double, double> MeasurementEntryType;
+
+ /// Type of the cumulative entries data structure. The code current caps the number of entries at a
+ /// user prescribed value to keep from overwriting all of memory when the process is allowed to run
+ /// unchecked over long periods. The maximum number of entries is nominally capped at 30 minutes of samples
+ /// taken 30 times per second, but it can be adjusted using he setMaxNumberOfMeasurements call. Note
+ /// that the last measurements taken are always saved. After the limit is reached the oldest entry is
+ /// discarded to make room for the new measurement.
+ typedef std::deque<MeasurementEntryType> MeasurementsType;
+
+ void update(double dt) override;
+
+ /// Set the desired manager type for this metric. Given the potential tight coupling of the
+ /// and the various other behaviors, this will provide us with the flexibility to choose
+ /// the appropriate manager for the task.
+ /// \param targetManagerType is the manager type to be used for managing this metric
+ void setTargetManagerType(int targetManagerType);
+
+ int getTargetManagerType() const override;
+
+ /// Set the maximum number of measurements to store.
+ void setMaxNumberOfMeasurements(size_t numberOfMeasurements);
+
+ /// \return Maximum number of measurements to be stored.
+ size_t getMaxNumberOfMeasurements() const;
+
+ /// \return Number of measurements currently stored (not the maximum number of measurements).
+ size_t getCurrentNumberOfMeasurements() const;
+
+ /// Get the amount of time since the last successful measurement reading based on the
+ /// accumulation of successive dt values.
+ /// \return the elapsed time
+ double getElapsedTime() const;
+
+ /// Return the measurement values obtained for this measurement.
+ virtual MeasurementsType getMeasurementValues();
+
+protected:
+
+ bool doWakeUp() override;
+
+ bool doInitialize() override;
+
+ /// Determine if it is appropriate to take a measurement.
+ /// \param dt is the elapsed time since the last call to update.
+ /// \return if it is currently valid to calculate the next measurement value.
+ /// \note Be careful with threading when implementing this call. Anything referenced both
+ /// here and in performMeasurement() must be safe.
+ virtual bool canMeasure(double dt);
+
+ /// Obtain the measurement.
+ /// \param dt is the elapsed time since the last call to update.
+ /// \return the next measurement value. This method should be overwritten to provide the
+ /// various measurements for the simulation.
+ /// \note Be careful with threading when implementing this call. Anything referenced both
+ /// here and in canMeasure() must be safe.
+ virtual double performMeasurement(double dt) = 0;
+
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+
+private:
+ /// measurement list
+ MeasurementsType m_measurementValues;
+ int m_targetManagerType;
+ double m_elapsedTime;
+ size_t m_maxNumberOfMeasurements;
+};
+};
+};
+
+#endif // SURGSIM_FRAMEWORK_SAMPLINGMETRICBASE_H
diff --git a/SurgSim/Framework/Scene.cpp b/SurgSim/Framework/Scene.cpp
index b248a8f..cdb781d 100644
--- a/SurgSim/Framework/Scene.cpp
+++ b/SurgSim/Framework/Scene.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest LLC.
+// Copyright 2013-2016, SimQuest LLC.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -15,18 +15,18 @@
#include "SurgSim/Framework/Scene.h"
+#include <boost/thread/locks.hpp>
+#include <set>
+#include <utility>
+#include <vector>
+#include <yaml-cpp/yaml.h>
+
+#include "SurgSim/Framework/Component.h"
#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.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
{
@@ -34,7 +34,8 @@ namespace Framework
{
Scene::Scene(std::weak_ptr<Runtime> runtime) :
- m_runtime(runtime)
+ m_runtime(runtime),
+ m_logger(Framework::Logger::getLogger("Framework/Scene"))
{
SURGSIM_ASSERT(!m_runtime.expired()) << "Can't create scene with empty runtime.";
}
@@ -46,12 +47,11 @@ Scene::~Scene()
void Scene::addSceneElement(std::shared_ptr<SceneElement> element)
{
- SURGSIM_ASSERT(!m_runtime.expired()) << "Runtime pointer is expired, cannot add SceneElement to Scene.";
+ auto runtime = getRuntime();
+ SURGSIM_ASSERT(runtime) << "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())
@@ -59,10 +59,36 @@ void Scene::addSceneElement(std::shared_ptr<SceneElement> element)
{
boost::lock_guard<boost::mutex> lock(m_sceneElementsMutex);
m_elements.push_back(element);
+ m_groups.add(element->getGroups(), element);
}
runtime->addSceneElement(element);
}
+}
+
+void Scene::removeSceneElement(std::shared_ptr<SceneElement> element)
+{
+ element->setActive(false);
+ element->removeComponents();
+ boost::lock_guard<boost::mutex> lock(m_sceneElementsMutex);
+ auto found = std::find(m_elements.begin(), m_elements.end(), element);
+ if (found != m_elements.end())
+ {
+ m_elements.erase(found);
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(m_logger)
+ << "Could not find element '" << element->getName() << "' in Scene, unable to remove";
+ }
+}
+
+void Scene::addSceneElements(std::vector<std::shared_ptr<SceneElement>> elements)
+{
+ for (auto element : elements)
+ {
+ addSceneElement(element);
+ }
}
std::shared_ptr<Runtime> Scene::getRuntime()
@@ -139,6 +165,32 @@ bool Scene::decode(const YAML::Node& node)
}
return result;
}
+
+SurgSim::DataStructures::Groups<std::string, std::shared_ptr<SceneElement>>& Scene::getGroups()
+{
+ return m_groups;
+}
+
+std::shared_ptr<Component> Scene::getComponent(const std::string& elementName, const std::string& componentName) const
+{
+ std::shared_ptr<Component> result;
+ auto element = getSceneElement(elementName);
+ if (element != nullptr)
+ {
+ result = element->getComponent(componentName);
+ if (result == nullptr)
+ {
+ SURGSIM_LOG_INFO(m_logger)
+ << "Could not find component '" << componentName << "' in Element '" << elementName << "'.";
+ }
+ }
+ else
+ {
+ SURGSIM_LOG_INFO(m_logger) << "Could not find element '" << elementName << "'.";
+ }
+ return result;
+}
+
}; // namespace Framework
}; // namespace SurgSim
diff --git a/SurgSim/Framework/Scene.h b/SurgSim/Framework/Scene.h
index 91f349d..b24edef 100644
--- a/SurgSim/Framework/Scene.h
+++ b/SurgSim/Framework/Scene.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,13 +16,15 @@
#ifndef SURGSIM_FRAMEWORK_SCENE_H
#define SURGSIM_FRAMEWORK_SCENE_H
-#include <vector>
+#include <boost/thread/mutex.hpp>
#include <memory>
#include <string>
-#include <boost/thread/mutex.hpp>
+#include <vector>
+#include "SurgSim/DataStructures/Groups.h"
#include "SurgSim/Framework/SceneElement.h"
+
namespace YAML
{
class Node;
@@ -33,6 +35,7 @@ namespace SurgSim
namespace Framework
{
+class Logger;
class Runtime;
/// Scene. Basic Container for SceneElements
@@ -48,9 +51,17 @@ public:
~Scene();
/// Adds a scene element to the Scene, the SceneElement will have its initialize() function called.
- /// \param element The element.
+ /// \param element The element.
void addSceneElement(std::shared_ptr<SceneElement> element);
+ /// Removes a scene element from the Scene
+ /// \param element The element.
+ void removeSceneElement(std::shared_ptr<SceneElement> element);
+
+ /// Invokes addSceneElement() for each element in the list.
+ /// \param elements the list of elements to be added.
+ void addSceneElements(std::vector<std::shared_ptr<SceneElement>> elements);
+
/// Gets all the scene elements in the scene.
/// \return The scene elements.
const std::vector<std::shared_ptr<SceneElement>>& getSceneElements() const;
@@ -59,6 +70,12 @@ public:
/// \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;
+ /// Look through the scene to find a component of a named element
+ /// \param elementName The name of the element to find
+ /// \param componentName The name of the component to find
+ /// \return the component that was found, nullptr if no component was found
+ std::shared_ptr<Component> getComponent(const std::string& elementName, const std::string& componentName) const;
+
/// Gets the runtime.
/// \return runtime The runtime for this scene.
std::shared_ptr<Runtime> getRuntime();
@@ -72,18 +89,24 @@ public:
/// \return true if the decoding succeeded and the node was formatted correctly, false otherwise
bool decode(const YAML::Node& node);
+ /// \return the groups of the scene
+ SurgSim::DataStructures::Groups<std::string, std::shared_ptr<SceneElement>>& getGroups();
+
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;
+
+ SurgSim::DataStructures::Groups<std::string, std::shared_ptr<SceneElement>> m_groups;
+
+ std::shared_ptr<Framework::Logger> m_logger;
};
}; // namespace Framework
diff --git a/SurgSim/Framework/SceneElement-inl.h b/SurgSim/Framework/SceneElement-inl.h
index 078b570..be19e14 100644
--- a/SurgSim/Framework/SceneElement-inl.h
+++ b/SurgSim/Framework/SceneElement-inl.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,6 +16,9 @@
#ifndef SURGSIM_FRAMEWORK_SCENEELEMENT_INL_H
#define SURGSIM_FRAMEWORK_SCENEELEMENT_INL_H
+#include "SurgSim/Framework/Component.h"
+
+
namespace SurgSim
{
@@ -40,6 +43,28 @@ std::vector<std::shared_ptr<T>> SceneElement::getComponents() const
return result;
}
+template <class T>
+T SceneElement::getValue(const std::string& component, const std::string& property) const
+{
+ auto found = m_components.find(component);
+ SURGSIM_ASSERT(found != m_components.end())
+ << "Component named " << component << " not found in SceneElement named " << getName()
+ << ". Cannot get " << property << " property.";
+ return found->second->getValue<T>(property);
+}
+
+template <class T>
+bool SceneElement::getValue(const std::string& component, const std::string& property, T* value) const
+{
+ bool result = false;
+ auto found = m_components.find(component);
+ if (found != m_components.end())
+ {
+ result = found->second->getValue(property, value);
+ }
+ return result;
+}
+
}; // namespace Framework
}; // namespace SurgSim
diff --git a/SurgSim/Framework/SceneElement.cpp b/SurgSim/Framework/SceneElement.cpp
index c0524c2..08e9dc7 100644
--- a/SurgSim/Framework/SceneElement.cpp
+++ b/SurgSim/Framework/SceneElement.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest LLC.
+// Copyright 2013-2016, SimQuest LLC.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -17,11 +17,14 @@
#include <yaml-cpp/yaml.h>
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
#include "SurgSim/Framework/Component.h"
-#include "SurgSim/Framework/Log.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"
+
namespace SurgSim
{
@@ -33,8 +36,8 @@ SceneElement::SceneElement(const std::string& name) :
m_isInitialized(false),
m_isActive(true)
{
- m_pose = std::make_shared<SurgSim::Framework::PoseComponent>("Pose");
- m_pose->setPose(SurgSim::Math::RigidTransform3d::Identity());
+ m_pose = std::make_shared<PoseComponent>("Pose");
+ m_pose->setPose(Math::RigidTransform3d::Identity());
m_components[m_pose->getName()] = m_pose;
}
@@ -64,7 +67,16 @@ bool SceneElement::addComponent(std::shared_ptr<Component> component)
if (result)
{
- component->setSceneElement(getSharedPtr());
+ try
+ {
+ // This will except if called during constructor
+ // the this will be set in initialize()
+ component->setSceneElement(shared_from_this());
+ }
+ catch (std::exception)
+ {
+ // intentionally empty
+ }
m_components[component->getName()] = component;
}
}
@@ -87,14 +99,40 @@ bool SceneElement::removeComponent(std::shared_ptr<Component> component)
bool SceneElement::removeComponent(const std::string& name)
{
bool result = false;
- if (m_components.find(name) != m_components.end())
+ auto found = m_components.find(name);
+ if (found != m_components.end())
{
+ if (isInitialized())
+ {
+ auto runtime = getRuntime();
+ SURGSIM_ASSERT(nullptr != runtime) << "Runtime cannot be expired when removing a component " << getName();
+ found->second->setLocalActive(false);
+ runtime->removeComponent(found->second);
+ }
+
size_t count = m_components.erase(name);
result = (count == 1);
}
return result;
}
+void SceneElement::removeComponents()
+{
+ if (!m_components.empty())
+ {
+ auto runtime = getRuntime();
+ SURGSIM_ASSERT(runtime) << "Runtime cannot be expired when removing components from " << getName();
+
+ for (auto it = std::begin(m_components); it != std::end(m_components); ++it)
+ {
+ it->second->setLocalActive(false);
+ runtime->removeComponent(it->second);
+ }
+
+ m_components.clear();
+ }
+}
+
std::shared_ptr<Component> SceneElement::getComponent(const std::string& name) const
{
std::shared_ptr<Component> result;
@@ -112,7 +150,7 @@ bool SceneElement::initialize()
SURGSIM_ASSERT(!m_isInitialized) << "Double initialization calls on SceneElement " << m_name;
m_isInitialized = doInitialize();
- // For completeness
+ // If it did not happen in addComponent do it here
m_pose->setSceneElement(getSharedPtr());
if (m_isInitialized)
@@ -129,6 +167,7 @@ bool SceneElement::initialize()
bool SceneElement::initializeComponent(std::shared_ptr<SurgSim::Framework::Component> component)
{
+ component->setSceneElement(getSharedPtr());
component->setScene(m_scene);
return component->initialize(getRuntime());
}
@@ -164,6 +203,24 @@ std::vector<std::shared_ptr<Component>> SceneElement::getComponents() const
return result;
}
+boost::any SceneElement::getValue(const std::string& component, const std::string& property) const
+{
+ auto found = m_components.find(component);
+ SURGSIM_ASSERT(found != m_components.end())
+ << "Component named " << component << " not found in SceneElement named " << getName()
+ << ". Cannot get " << property << " property.";
+ return found->second->getValue(property);
+}
+
+void SceneElement::setValue(const std::string& component, const std::string& property, const boost::any& value)
+{
+ auto found = m_components.find(component);
+ SURGSIM_ASSERT(found != m_components.end())
+ << "Component named " << component << " not found in SceneElement named " << getName()
+ << ". Cannot get " << property << " property.";
+ found->second->setValue(property, value);
+}
+
void SceneElement::setScene(std::weak_ptr<Scene> scene)
{
m_scene = scene;
@@ -218,6 +275,13 @@ YAML::Node SceneElement::encode(bool standalone) const
data["Name"] = getName();
data["IsActive"] = isActive();
+ // Only encode groups when they are there, also encode them using the flow style i.e. with []
+ if (m_groups.size() > 0)
+ {
+ data["Groups"] = m_groups;
+ data["Groups"].SetStyle(YAML::EmitterStyle::Flow);
+ }
+
for (auto component = std::begin(m_components); component != std::end(m_components); ++component)
{
if (standalone)
@@ -242,18 +306,30 @@ bool SceneElement::decode(const YAML::Node& node)
{
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() << ">.";
+ 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>();
+ SURGSIM_ASSERT(data.IsDefined() && !data.IsNull())
+ << "Content of node is empty, for class " << className << ". This is probably an indentation issue.";
+
+ if (data["Name"].IsScalar())
+ {
+ m_name = data["Name"].as<std::string>();
+ }
if (data["IsActive"].IsDefined())
{
m_isActive = data["IsActive"].as<bool>();
}
+ if (data["Groups"].IsDefined())
+ {
+
+ m_groups = data["Groups"].as<std::unordered_set<std::string>>();
+ }
+
if (data["Components"].IsSequence())
{
for (auto nodeIt = data["Components"].begin(); nodeIt != data["Components"].end(); ++nodeIt)
@@ -261,12 +337,19 @@ bool SceneElement::decode(const YAML::Node& node)
if ("SurgSim::Framework::PoseComponent" == nodeIt->begin()->first.as<std::string>())
{
removeComponent(m_pose);
- m_pose = nodeIt->as<std::shared_ptr<SurgSim::Framework::PoseComponent>>();
+ m_pose = nodeIt->as<std::shared_ptr<PoseComponent>>();
addComponent(m_pose);
}
else
{
- addComponent(nodeIt->as<std::shared_ptr<SurgSim::Framework::Component>>());
+ try
+ {
+ addComponent(nodeIt->as<std::shared_ptr<Component>>());
+ }
+ catch (YAML::Exception e)
+ {
+ SURGSIM_FAILURE() << e.what() << "for " << std::endl << *nodeIt;
+ }
}
}
result = true;
@@ -291,5 +374,46 @@ bool SceneElement::isActive() const
return m_isActive;
}
+void SceneElement::addToGroup(const std::string& group)
+{
+ m_groups.insert(group);
+ if (isInitialized())
+ {
+ getScene()->getGroups().add(group, getSharedPtr());
+ }
+}
+
+void SceneElement::removeFromGroup(const std::string& group)
+{
+ m_groups.erase(group);
+ if (isInitialized())
+ {
+ getScene()->getGroups().remove(group, getSharedPtr());
+ }
+}
+
+void SceneElement::setGroups(const std::vector<std::string>& groups)
+{
+ if (isInitialized())
+ {
+ auto& groupings = getScene()->getGroups();
+ groupings.remove(getSharedPtr());
+ groupings.add(groups, getSharedPtr());
+ }
+ std::unordered_set<std::string> temp(groups.cbegin(), groups.cend());
+ std::swap(m_groups, temp);
+}
+
+std::vector<std::string> SceneElement::getGroups() const
+{
+ return std::vector<std::string>(m_groups.cbegin(), m_groups.cend());
+}
+
+bool SceneElement::inGroup(const std::string& name)
+{
+ auto groups = getGroups();
+ return (std::find(groups.begin(), groups.end(), name) != groups.end());
+}
+
}; // namespace Framework
}; // namespace SurgSim
diff --git a/SurgSim/Framework/SceneElement.h b/SurgSim/Framework/SceneElement.h
index 37a227f..7391398 100644
--- a/SurgSim/Framework/SceneElement.h
+++ b/SurgSim/Framework/SceneElement.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,14 +16,17 @@
#ifndef SURGSIM_FRAMEWORK_SCENEELEMENT_H
#define SURGSIM_FRAMEWORK_SCENEELEMENT_H
-#include <string>
-#include <memory>
#include <algorithm>
+#include <boost/any.hpp>
+#include <memory>
+#include <string>
#include <unordered_map>
+#include <unordered_set>
#include <vector>
#include "SurgSim/Math/RigidTransform.h"
+
namespace YAML
{
class Node;
@@ -74,6 +77,9 @@ public:
/// \return true if it succeeds, false if it fails or the component cannot be found.
bool removeComponent(const std::string& name);
+ /// Removes all components
+ void removeComponents();
+
/// Gets the component identified by name.
/// \param name The name.
/// \return The component or nullptr if the component cannot be found.
@@ -81,13 +87,66 @@ public:
/// Gets all the components of this SceneElement.
/// \return The components.
- std::vector<std::shared_ptr<Component> > getComponents() const;
+ 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;
+ /// Retrieves the property value from a component
+ /// \throws SurgSim::Framework::AssertionFailure If the conversion fails or the property cannot be found.
+ /// \tparam T The requested type for the property.
+ /// \param component The name of the component.
+ /// \param property The name of the property.
+ /// \return The value of the property if found.
+ template <class T>
+ T getValue(const std::string& component, const std::string& property) const;
+
+ /// Retrieves the property value from a component
+ /// \throws SurgSim::Framework::AssertionFailure if the property cannot be found
+ /// \param component The name of the component.
+ /// \param property The name of the property.
+ /// \return The value of the property if found.
+ boost::any getValue(const std::string& component, const std::string& property) const;
+
+ /// Retrieves the property value from a component, and convertis it to the
+ /// type of the output parameter. This does not throw.
+ /// \tparam T the type of the property
+ /// \param component The name of the component.
+ /// \param property 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 property is found.
+ template <class T>
+ bool getValue(const std::string& component, const std::string& property, T* value) const;
+
+ /// Sets the property value of a component
+ /// \throws SurgSim::Framework::AssertionFailure If the property cannot be found.
+ /// \param component The name of the component.
+ /// \param property The name of the property.
+ /// \param value The value that it should be set to.
+ void setValue(const std::string& component, const std::string& property, const boost::any& value);
+
+ /// Add this scene element to the given group
+ /// \param group name of the group
+ void addToGroup(const std::string& group);
+
+ /// Remove this scene element from the given group
+ /// \param group name of the group
+ void removeFromGroup(const std::string& group);
+
+ /// Set the groups of this scene element
+ /// \param groups all the groups that this scene element should be in
+ void setGroups(const std::vector<std::string>& groups);
+
+ /// \return all the groups of this scene element
+ std::vector<std::string> getGroups() const;
+
+ /// Test whether this SceneElement is in a specific group
+ /// \param name group to test
+ /// \return true if the SceneElement is in group name
+ bool inGroup(const std::string& name);
+
/// Executes the initialize operation.
/// \return true if it succeeds, false if it fails.
bool initialize();
@@ -175,6 +234,9 @@ private:
/// A (weak) back pointer to the Runtime containing this SceneElement
std::weak_ptr<Runtime> m_runtime;
+ /// Local groups for serialization local handling
+ std::unordered_set<std::string> m_groups;
+
/// Method to initialize this SceneElement. To be overridden by derived class(es).
/// \return True if initialization is successful; Otherwise, false.
virtual bool doInitialize() = 0;
diff --git a/SurgSim/Framework/SharedInstance-inl.h b/SurgSim/Framework/SharedInstance-inl.h
index c34567e..4b8b475 100644
--- a/SurgSim/Framework/SharedInstance-inl.h
+++ b/SurgSim/Framework/SharedInstance-inl.h
@@ -27,8 +27,8 @@
namespace SurgSim
{
- namespace Framework
- {
+namespace Framework
+{
template <typename T>
SharedInstance<T>::SharedInstance() :
diff --git a/SurgSim/Framework/ThreadPool-inl.h b/SurgSim/Framework/ThreadPool-inl.h
new file mode 100644
index 0000000..7773093
--- /dev/null
+++ b/SurgSim/Framework/ThreadPool-inl.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_THREADPOOL_INL_H
+#define SURGSIM_FRAMEWORK_THREADPOOL_INL_H
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+class ThreadPool::TaskBase
+{
+public:
+ virtual void execute() = 0;
+
+ virtual ~TaskBase() {}
+};
+
+template<class R>
+class ThreadPool::Task : public ThreadPool::TaskBase
+{
+public:
+ explicit Task(std::function<R()> function) :
+ m_task(function)
+ {
+ }
+
+ void execute() override
+ {
+ m_task();
+ }
+
+ std::future<R> getFuture()
+ {
+ return m_task.get_future();
+ }
+
+private:
+ std::packaged_task<R()> m_task;
+};
+
+template <class R>
+std::future<R> ThreadPool::enqueue(std::function<R()> function)
+{
+ std::unique_ptr<Task<R>> task = std::unique_ptr<Task<R>>(new Task<R>(function));
+ std::future<R> future = task->getFuture();
+ {
+ boost::unique_lock<boost::mutex> lock(m_mutex);
+ m_tasks.push(std::move(task));
+ }
+ m_threadSignaler.notify_one();
+ return future;
+}
+
+};
+};
+
+#endif //SURGSIM_FRAMEWORK_THREADPOOL_INL_H
+
+
diff --git a/SurgSim/Framework/ThreadPool.cpp b/SurgSim/Framework/ThreadPool.cpp
new file mode 100644
index 0000000..63293f4
--- /dev/null
+++ b/SurgSim/Framework/ThreadPool.cpp
@@ -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.
+
+#include "SurgSim/Framework/ThreadPool.h"
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+ThreadPool::ThreadPool(size_t numThreads) :
+ m_destructing(false)
+{
+ // Main loop for each worker thread.
+ auto threadLoop = [this] ()
+ {
+ while (true)
+ {
+ std::unique_ptr<TaskBase> task;
+ {
+ boost::unique_lock<boost::mutex> lock(m_mutex);
+ m_threadSignaler.wait(lock, [this] { return m_destructing || !m_tasks.empty(); });
+ if (m_destructing)
+ {
+ return;
+ }
+
+ task = std::move(m_tasks.front());
+ m_tasks.pop();
+ }
+ task->execute();
+ }
+ };
+
+ for (size_t i = 0; i < numThreads; i++)
+ {
+ m_threads.emplace_back(threadLoop);
+ }
+}
+
+ThreadPool::~ThreadPool()
+{
+ {
+ boost::unique_lock<boost::mutex> lock(m_mutex);
+ m_destructing = true;
+ }
+ m_threadSignaler.notify_all();
+
+ for (auto& thread : m_threads)
+ {
+ thread.join();
+ }
+}
+
+};
+};
diff --git a/SurgSim/Framework/ThreadPool.h b/SurgSim/Framework/ThreadPool.h
new file mode 100644
index 0000000..f81dc59
--- /dev/null
+++ b/SurgSim/Framework/ThreadPool.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_FRAMEWORK_THREADPOOL_H
+#define SURGSIM_FRAMEWORK_THREADPOOL_H
+
+#include <atomic>
+#include <boost/thread.hpp>
+#include <functional>
+#include <future>
+#include <list>
+#include <memory>
+#include <queue>
+
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+/// A thread pool for completing heterogenous tasks
+///
+/// The thread pool is a class that completes given tasks using a set of worker
+/// threads. These threads pull tasks off of a task queue. Once finished with
+/// the task, the thread gets the next task if one is available, or waits for
+/// another task to be added. The tasks can be heterogenous, meaning any
+/// callable target can be added with any return type.
+///
+/// Example Usage:
+/// \code{.cpp}
+/// double f1() { return 1.0; }
+/// int f2(int val) { return val; }
+///
+/// int main()
+/// {
+/// ThreadPool pool;
+///
+/// // Add a task
+/// std::future<double> result1 = pool.enqueue<double>(f1);
+///
+/// // Add a task using std::bind
+/// std::future<int> result2 = pool.enqueue<int>(std::bind(f2, 2));
+///
+/// // Add a task using a lambda function
+/// std::future<std::string> result3 = pool.enqueue<std::string>([]() {return "string"; });
+///
+/// // Print out result when task is completed
+/// std::cout << "Result 1: " << result1.get() << std::endl;
+/// std::cout << "Result 2: " << result2.get() << std::endl;
+/// std::cout << "Result 3: " << result3.get() << std::endl;
+/// }
+/// \endcode
+class ThreadPool
+{
+public:
+ /// Constructor
+ /// \param numThreads The number of worker threads
+ explicit ThreadPool(size_t numThreads = boost::thread::hardware_concurrency());
+
+ /// Desctructor
+ ~ThreadPool();
+
+ /// Queue a task to be run by the ThreadPool
+ /// \note The task must not take any arguments. To add a function that does
+ /// require arguments use std::bind.
+ /// \tparam R return type of the task
+ /// \param function The task to be queued
+ /// \return a std::future that holds the results (of type R) once completed
+ template <class R>
+ std::future<R> enqueue(std::function<R()> function);
+
+private:
+ /// @{
+ /// Prevent default copy construction and default assignment
+ ThreadPool(const ThreadPool& other);
+ ThreadPool& operator=(const ThreadPool& other);
+ /// @}
+
+ /// Abstract base class for all tasks
+ class TaskBase;
+
+ /// Actual tasks, with typed return type
+ template<class R>
+ class Task;
+
+ /// The worker threads
+ std::list<boost::thread> m_threads;
+
+ /// Queued tasks waiting for an available thread
+ std::queue<std::unique_ptr<TaskBase>> m_tasks;
+
+ /// Mutex for protecting the tasks queue
+ boost::mutex m_mutex;
+
+ /// Signaler for waking up threads waiting for tasks
+ boost::condition_variable m_threadSignaler;
+
+ /// True if the ThreadPool is destructing
+ std::atomic<bool> m_destructing;
+};
+
+};
+};
+
+#include "SurgSim/Framework/ThreadPool-inl.h"
+
+#endif //SURGSIM_FRAMEWORK_THREADPOOL_H
+
+
+
diff --git a/SurgSim/Framework/Timer.cpp b/SurgSim/Framework/Timer.cpp
index 6b821c3..af327b1 100644
--- a/SurgSim/Framework/Timer.cpp
+++ b/SurgSim/Framework/Timer.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -29,8 +29,11 @@ Timer::Timer() :
void Timer::start()
{
- m_frameDurations.clear();
- m_clockFails = 0;
+ { // Define scope around m_frameDurations to lock only this access
+ boost::unique_lock<boost::shared_mutex> lock(m_sharedMutex);
+ m_frameDurations.clear();
+ m_clockFails = 0;
+ }
beginFrame();
}
@@ -42,10 +45,15 @@ void Timer::beginFrame()
void Timer::endFrame()
{
TimerTimePoint currentTime = now();
- m_frameDurations.push_back(currentTime - m_lastTime);
- if (m_frameDurations.size() > m_maxNumberOfFrames)
- {
- m_frameDurations.pop_front();
+ TimerDuration duration = currentTime - m_lastTime;
+
+ { // Define scope around m_frameDurations to lock only this access
+ boost::unique_lock<boost::shared_mutex> lock(m_sharedMutex);
+ m_frameDurations.push_back(duration);
+ if (m_frameDurations.size() > m_maxNumberOfFrames)
+ {
+ m_frameDurations.pop_front();
+ }
}
}
@@ -57,16 +65,34 @@ void Timer::markFrame()
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());
+ TimerDuration cumulativeTime;
+
+ { // Define scope around m_frameDurations to lock only this access
+ boost::shared_lock<boost::shared_mutex> lock(m_sharedMutex);
+
+ cumulativeTime = std::accumulate(std::begin(m_frameDurations), std::end(m_frameDurations),
+ TimerDuration::zero());
+ }
+
return cumulativeTime.count();
}
double Timer::getAverageFramePeriod() const
{
- return getCumulativeTime() / m_frameDurations.size();
+ TimerDuration cumulativeTime;
+ size_t numDuration;
+
+ { // Define scope around m_frameDurations to lock only this access
+ boost::shared_lock<boost::shared_mutex> lock(m_sharedMutex);
+
+ numDuration = m_frameDurations.size();
+ cumulativeTime = std::accumulate(std::begin(m_frameDurations), std::end(m_frameDurations),
+ TimerDuration::zero());
+ }
+ SURGSIM_ASSERT(numDuration > 0) <<
+ "Attempted to access the frames for a Timer with no frames.";
+
+ return cumulativeTime.count() / static_cast<double>(numDuration);
}
double Timer::getAverageFrameRate() const
@@ -76,6 +102,8 @@ double Timer::getAverageFrameRate() const
double Timer::getLastFramePeriod() const
{
+ boost::shared_lock<boost::shared_mutex> lock(m_sharedMutex);
+
SURGSIM_ASSERT(m_frameDurations.size() > 0) <<
"Attempted to access the last frame period for a Timer with no frames.";
return m_frameDurations.back().count();
@@ -88,6 +116,8 @@ double Timer::getLastFrameRate() const
void Timer::setMaxNumberOfFrames(size_t maxNumberOfFrames)
{
+ boost::unique_lock<boost::shared_mutex> lock(m_sharedMutex);
+
m_maxNumberOfFrames = (maxNumberOfFrames > 0) ? maxNumberOfFrames : 1;
if (m_frameDurations.size() > m_maxNumberOfFrames)
{
@@ -96,8 +126,15 @@ void Timer::setMaxNumberOfFrames(size_t maxNumberOfFrames)
}
}
+size_t Timer::getMaxNumberOfFrames()
+{
+ return m_maxNumberOfFrames;
+}
+
size_t Timer::getCurrentNumberOfFrames() const
{
+ boost::shared_lock<boost::shared_mutex> lock(m_sharedMutex);
+
return m_frameDurations.size();
}
@@ -119,6 +156,8 @@ Timer::TimerTimePoint Timer::now()
double Timer::getMaxFramePeriod() const
{
+ boost::shared_lock<boost::shared_mutex> lock(m_sharedMutex);
+
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();
@@ -126,10 +165,18 @@ double Timer::getMaxFramePeriod() const
double Timer::getMinFramePeriod() const
{
+ boost::shared_lock<boost::shared_mutex> lock(m_sharedMutex);
+
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();
}
+bool Timer::isBufferFull() const
+{
+ boost::shared_lock<boost::shared_mutex> lock(m_sharedMutex);
+ return (m_frameDurations.size() == m_maxNumberOfFrames);
+}
+
}; // namespace Framework
}; // namespace SurgSim
diff --git a/SurgSim/Framework/Timer.h b/SurgSim/Framework/Timer.h
index 9ff56a3..017e9d6 100644
--- a/SurgSim/Framework/Timer.h
+++ b/SurgSim/Framework/Timer.h
@@ -17,6 +17,7 @@
#define SURGSIM_FRAMEWORK_TIMER_H
#include <boost/chrono.hpp>
+#include <boost/thread/shared_mutex.hpp>
#include <deque>
namespace SurgSim
@@ -46,7 +47,7 @@ public:
/// 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 the sum of the durations over all the stored frames.
/// \return Sum of stored frame durations in seconds.
double getCumulativeTime() const;
@@ -70,6 +71,9 @@ public:
/// Set the maximum number of frames to store.
void setMaxNumberOfFrames(size_t numberOfFrames);
+ /// \return The maximum number of frames to store.
+ size_t getMaxNumberOfFrames();
+
/// \return Number of frames currently stored (not the maximum number of frames).
size_t getCurrentNumberOfFrames() const;
@@ -83,6 +87,9 @@ public:
/// \return The minimum duration across all the stored frames. Asserts if there are no frames.
double getMinFramePeriod() const;
+ /// \return true if the frame buffer is full.
+ bool isBufferFull() const;
+
private:
/// The Clock used by the Timer class.
typedef boost::chrono::steady_clock TimerClock;
@@ -109,6 +116,9 @@ private:
/// Durations of the frames, i.e., the "stored frames".
std::deque<TimerDuration> m_frameDurations;
+ /// Mutex to access the data structure m_frameDurations safely
+ mutable boost::shared_mutex m_sharedMutex;
+
/// Number of clock errors since last \c start.
size_t m_clockFails;
};
diff --git a/SurgSim/Framework/TransferPropertiesBehavior.cpp b/SurgSim/Framework/TransferPropertiesBehavior.cpp
index fa6b6cd..55c9339 100644
--- a/SurgSim/Framework/TransferPropertiesBehavior.cpp
+++ b/SurgSim/Framework/TransferPropertiesBehavior.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -113,7 +113,7 @@ bool TransferPropertiesBehavior::doWakeUp()
void TransferPropertiesBehavior::setTargetManagerType(int managerType)
{
- SURGSIM_ASSERT(managerType > 0 && managerType < MANAGER_TYPE_COUNT) << "Invalid manager type.";
+ SURGSIM_ASSERT(managerType >= 0 && managerType < MANAGER_TYPE_COUNT) << "Invalid manager type.";
SURGSIM_ASSERT(!isInitialized()) << "Cannot change the manager type after initialization.";
m_targetManager = managerType;
}
diff --git a/SurgSim/Framework/TransferPropertiesBehavior.h b/SurgSim/Framework/TransferPropertiesBehavior.h
index 05b5b92..2d160ac 100644
--- a/SurgSim/Framework/TransferPropertiesBehavior.h
+++ b/SurgSim/Framework/TransferPropertiesBehavior.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,12 +16,12 @@
#ifndef SURGSIM_FRAMEWORK_TRANSFERPROPERTIESBEHAVIOR_H
#define SURGSIM_FRAMEWORK_TRANSFERPROPERTIESBEHAVIOR_H
+#include <boost/thread/mutex.hpp>
#include <string>
-#include "SurgSim/Framework/Behavior.h"
#include "SurgSim/Framework/Accessible.h"
+#include "SurgSim/Framework/Behavior.h"
-#include <boost/thread/mutex.hpp>
namespace SurgSim
{
@@ -72,17 +72,16 @@ public:
/// \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;
+ int getTargetManagerType() const override;
+
+ void update(double dt) override;
private:
///@{
/// Overridden from Behavior
- virtual bool doInitialize() override;
- virtual bool doWakeUp() override;
- virtual int getTargetManagerType() const;
+ bool doInitialize() override;
+ bool doWakeUp() override;
///@}
/// Local typedefs
@@ -96,6 +95,8 @@ private:
/// Queue for adding new connections
std::vector<Connection> m_incomingConnections;
+
+ /// The manager type that will handle this behavior
int m_targetManager;
};
diff --git a/SurgSim/Framework/UnitTests/AccessibleTests.cpp b/SurgSim/Framework/UnitTests/AccessibleTests.cpp
index f42181f..501d8bf 100644
--- a/SurgSim/Framework/UnitTests/AccessibleTests.cpp
+++ b/SurgSim/Framework/UnitTests/AccessibleTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -23,6 +23,10 @@
#include "SurgSim/Math/Matrix.h"
+
+enum TestEnum : SURGSIM_ENUM_TYPE;
+SURGSIM_SERIALIZABLE_ENUM(TestEnum, (TEST_ENUM_A)(TEST_ENUM_B));
+
namespace
{
@@ -34,9 +38,10 @@ public:
readWrite(100.0),
readOnly(100),
sharedPtr(std::make_shared<int>(4)),
+ serializableEnum(TEST_ENUM_A),
overloadedValue(200.0),
virtualProperty(300),
- privateProperty(100) // Don't forget to keep this last, otherwise there will be a gcc warning
+ privateProperty(100)
{
setGetter("normal", std::bind(&TestClass::getNormal, this));
setSetter("normal", std::bind(&TestClass::setNormal, this, std::bind(SurgSim::Framework::convert<int>,
@@ -52,6 +57,9 @@ public:
SURGSIM_ADD_SERIALIZABLE_PROPERTY(TestClass, float, serializableProperty,
getSerializableProperty, setSerializableProperty);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(TestClass, TestEnum, serializableEnum,
+ getSerializableEnum, setSerializableEnum);
+
SURGSIM_ADD_RO_PROPERTY(TestClass, int, virtualProperty, getVirtualProperty);
SURGSIM_ADD_RO_PROPERTY(TestClass, int, overriddenProperty, getReadWrite);
@@ -61,11 +69,12 @@ public:
double readWrite;
int readOnly;
-
std::shared_ptr<int> sharedPtr;
float serializableProperty;
+ TestEnum serializableEnum;
+
double overloadedValue;
int virtualProperty;
@@ -120,6 +129,15 @@ public:
serializableProperty = val;
}
+ TestEnum getSerializableEnum() const
+ {
+ return serializableEnum;
+ }
+ void setSerializableEnum(TestEnum val)
+ {
+ serializableEnum = val;
+ }
+
void getOverloadedFunction(double* x) const {}
double getOverloadedFunction() const
{
@@ -163,7 +181,7 @@ public:
}
// Overrides the accessor, this tests if the virtual function resolution works on the function pointer
- virtual int getVirtualProperty() override
+ int getVirtualProperty() override
{
return otherValue;
}
@@ -171,6 +189,7 @@ public:
};
}
+
namespace SurgSim
{
namespace Framework
@@ -358,19 +377,46 @@ TEST(AccessibleTest, ConvertDoubleToFloat)
EXPECT_TRUE(target.isApprox(sourceFloat));
}
+TEST(AccessibleTest, ConvertConstCharToString)
+{
+ std::string source("Test Value");
+
+ {
+ std::string target;
+ ASSERT_NO_THROW({target = convert<std::string>(source);});
+ EXPECT_EQ(source, target);
+ }
+ {
+ std::string target;
+ ASSERT_NO_THROW({target = convert<std::string>(source.c_str());});
+ EXPECT_EQ(source, target);
+ }
+
+ float notString;
+ ASSERT_THROW({convert<std::string>(notString);}, boost::bad_any_cast);
+}
+
TEST(AccessibleTests, Serialize)
{
TestClass a;
a.serializableProperty = 100;
+ a.serializableEnum = TEST_ENUM_B;
YAML::Node node = a.encode();
EXPECT_TRUE(node.IsMap());
EXPECT_EQ(100, node["serializableProperty"].as<int>());
+ EXPECT_EQ(TEST_ENUM_B, node["serializableEnum"].as<TestEnum>());
+ EXPECT_EQ("TEST_ENUM_B", node["serializableEnum"].as<std::string>());
node["serializableProperty"] = 50;
+ node["serializableEnum"] = "TEST_ENUM_A";
EXPECT_NO_THROW(a.decode(node));
EXPECT_EQ(50, a.serializableProperty);
+ EXPECT_EQ(TEST_ENUM_A, a.serializableEnum);
+
+ node["serializableEnum"] = "INVALID_ENUM_STRING";
+ EXPECT_THROW(a.decode(node), SurgSim::Framework::AssertionFailure);
}
class MultipleValuesClass : public Accessible
@@ -388,7 +434,7 @@ public:
{
return a;
}
- void setA(std::string val)
+ void setA(const std::string& val)
{
a = val;
}
@@ -398,7 +444,7 @@ public:
{
return b;
}
- void setB(std::string val)
+ void setB(const std::string& val)
{
b = val;
}
@@ -408,7 +454,7 @@ public:
{
return c;
}
- void setC(std::string val)
+ void setC(const std::string& val)
{
c = val;
}
diff --git a/SurgSim/Framework/UnitTests/AssetTests.cpp b/SurgSim/Framework/UnitTests/AssetTests.cpp
index 3c1de38..dd50b1a 100644
--- a/SurgSim/Framework/UnitTests/AssetTests.cpp
+++ b/SurgSim/Framework/UnitTests/AssetTests.cpp
@@ -31,6 +31,8 @@ public:
MockAsset() {}
~MockAsset() {}
+ SURGSIM_CLASSNAME(MockAsset)
+
virtual bool doLoad(const std::string& fileName)
{
bool result = false;
diff --git a/SurgSim/Framework/UnitTests/BasicSceneElementTests.cpp b/SurgSim/Framework/UnitTests/BasicSceneElementTests.cpp
index ebbf90d..42e6d42 100644
--- a/SurgSim/Framework/UnitTests/BasicSceneElementTests.cpp
+++ b/SurgSim/Framework/UnitTests/BasicSceneElementTests.cpp
@@ -26,6 +26,8 @@
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Testing/Utilities.h"
+
#include <gtest/gtest.h>
#include <random>
@@ -86,6 +88,7 @@ TEST(BasicSceneElementTests, SerializationTest)
sceneElement->addComponent(representation1);
sceneElement->addComponent(representation2);
sceneElement->setActive(false);
+ sceneElement->addToGroup("One");
RigidTransform3d pose(makeRigidTransform(Quaterniond(0.0, 1.0, 0.0, 0.0), Vector3d(1.0, 2.0, 3.0)));
sceneElement->setPose(pose);
@@ -104,6 +107,7 @@ TEST(BasicSceneElementTests, SerializationTest)
EXPECT_EQ(3u, result->getComponents().size());
EXPECT_TRUE(pose.isApprox(result->getPose()));
EXPECT_FALSE(result->isActive());
+ EXPECT_TRUE(SurgSim::Testing::doesContain(result->getGroups(), "One"));
}
}; // namespace Blocks
diff --git a/SurgSim/Framework/UnitTests/BasicThreadTests.cpp b/SurgSim/Framework/UnitTests/BasicThreadTests.cpp
index 49d3375..60a158a 100644
--- a/SurgSim/Framework/UnitTests/BasicThreadTests.cpp
+++ b/SurgSim/Framework/UnitTests/BasicThreadTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -61,6 +61,8 @@ TEST(BasicThreadTest, Stop)
EXPECT_TRUE(m.isRunning());
m.stop();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+
EXPECT_TRUE(m.didBeforeStop);
EXPECT_FALSE(m.isRunning());
}
@@ -88,18 +90,18 @@ TEST(BasicThreadTest, RunTimeManagement)
std::shared_ptr<Barrier> barrier = std::make_shared<Barrier>(2);
EXPECT_FALSE(m.didInitialize);
EXPECT_FALSE(m.didStartUp);
+ EXPECT_FALSE(m.isRunning());
m.start(barrier);
boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ EXPECT_TRUE(m.isRunning());
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();
}
@@ -114,7 +116,7 @@ TEST(BasicThreadTest, DestructStoppedThread)
m->stop();
- EXPECT_NO_THROW(m.release());
+ EXPECT_NO_THROW(m.reset());
}
TEST(BasicThreadTest, SynchronousThread)
@@ -177,11 +179,10 @@ TEST(BasicThreadTest, PauseResumeUpdateTest)
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);
}
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ EXPECT_EQ(m.count, previousCount - 11);
m.setIdle(true);
@@ -193,11 +194,10 @@ TEST(BasicThreadTest, PauseResumeUpdateTest)
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);
}
+ EXPECT_EQ(previousCount, m.count);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
m.setIdle(false);
@@ -206,14 +206,13 @@ TEST(BasicThreadTest, PauseResumeUpdateTest)
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);
}
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ EXPECT_EQ(m.count, previousCount - 10);
barrier->wait(false);
- boost::this_thread::sleep(boost::posix_time::milliseconds(10));
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
m.stop();
}
@@ -251,7 +250,7 @@ TEST(BasicThreadTest, SwitchSyncOnThread)
// 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);
+ EXPECT_EQ(count - 1, m.count);
count = count - 1;
m.setSynchronous(false);
@@ -271,6 +270,35 @@ TEST(BasicThreadTest, SwitchSyncOnThread)
m.stop();
}
+TEST(BasicThreadTest, RealTimings)
+{
+ MockThread m;
+ m.start(nullptr);
+
+ while (m.getCpuTime() < 1e-6);
+ EXPECT_GT(m.getCpuTime(), 1e-6);
+ EXPECT_GT(m.getUpdateCount(), 0u);
+
+ // Ask the manager to idle for a while, just the time for us to reset the timer and check right after
+ // what the timer contains (the delay between the reset and the checks could trigger a race condition).
+ // Asking the thread to idle suppress this race condition.
+ m.setIdle(true);
+
+ // Reset the timer (=> no more frames in the timer queue)
+ m.resetCpuTimeAndUpdateCount();
+ EXPECT_NEAR(0.0, m.getCpuTime(), 1e-9);
+ EXPECT_EQ(m.getUpdateCount(), 0u);
+
+ // Resume the thread loop update.
+ m.setIdle(false);
+
+ while (m.getCpuTime() < 1e-6);
+ EXPECT_GT(m.getCpuTime(), 1e-6);
+ EXPECT_GT(m.getUpdateCount(), 0u);
+
+ 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
{
@@ -284,6 +312,7 @@ public:
void TearDown()
{
m->stop();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
}
std::shared_ptr<MockThread> m;
@@ -294,7 +323,8 @@ TEST_F(BasicThreadDeathTest, DestructLiveThread)
::testing::FLAGS_gtest_death_test_style = "threadsafe";
boost::this_thread::sleep(boost::posix_time::milliseconds(100));
- ASSERT_DEATH_IF_SUPPORTED({
+ ASSERT_DEATH_IF_SUPPORTED(
+ {
SurgSim::Framework::AssertMessage::setFailureBehaviorToDeath();
m.reset();
}, "");
diff --git a/SurgSim/Framework/UnitTests/BehaviorManagerTest.cpp b/SurgSim/Framework/UnitTests/BehaviorManagerTest.cpp
index 4213b3e..c5739d8 100644
--- a/SurgSim/Framework/UnitTests/BehaviorManagerTest.cpp
+++ b/SurgSim/Framework/UnitTests/BehaviorManagerTest.cpp
@@ -46,10 +46,11 @@ TEST(BehaviorManagerTest, BehaviorInitTest)
runtime->start();
EXPECT_TRUE(behaviorManager->isInitialized());
- boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ boost::this_thread::sleep(boost::posix_time::milliseconds(250));
+ EXPECT_TRUE(behavior->isAwake());
runtime->stop();
- EXPECT_TRUE(behavior->isAwake());
+ EXPECT_FALSE(behavior->isAwake());
EXPECT_FALSE(component->isAwake());
EXPECT_GT(behavior->updateCount, 0);
diff --git a/SurgSim/Framework/UnitTests/CMakeLists.txt b/SurgSim/Framework/UnitTests/CMakeLists.txt
index cced9df..a2e444e 100644
--- a/SurgSim/Framework/UnitTests/CMakeLists.txt
+++ b/SurgSim/Framework/UnitTests/CMakeLists.txt
@@ -36,9 +36,11 @@ set(UNIT_TEST_SOURCES
ObjectFactoryTests.cpp
ReuseFactoryTest.cpp
RuntimeTest.cpp
+ SamplingMetricBaseTest.cpp
SceneElementTest.cpp
SceneTest.cpp
SharedInstanceTest.cpp
+ ThreadPoolTest.cpp
TimerTest.cpp
TransferPropertiesBehaviorTests.cpp
)
diff --git a/SurgSim/Framework/UnitTests/ComponentTest.cpp b/SurgSim/Framework/UnitTests/ComponentTest.cpp
index 61bd4ef..3909280 100644
--- a/SurgSim/Framework/UnitTests/ComponentTest.cpp
+++ b/SurgSim/Framework/UnitTests/ComponentTest.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -246,7 +246,7 @@ TEST(ComponentTests, NullPointerSerialization)
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>>());
+ containerNode.as<std::shared_ptr<Component>>());
EXPECT_EQ(nullptr, newContainer->getComponentOne());
EXPECT_EQ(nullptr, newContainer->getComponentTwo());
EXPECT_EQ(container->getName(), newContainer->getName());
@@ -486,7 +486,9 @@ TEST(ComponentTests, SetActiveTest)
EXPECT_FALSE(component->getValue<bool>("IsActive"));
auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("SceneElement");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
sceneElement->addComponent(component);
+ runtime->getScene()->addSceneElement(sceneElement);
EXPECT_TRUE(sceneElement->isActive());
// An inactive component in an active SceneElement is 'inactive'.
@@ -519,6 +521,7 @@ TEST(ComponentTests, SetActiveTest)
break;
}
}
+ ASSERT_NE(nullptr, decodedComponent);
EXPECT_EQ(nullptr, decodedComponent->getSceneElement());
EXPECT_TRUE(decodedComponent->isActive());
EXPECT_TRUE(decodedComponent->isLocalActive());
@@ -528,9 +531,26 @@ TEST(ComponentTests, SetActiveTest)
decodedComponent = nullptr;
auto decodedSceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Decoded");
decodedSceneElement->decode(node);
+ runtime->getScene()->addSceneElement(decodedSceneElement);
EXPECT_FALSE(decodedSceneElement->isActive());
decodedComponent = decodedSceneElement->getComponent("Component");
EXPECT_NE(nullptr, decodedComponent->getSceneElement());
EXPECT_FALSE(decodedComponent->isActive());
EXPECT_TRUE(decodedComponent->isLocalActive());
}
+
+TEST(ComponentTests, CheckAndConvertTest)
+{
+ using SurgSim::Framework::checkAndConvert;
+
+ auto original = std::make_shared<MockComponent>("test");
+ auto other = std::make_shared<MockBehavior>("other");
+ std::shared_ptr<Component> source = original;
+ std::shared_ptr<MockComponent> result;
+
+ EXPECT_NO_THROW(result = checkAndConvert<MockComponent>(source, "MockComponent"));
+ EXPECT_EQ(result, original);
+
+
+ EXPECT_ANY_THROW(result = checkAndConvert<MockComponent>(other, "MockBehavior"));
+}
diff --git a/SurgSim/Framework/UnitTests/Data/SceneElementTest/circle-1.yaml b/SurgSim/Framework/UnitTests/Data/SceneElementTest/circle-1.yaml
new file mode 100644
index 0000000..b986b8b
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/Data/SceneElementTest/circle-1.yaml
@@ -0,0 +1,9 @@
+- SurgSim::Framework::BasicSceneElement:
+ Name: element
+ IsActive: true
+ Components:
+ - MockComponent:
+ Name: clone
+- INCLUDE: SceneElementTest/circle-2.yaml
+
+
diff --git a/SurgSim/Framework/UnitTests/Data/SceneElementTest/circle-2.yaml b/SurgSim/Framework/UnitTests/Data/SceneElementTest/circle-2.yaml
new file mode 100644
index 0000000..967b8af
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/Data/SceneElementTest/circle-2.yaml
@@ -0,0 +1,9 @@
+- SurgSim::Framework::BasicSceneElement:
+ Name: element
+ IsActive: true
+ Components:
+ - MockComponent:
+ Name: clone
+- INCLUDE: SceneElementTest/circle-1.yaml
+
+
diff --git a/SurgSim/Framework/UnitTests/Data/SceneElementTest/included.yaml b/SurgSim/Framework/UnitTests/Data/SceneElementTest/included.yaml
new file mode 100644
index 0000000..1164017
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/Data/SceneElementTest/included.yaml
@@ -0,0 +1,13 @@
+- SurgSim::Framework::BasicSceneElement:
+ Name: element2
+ IsActive: true
+ Components:
+ - MockComponent:
+ Name: clone
+- SurgSim::Framework::BasicSceneElement:
+ Name: element3
+ IsActive: true
+ Components:
+ - MockComponent:
+ Name: clone
+
diff --git a/SurgSim/Framework/UnitTests/Data/SceneElementTest/includer.yaml b/SurgSim/Framework/UnitTests/Data/SceneElementTest/includer.yaml
new file mode 100644
index 0000000..cdcf1c4
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/Data/SceneElementTest/includer.yaml
@@ -0,0 +1,10 @@
+- INCLUDE: SceneElementTest/single.yaml
+- SurgSim::Framework::BasicSceneElement:
+ Name: element
+ IsActive: true
+ Components:
+ - MockComponent:
+ Name: clone
+- INCLUDE: SceneElementTest/included.yaml
+
+
diff --git a/SurgSim/Framework/UnitTests/Data/SceneElementTest/single.yaml b/SurgSim/Framework/UnitTests/Data/SceneElementTest/single.yaml
new file mode 100644
index 0000000..2fd80fd
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/Data/SceneElementTest/single.yaml
@@ -0,0 +1,6 @@
+SurgSim::Framework::BasicSceneElement:
+ Name: element2
+ IsActive: true
+ Components:
+ - MockComponent:
+ Name: clone
diff --git a/SurgSim/Framework/UnitTests/Data/SceneTestData/bad.yaml b/SurgSim/Framework/UnitTests/Data/SceneTestData/bad.yaml
new file mode 100644
index 0000000..42daa67
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/Data/SceneTestData/bad.yaml
@@ -0,0 +1,10 @@
+- SurgSim::Framework::BasicSceneElement:
+ Name: element2
+ IsActive: true
+ Components:
+ - MockComponent:
+ Name: clone
+ Id 1de26315-82a7-46b2-ae38-324d25009629
+ IsLocalActive: true
+ SucceedWithInit: true
+ SucceedWithWakeUp: true
diff --git a/SurgSim/Framework/UnitTests/Data/SceneTestData/element.yaml b/SurgSim/Framework/UnitTests/Data/SceneTestData/element.yaml
new file mode 100644
index 0000000..4afad5e
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/Data/SceneTestData/element.yaml
@@ -0,0 +1,10 @@
+- SurgSim::Framework::BasicSceneElement:
+ Name: element2
+ IsActive: true
+ Components:
+ - MockComponent:
+ Name: clone
+ Id: 1de26315-82a7-46b2-ae38-324d25009629
+ IsLocalActive: true
+ SucceedWithInit: true
+ SucceedWithWakeUp: true
diff --git a/SurgSim/Framework/UnitTests/Data/SceneTestData/elements.yaml b/SurgSim/Framework/UnitTests/Data/SceneTestData/elements.yaml
new file mode 100644
index 0000000..f58d5c8
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/Data/SceneTestData/elements.yaml
@@ -0,0 +1,20 @@
+- SurgSim::Framework::BasicSceneElement:
+ IsActive: true
+ Name: element0
+ Components:
+ - MockComponent:
+ Name: component0
+ Id: 394ca44f-6117-4eef-90a1-52b81e430e3a
+ IsLocalActive: true
+ SucceedWithInit: true
+ SucceedWithWakeUp: true
+- SurgSim::Framework::BasicSceneElement:
+ Name: element1
+ IsActive: true
+ Components:
+ - MockComponent:
+ Name: component1
+ Id: 2d27d6e1-2628-40a7-9e1d-da8b5032228f
+ IsLocalActive: true
+ SucceedWithInit: true
+ SucceedWithWakeUp: true
\ No newline at end of file
diff --git a/SurgSim/Framework/UnitTests/Data/SceneTestData/scene.yaml b/SurgSim/Framework/UnitTests/Data/SceneTestData/scene.yaml
new file mode 100644
index 0000000..9df8aac
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/Data/SceneTestData/scene.yaml
@@ -0,0 +1,36 @@
+SurgSim::Framework::Scene:
+ SceneElements:
+ - SurgSim::Framework::BasicSceneElement:
+ IsActive: true
+ Components:
+ - SurgSim::Framework::PoseComponent:
+ Name: Pose
+ Id: 71fc22e9-a908-48d5-b0f9-198d60513117
+ IsLocalActive: true
+ Pose:
+ Translation: [0, 0, 0]
+ Quaternion: [0, 0, 0, 1]
+ - MockComponent:
+ Name: component0
+ IsLocalActive: true
+ SucceedWithInit: true
+ SucceedWithWakeUp: true
+ Id: 350c883b-0283-4e34-8148-5b281b758e3b
+ Name: element0
+ - SurgSim::Framework::BasicSceneElement:
+ Components:
+ - MockComponent:
+ IsLocalActive: true
+ SucceedWithInit: true
+ SucceedWithWakeUp: true
+ Id: 5def200c-6515-4c2b-a818-936d616c43af
+ Name: component1
+ - SurgSim::Framework::PoseComponent:
+ IsLocalActive: true
+ Pose:
+ Quaternion: [0, 0, 0, 1]
+ Translation: [0, 0, 0]
+ Id: 65a4bc87-4d3f-487b-aac9-a9db5a250a28
+ Name: Pose
+ Name: element1
+ IsActive: true
\ No newline at end of file
diff --git a/SurgSim/Framework/UnitTests/LockedContainerTest.cpp b/SurgSim/Framework/UnitTests/LockedContainerTest.cpp
index 83c0165..59e95f1 100644
--- a/SurgSim/Framework/UnitTests/LockedContainerTest.cpp
+++ b/SurgSim/Framework/UnitTests/LockedContainerTest.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -578,11 +578,12 @@ void testReaderAndWriters(int numWriters)
LockedContainer<BigData> data;
- std::vector<DataWriter*> writers(numWriters);
+ std::vector<std::unique_ptr<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);
+ writers[i] = std::unique_ptr<DataWriter>(new DataWriter(data, static_cast<int>(i), numWriters,
+ NUM_TOTAL_WRITES / numWriters));
}
{
BigData value;
diff --git a/SurgSim/Framework/UnitTests/LoggerManagerTest.cpp b/SurgSim/Framework/UnitTests/LoggerManagerTest.cpp
index 56c81a8..5cde623 100644
--- a/SurgSim/Framework/UnitTests/LoggerManagerTest.cpp
+++ b/SurgSim/Framework/UnitTests/LoggerManagerTest.cpp
@@ -125,14 +125,11 @@ TEST(LoggerManagerTest, Threshold)
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
+ /// setThreshold(pattern, level) will affect newly created loggers if the logger's name matches the pattern
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());
+ EXPECT_EQ(SurgSim::Framework::LOG_LEVEL_WARNING, logger5->getThreshold());
/// Logger manager should own the logger.
loggerManager->getLogger("xxx")->setThreshold(SurgSim::Framework::LOG_LEVEL_DEBUG);
diff --git a/SurgSim/Framework/UnitTests/MockObjects.cpp b/SurgSim/Framework/UnitTests/MockObjects.cpp
index 33cfcad..0401e75 100644
--- a/SurgSim/Framework/UnitTests/MockObjects.cpp
+++ b/SurgSim/Framework/UnitTests/MockObjects.cpp
@@ -22,7 +22,8 @@ MockComponent::MockComponent(const std::string& name, bool succeedInit, bool suc
succeedWithInit(succeedInit),
succeedWithWakeUp(succeedWakeUp),
didWakeUp(false),
- didInit(false)
+ didInit(false),
+ didRetire(false)
{
SURGSIM_ADD_SERIALIZABLE_PROPERTY(
MockComponent, bool, SucceedWithInit, getSucceedWithInit, setSucceedWithInit);
@@ -47,6 +48,12 @@ bool MockComponent::doWakeUp()
return succeedWithWakeUp;
}
+void MockComponent::doRetire()
+{
+ didRetire = true;
+ Component::doRetire();
+}
+
bool MockComponent::getSucceedWithInit() const
{
return succeedWithInit;
diff --git a/SurgSim/Framework/UnitTests/MockObjects.h b/SurgSim/Framework/UnitTests/MockObjects.h
index 3d0726d..f6f0c41 100644
--- a/SurgSim/Framework/UnitTests/MockObjects.h
+++ b/SurgSim/Framework/UnitTests/MockObjects.h
@@ -25,6 +25,7 @@
#include "SurgSim/Framework/ComponentManager.h"
#include "SurgSim/Framework/Representation.h"
#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/SamplingMetricBase.h"
#include "SurgSim/Framework/Scene.h"
#include "SurgSim/Framework/SceneElement.h"
@@ -136,8 +137,9 @@ public:
virtual ~MockComponent();
- virtual bool doInitialize();
- virtual bool doWakeUp();
+ bool doInitialize() override;
+ bool doWakeUp() override;
+ void doRetire() override;
bool getSucceedWithInit() const;
void setSucceedWithInit(bool val);
@@ -149,12 +151,13 @@ public:
bool succeedWithWakeUp;
bool didWakeUp;
bool didInit;
+ bool didRetire;
};
class MockBehavior : public SurgSim::Framework::Behavior
{
public:
- MockBehavior(const std::string& name, bool succeedInit = true, bool succeedWakeUp = true) :
+ explicit MockBehavior(const std::string& name, bool succeedInit = true, bool succeedWakeUp = true) :
Behavior(name),
succeedWithInit(succeedInit),
succeedWithWakeUp(succeedWakeUp),
@@ -189,7 +192,7 @@ public:
class MockManager : public SurgSim::Framework::ComponentManager
{
public:
- MockManager(bool succeedInit = true, bool succeedStartup = true) :
+ explicit MockManager(bool succeedInit = true, bool succeedStartup = true) :
succeedInit(succeedInit),
succeedStartup(succeedStartup),
didInitialize(false),
@@ -203,7 +206,7 @@ public:
{
}
- virtual int getType() const override
+ int getType() const override
{
return SurgSim::Framework::MANAGER_TYPE_NONE;
}
@@ -256,14 +259,16 @@ private:
virtual void doBeforeStop()
{
didBeforeStop = true;
+ retireComponents(m_components);
+ ComponentManager::doBeforeStop();
}
- virtual bool executeAdditions(const std::shared_ptr<SurgSim::Framework::Component>& component) override
+ 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
+ bool executeRemovals(const std::shared_ptr<SurgSim::Framework::Component>& component) override
{
return tryRemoveComponent(component, &m_components);
}
@@ -326,4 +331,53 @@ private:
};
+class MockSamplingMetric : public SurgSim::Framework::SamplingMetricBase
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ explicit MockSamplingMetric(const std::string& name,
+ bool ableToMeasure = true,
+ double initialMeasurement = 0.0) :
+ SurgSim::Framework::SamplingMetricBase(name),
+ m_canMeasure(ableToMeasure),
+ m_measurement(initialMeasurement)
+ {
+ }
+
+ SURGSIM_CLASSNAME(MockSamplingMetric);
+
+ /// Type of the individual entries in the measurement data structure. The first field of the
+ /// pair holds the elapsed time since the start of the measurement process and the second field
+ /// of the pair holds the measurement value obtained at that time.
+ typedef SurgSim::Framework::SamplingMetricBase::MeasurementEntryType MeasurementEntryType;
+
+ /// Type of the cumulative entries data structure. The code current caps the number of entries at a
+ /// user prescribed value to keep from overwriting all of memory when the process is allowed to run
+ /// unchecked over long periods. The maximum number of entries is nominally capped at 30 minutes of samples
+ /// taken 30 times per second, but it can be adjusted using he setMaxNumberOfMeasurements call. Note
+ /// that we always save the last measurements taken. After the limit is reached we delete the oldest
+ /// current entry every time we need to add a new measurement.
+ typedef SurgSim::Framework::SamplingMetricBase::MeasurementsType MeasurementsType;
+
+private:
+ /// Whether the representation has been initialized
+ bool m_canMeasure;
+
+ /// Next measurement value to return.
+ double m_measurement;
+
+ /// Return if the measurement can be made at this point
+ /// \return true or false
+ bool canMeasure(double dt)
+ {
+ return m_canMeasure;
+ }
+
+ double performMeasurement(double dt)
+ {
+ return ++m_measurement;
+ }
+};
+
#endif // SURGSIM_FRAMEWORK_UNITTESTS_MOCKOBJECTS_H
diff --git a/SurgSim/Framework/UnitTests/ObjectFactoryTests.cpp b/SurgSim/Framework/UnitTests/ObjectFactoryTests.cpp
index 1e803c3..7dc49a6 100644
--- a/SurgSim/Framework/UnitTests/ObjectFactoryTests.cpp
+++ b/SurgSim/Framework/UnitTests/ObjectFactoryTests.cpp
@@ -33,7 +33,7 @@ public:
}
- explicit TestClass(std::string val) : stringValue(val)
+ explicit TestClass(const std::string& val) : stringValue(val)
{
}
diff --git a/SurgSim/Framework/UnitTests/RuntimeTest.cpp b/SurgSim/Framework/UnitTests/RuntimeTest.cpp
index 1f34331..d62918f 100644
--- a/SurgSim/Framework/UnitTests/RuntimeTest.cpp
+++ b/SurgSim/Framework/UnitTests/RuntimeTest.cpp
@@ -58,7 +58,7 @@ 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));
+ std::shared_ptr<MockManager> managerFails(new MockManager(false, true));
runtime->addManager(managerSucceeds);
runtime->addManager(managerFails);
@@ -70,7 +70,7 @@ 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));
+ std::shared_ptr<MockManager> managerFails(new MockManager(true, false));
runtime->addManager(managerSucceeds);
runtime->addManager(managerFails);
@@ -118,13 +118,18 @@ TEST(RuntimeTest, SceneInitialization)
EXPECT_TRUE(elements[i]->didInit);
}
- for (int i=0; i<4; i++)
+ for (int i = 0; i < 4; i++)
{
EXPECT_TRUE(components[i]->didInit);
}
runtime->stop();
EXPECT_TRUE(manager->didBeforeStop);
+
+ for (int i = 0; i < 4; i++)
+ {
+ EXPECT_TRUE(components[i]->didRetire);
+ }
}
TEST(RuntimeTest, PausedStep)
@@ -149,11 +154,11 @@ TEST(RuntimeTest, PausedStep)
runtime->step();
boost::this_thread::sleep(boost::posix_time::milliseconds(150));
- EXPECT_EQ(count+1,manager1->count);
+ EXPECT_EQ(count + 1, manager1->count);
runtime->step();
boost::this_thread::sleep(boost::posix_time::milliseconds(150));
- EXPECT_EQ(count+2,manager1->count);
+ EXPECT_EQ(count + 2, manager1->count);
runtime->stop();
}
@@ -237,4 +242,99 @@ TEST(RuntimeTest, AddComponentAddDuringRuntime)
boost::this_thread::sleep(boost::posix_time::milliseconds(150));
runtime->stop();
+
+ EXPECT_FALSE(components[1]->isAwake());
+}
+
+TEST(RuntimeTest, LoadAndAddScene)
+{
+ auto runtime = std::make_shared<Runtime>("config.txt");
+
+ EXPECT_NO_THROW(runtime->loadScene("SceneTestData/scene.yaml"));
+
+ auto scene0 = runtime->getScene();
+
+ EXPECT_NE(nullptr, scene0);
+ EXPECT_EQ(2L, scene0->getSceneElements().size());
+
+ EXPECT_NO_THROW(runtime->loadScene("SceneTestData/scene.yaml"));
+
+ auto scene1 = runtime->getScene();
+
+ EXPECT_NE(nullptr, scene1);
+ EXPECT_NE(scene0, scene1);
+
+ EXPECT_NO_THROW(runtime->addSceneElements("SceneTestData/elements.yaml"));
+
+ auto scene2 = runtime->getScene();
+
+ EXPECT_NE(nullptr, scene2);
+ EXPECT_EQ(scene1, scene2);
+
+ EXPECT_EQ(4L, scene2->getSceneElements().size());
+}
+
+TEST(RuntimeTest, LoadAndDuplicate)
+{
+ auto runtime = std::make_shared<Runtime>("config.txt");
+ EXPECT_NO_THROW(runtime->loadScene("SceneTestData/scene.yaml"));
+ auto scene0 = runtime->getScene();
+
+ auto elements0 = runtime->duplicateSceneElements("SceneTestData/element.yaml");
+ auto elements1 = runtime->duplicateSceneElements("SceneTestData/element.yaml");
+
+ EXPECT_EQ(1L, elements0.size());
+ EXPECT_EQ(1L, elements1.size());
+
+ // Without the correct cloning, both of these elements would point to the same instance which would be bad
+
+ auto component0 = elements0[0]->getComponent("clone");
+ auto component1 = elements1[0]->getComponent("clone");
+ ASSERT_NE(nullptr, component0);
+ ASSERT_NE(nullptr, component1);
+
+ EXPECT_NE(component0, component1);
+
+ component0->setLocalActive(false);
+ EXPECT_NE(component0->isLocalActive(), component1->isLocalActive());
+}
+
+TEST(RuntimeTest, DuplicateBadYaml)
+{
+ auto runtime = std::make_shared<Runtime>("config.txt");
+ EXPECT_NO_THROW(runtime->loadScene("SceneTestData/scene.yaml"));
+ auto scene0 = runtime->getScene();
+
+ EXPECT_ANY_THROW(auto elements0 = runtime->duplicateSceneElements("SceneTestData/bad.yaml"););
}
+
+TEST(RuntimeTest, ManagerAccess)
+{
+ auto runtime = std::make_shared<Runtime>();
+
+ auto managers = runtime->getManagers();
+ EXPECT_EQ(0L, managers.size());
+
+ auto manager1 = std::make_shared<MockManager>();
+ auto manager2 = std::make_shared<MockManager>();
+
+ runtime->addManager(manager1);
+ runtime->addManager(manager2);
+
+ managers = runtime->getManagers();
+ EXPECT_EQ(2L, managers.size());
+}
+
+TEST(RuntimeTest, TypedManagerAccess)
+{
+ auto runtime = std::make_shared<Runtime>();
+ auto manager1 = std::make_shared<MockManager>();
+
+ runtime->addManager(manager1);
+
+ auto manager = runtime->getManager<MockManager>();
+ EXPECT_EQ(manager1, manager);
+
+ auto impossible = runtime->getManager<Runtime>();
+ EXPECT_EQ(nullptr, impossible);
+}
\ No newline at end of file
diff --git a/SurgSim/Framework/UnitTests/SamplingMetricBaseTest.cpp b/SurgSim/Framework/UnitTests/SamplingMetricBaseTest.cpp
new file mode 100644
index 0000000..fbcee4c
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/SamplingMetricBaseTest.cpp
@@ -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.
+
+#include <gtest/gtest.h>
+#include <deque>
+
+#include "SurgSim/Framework/SamplingMetricBase.h"
+#include "SurgSim/Framework/UnitTests/MockObjects.h"
+
+TEST(SamplingMetricBaseTest, SamplingMetricBaseInitTest)
+{
+ std::shared_ptr<MockSamplingMetric> mockMetric(std::make_shared<MockSamplingMetric>("Test Metric"));
+
+ auto samples = mockMetric->getMeasurementValues();
+
+ // Test initialization settings.
+ EXPECT_EQ("Test Metric", mockMetric->getName());
+ EXPECT_EQ(0, samples.size());
+ EXPECT_EQ(0.0, mockMetric->getElapsedTime());
+ EXPECT_EQ(SurgSim::Framework::MANAGER_TYPE_BEHAVIOR, mockMetric->getTargetManagerType());
+ EXPECT_LT(0u, mockMetric->getMaxNumberOfMeasurements());
+ EXPECT_EQ(0, mockMetric->getCurrentNumberOfMeasurements());
+}
+
+TEST(SamplingMetricBaseTest, SetGetTests)
+{
+ std::shared_ptr<MockSamplingMetric> mockMetric(std::make_shared<MockSamplingMetric>("Test Metric"));
+
+ auto samples = mockMetric->getMeasurementValues();
+
+ // Test sets and gets.
+ mockMetric->setName("Test2");
+ mockMetric->setMaxNumberOfMeasurements(5);
+ mockMetric->setTargetManagerType(SurgSim::Framework::MANAGER_TYPE_PHYSICS);
+
+ EXPECT_EQ("Test2", mockMetric->getName());
+ EXPECT_EQ(5, mockMetric->getMaxNumberOfMeasurements());
+ EXPECT_EQ(SurgSim::Framework::MANAGER_TYPE_PHYSICS, mockMetric->getTargetManagerType());
+
+}
+
+TEST(SamplingMetricBaseTest, AbleToPerformMeasurementsTests)
+{
+ std::shared_ptr<MockSamplingMetric> mockMetric(std::make_shared<MockSamplingMetric>("Test Metric", true, 1.0));
+
+ // Make 10 updates
+ mockMetric->setMaxNumberOfMeasurements(5);
+ for (int counter = 0; counter < 10; ++counter)
+ {
+ mockMetric->update(static_cast<double>(counter));
+ }
+ auto samples = mockMetric->getMeasurementValues();
+
+ EXPECT_EQ(5, mockMetric->getCurrentNumberOfMeasurements());
+ EXPECT_EQ(5, samples.size());
+ EXPECT_EQ(45.0, mockMetric->getElapsedTime());
+
+ // When we check the results, our deque holds 5 places. We skip the
+ // first 5 because they already rolled off the end. The metric value
+ // starts at one and accumulates. At position 5, we should already have
+ // a value of 15 accumulated.
+ int counter = 5;
+ int accumulator = 15;
+
+ for (auto sampleIterator = samples.begin();
+ sampleIterator != samples.end();
+ ++sampleIterator)
+ {
+ EXPECT_EQ(static_cast<double>(accumulator), sampleIterator->first);
+ EXPECT_EQ(static_cast<double>(counter + 2), sampleIterator->second);
+ ++counter;
+ accumulator += counter;
+ }
+}
+
+TEST(SamplingMetricBaseTest, UnableToPerformMeasurementsTests)
+{
+ std::shared_ptr<MockSamplingMetric> mockMetric(std::make_shared<MockSamplingMetric>("Test Metric", false, 1.0));
+ double accumulatedTime = 0.0;
+ // Make 10 updates
+ mockMetric->setMaxNumberOfMeasurements(5);
+
+ for (int counter = 0; counter < 10; ++counter)
+ {
+ mockMetric->update(static_cast<double>(counter));
+ accumulatedTime += counter;
+ }
+ auto samples = mockMetric->getMeasurementValues();
+
+ EXPECT_EQ(0, mockMetric->getCurrentNumberOfMeasurements());
+ EXPECT_EQ(0, samples.size());
+ EXPECT_EQ(accumulatedTime, mockMetric->getElapsedTime());
+}
diff --git a/SurgSim/Framework/UnitTests/SceneElementTest.cpp b/SurgSim/Framework/UnitTests/SceneElementTest.cpp
index 89586b2..01fedab 100644
--- a/SurgSim/Framework/UnitTests/SceneElementTest.cpp
+++ b/SurgSim/Framework/UnitTests/SceneElementTest.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -13,14 +13,20 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include <boost/any.hpp>
#include <gtest/gtest.h>
#include "SurgSim/Framework/BasicSceneElement.h"
#include "SurgSim/Framework/PoseComponent.h"
+#include "SurgSim/Framework/Scene.h"
#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Testing/Utilities.h"
#include "MockObjects.h" //NOLINT
@@ -63,18 +69,69 @@ TEST(SceneElementTest, UpdateFunctions)
EXPECT_TRUE(element.didFixedUpdate);
}
-TEST(SceneElementTest, AddAndTestComponents)
+TEST(SceneElementTest, AddAndTestComponentsAssembly)
{
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ std::shared_ptr<MockManager> manager = std::make_shared<MockManager>();
std::shared_ptr<MockSceneElement> element = std::make_shared<MockSceneElement>();
std::shared_ptr<MockComponent> component = std::make_shared<MockComponent>("TestComponent");
+ runtime->addManager(manager);
EXPECT_TRUE(element->addComponent(component));
- // SceneElement should be set after add
- EXPECT_EQ(component->getSceneElement(), element);
+ // SceneElement in Component should be set immediately
+ EXPECT_EQ(nullptr, component->getScene());
+ EXPECT_NE(nullptr, component->getSceneElement());
+
+ // Scene and SceneElement should be set after add
+ runtime->getScene()->addSceneElement(element);
+ EXPECT_EQ(element->getScene(), component->getScene());
+ EXPECT_EQ(element, component->getSceneElement());
+
+ // Verify the component made it to the manager
+ runtime->start(true);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ ASSERT_EQ(1, manager->getComponents().size());
+ EXPECT_EQ(component, manager->getComponents()[0]);
+ runtime->stop();
+}
+
+class ConstructorSceneElement : public MockSceneElement
+{
+public:
+ ConstructorSceneElement()
+ {
+ component = std::make_shared<MockComponent>("TestComponent");
+ addComponent(component);
+ }
- // Scene in Component will not be set until initialization.
- EXPECT_NE(component->getScene(), element->getScene() );
+ std::shared_ptr<MockComponent> component;
+};
+
+TEST(SceneElementTest, AddAndTestComponentsConstructor)
+{
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ std::shared_ptr<MockManager> manager = std::make_shared<MockManager>();
+ std::shared_ptr<ConstructorSceneElement> element = std::make_shared<ConstructorSceneElement>();
+ auto component = element->component;
+ runtime->addManager(manager);
+
+
+ // Scene and SceneElement in Component will not be set until initialization (add to scene).
+ EXPECT_EQ(nullptr, component->getScene());
+ EXPECT_EQ(nullptr, component->getSceneElement());
+
+ // Scene and SceneElement should be set after add
+ runtime->getScene()->addSceneElement(element);
+ EXPECT_EQ(element->getScene(), component->getScene());
+ EXPECT_EQ(element, component->getSceneElement());
+
+ // Verify the component made it to the manager
+ runtime->start(true);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ ASSERT_EQ(1, manager->getComponents().size());
+ EXPECT_EQ(component, manager->getComponents()[0]);
+ runtime->stop();
}
TEST(SceneElementTest, AddAndAccessComponents)
@@ -101,8 +158,10 @@ TEST(SceneElementTest, AddAndAccessComponents)
EXPECT_EQ(nullptr, fetched);
}
-TEST(SceneElementTest, RemoveComponents)
+TEST(SceneElementTest, RemoveComponent)
{
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ std::shared_ptr<MockManager> manager = std::make_shared<MockManager>();
std::shared_ptr<MockSceneElement> element(new MockSceneElement());
std::shared_ptr<MockComponent> component1(new MockComponent("TestComponent1"));
@@ -111,14 +170,51 @@ TEST(SceneElementTest, RemoveComponents)
EXPECT_TRUE(element->addComponent(component1));
EXPECT_TRUE(element->addComponent(component2));
+ runtime->addManager(manager);
+ runtime->getScene()->addSceneElement(element);
+ runtime->start(true);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ EXPECT_EQ(2, manager->getComponents().size());
+
EXPECT_TRUE(element->removeComponent("TestComponent2"));
EXPECT_EQ(nullptr, element->getComponent("TestComponent2"));
-
- // Add back should work
- EXPECT_TRUE(element->addComponent(component2));
+ runtime->step();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ EXPECT_EQ(1, manager->getComponents().size());
EXPECT_TRUE(element->removeComponent(component1));
EXPECT_EQ(nullptr, element->getComponent("TestComponent1"));
+ runtime->step();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ EXPECT_EQ(0, manager->getComponents().size());
+
+ runtime->stop();
+}
+
+TEST(SceneElementTest, RemoveComponents)
+{
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ std::shared_ptr<MockManager> manager = std::make_shared<MockManager>();
+ 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));
+
+ runtime->addManager(manager);
+ runtime->getScene()->addSceneElement(element);
+ runtime->start(true);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ EXPECT_EQ(2, manager->getComponents().size());
+
+ element->removeComponents();
+ runtime->step();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(150));
+ EXPECT_EQ(0, manager->getComponents().size());
+
+ runtime->stop();
}
TEST(SceneElementTest, GetComponentsTest)
@@ -165,6 +261,37 @@ TEST(SceneElementTest, GetTypedComponentsTests)
EXPECT_EQ(0u, element->getComponents<MockComponent>().size());
}
+TEST(SceneElementTest, GetSetValue)
+{
+ std::shared_ptr<MockSceneElement> element(new MockSceneElement());
+ std::shared_ptr<MockComponent> component(new MockComponent("Component"));
+ element->addComponent(component);
+
+ {
+ float value;
+ EXPECT_FALSE(element->getValue("Component", "MissingProperty", &value));
+ EXPECT_THROW(element->getValue("Component", "MissingProperty"), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(element->getValue<bool>("Component", "MissingProperty"), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(element->setValue("Component", "MissingProperty", value), SurgSim::Framework::AssertionFailure);
+ }
+ {
+ bool value = false;
+ component->succeedWithInit = true;
+ EXPECT_TRUE(element->getValue("Component", "SucceedWithInit", &value));
+ EXPECT_TRUE(value);
+ EXPECT_NO_THROW(element->getValue("Component", "SucceedWithInit"));
+ EXPECT_NO_THROW(element->getValue<bool>("Component", "SucceedWithInit"));
+ EXPECT_TRUE(boost::any_cast<bool>(element->getValue("Component", "SucceedWithInit")));
+ EXPECT_TRUE(element->getValue<bool>("Component", "SucceedWithInit"));
+ }
+ {
+ bool value = true;
+ component->succeedWithWakeUp = false;
+ EXPECT_NO_THROW(element->setValue("Component", "SucceedWithWakeUp", value));
+ EXPECT_TRUE(component->succeedWithWakeUp);
+ }
+}
+
TEST(SceneElementTest, InitComponentTest)
{
std::shared_ptr<MockSceneElement> element(new MockSceneElement());
@@ -190,3 +317,96 @@ TEST(SceneElementTest, DoubleInitTest)
ASSERT_ANY_THROW(element->initialize());
}
+
+TEST(SceneElement, InGroup)
+{
+ std::shared_ptr<MockSceneElement> element(new MockSceneElement());
+
+ EXPECT_FALSE(element->inGroup("xxx"));
+ element->addToGroup("One");
+ EXPECT_TRUE(element->inGroup("One"));
+ EXPECT_FALSE(element->inGroup("xxx"));
+}
+
+TEST(SceneElementTest, NoSceneGroupsTest)
+{
+ std::shared_ptr<MockSceneElement> element(new MockSceneElement());
+ EXPECT_TRUE(element->getGroups().empty());
+
+ element->addToGroup("One");
+ EXPECT_TRUE(element->inGroup("One"));
+
+ element->addToGroup("Two");
+ EXPECT_TRUE(element->inGroup("One"));
+ EXPECT_TRUE(element->inGroup("Two"));
+
+ element->removeFromGroup("One");
+ EXPECT_TRUE(element->inGroup("Two"));
+ EXPECT_FALSE(element->inGroup("One"));
+
+ std::vector<std::string> newGroups;
+ newGroups.push_back("Three");
+ newGroups.push_back("Four");
+
+ element->setGroups(newGroups);
+ EXPECT_FALSE(element->inGroup("One"));
+ EXPECT_FALSE(element->inGroup("Two"));
+ EXPECT_TRUE(element->inGroup("Three"));
+ EXPECT_TRUE(element->inGroup("Four"));
+
+ std::vector<std::string> empty;
+ element->setGroups(empty);
+ EXPECT_FALSE(element->inGroup("One"));
+ EXPECT_FALSE(element->inGroup("Two"));
+ EXPECT_FALSE(element->inGroup("Three"));
+ EXPECT_FALSE(element->inGroup("Four"));
+
+}
+
+TEST(SceneElementTest, Include)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+
+ std::string file;
+
+ ASSERT_TRUE(runtime->getApplicationData()->tryFindFile("SceneElementTest/includer.yaml", &file));
+
+ YAML::Node node = YAML::LoadFile(file);
+ ASSERT_TRUE(node.IsSequence());
+
+ auto elements = node.as<std::vector<std::shared_ptr<SurgSim::Framework::SceneElement>>>();
+
+ EXPECT_EQ(4u, elements.size());
+}
+
+
+TEST(SceneElementTest, IncludeCircle)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+
+ std::string file;
+
+ ASSERT_TRUE(runtime->getApplicationData()->tryFindFile("SceneElementTest/circle-1.yaml", &file));
+
+ YAML::Node node = YAML::LoadFile(file);
+ ASSERT_TRUE(node.IsSequence());
+
+ ASSERT_THROW(auto elements = node.as<std::vector<std::shared_ptr<SurgSim::Framework::SceneElement>>>(),
+ SurgSim::Framework::AssertionFailure);
+}
+
+TEST(SceneElementTest, SingleAsArray)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+
+ std::string file;
+
+ ASSERT_TRUE(runtime->getApplicationData()->tryFindFile("SceneElementTest/single.yaml", &file));
+
+ YAML::Node node = YAML::LoadFile(file);
+ ASSERT_FALSE(node.IsSequence());
+
+ auto elements = node.as<std::vector<std::shared_ptr<SurgSim::Framework::SceneElement>>>();
+
+ EXPECT_EQ(1u, elements.size());
+}
diff --git a/SurgSim/Framework/UnitTests/SceneTest.cpp b/SurgSim/Framework/UnitTests/SceneTest.cpp
index d331240..a2a07ce 100644
--- a/SurgSim/Framework/UnitTests/SceneTest.cpp
+++ b/SurgSim/Framework/UnitTests/SceneTest.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -14,12 +14,15 @@
// 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"
+#include "SurgSim/Framework/UnitTests/MockObjects.h"
+#include "SurgSim/Testing/Utilities.h"
+
namespace SurgSim
{
@@ -67,6 +70,45 @@ TEST(SceneTest, AddAndTestScene)
EXPECT_EQ(scene, element->getScene());
}
+TEST(SceneTest, TryFind)
+{
+ 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);
+
+ std::shared_ptr<Component> result;
+
+ EXPECT_EQ(component0, scene->getComponent("element0", "component0"));
+ EXPECT_EQ(component1, scene->getComponent("element1", "component1"));
+
+ EXPECT_EQ(nullptr, scene->getComponent("element1", "xxx"));
+ EXPECT_EQ(nullptr, scene->getComponent("xxx", "component0"));
+}
+
+TEST(SceneTest, Removal)
+{
+ auto runtime(std::make_shared<Runtime>());
+ std::shared_ptr<Scene> scene = runtime->getScene();
+
+ scene->addSceneElement(std::make_shared<MockSceneElement>("element1"));
+ scene->addSceneElement(std::make_shared<MockSceneElement>("element2"));
+ scene->addSceneElement(std::make_shared<MockSceneElement>("element3"));
+
+ EXPECT_EQ(3L, scene->getSceneElements().size());
+ EXPECT_NO_THROW(scene->removeSceneElement(scene->getSceneElement("element1")));
+ EXPECT_EQ(2L, scene->getSceneElements().size());
+}
+
TEST(SceneTest, CheckForExpiredRuntime)
{
auto runtime = std::make_shared<Runtime>();
@@ -120,5 +162,75 @@ TEST(SceneTest, YamlTest)
}
+TEST(SceneTest, LoadSceneTest)
+{
+ auto runtime = std::make_shared<Runtime>("config.txt");
+
+ ASSERT_NO_THROW(runtime->loadScene("SceneTestData/scene.yaml"));
+
+ auto scene = runtime->getScene();
+ auto element = scene->getSceneElement("element0");
+ auto component = element->getComponent("component0");
+ component->setLocalActive(false);
+
+ // If we get stale components this would assert as the components would be initialized alread
+ ASSERT_NO_THROW(runtime->loadScene("SceneTestData/scene.yaml"));
+
+ scene = runtime->getScene();
+ element = scene->getSceneElement("element0");
+ component = element->getComponent("component0");
+
+ // Another check for fresh components, if they are from the cache, this would be false
+ EXPECT_TRUE(component->isLocalActive());
+}
+
+TEST(SceneTest, SceneElementGroups)
+{
+
+ using SurgSim::Testing::doesContain;
+
+ auto runtime = std::make_shared<Runtime>("config.txt");
+ auto scene = runtime->getScene();
+
+ auto element1 = std::make_shared<BasicSceneElement>("element1");
+ auto element2 = std::make_shared<BasicSceneElement>("element2");
+
+ element1->addToGroup("One");
+
+ auto& groups = scene->getGroups();
+
+ EXPECT_TRUE(groups.getGroups().empty());
+
+ scene->addSceneElement(element1);
+
+ EXPECT_EQ(1L, groups.getGroups().size());
+ EXPECT_TRUE(doesContain(groups.getGroups(), "One"));
+ EXPECT_TRUE(doesContain(groups.getMembers("One"), element1));
+
+ scene->addSceneElement(element2);
+ EXPECT_EQ(1L, groups.getGroups().size());
+
+ element2->addToGroup("Two");
+ EXPECT_EQ(2L, groups.getGroups().size());
+ EXPECT_TRUE(doesContain(groups.getGroups(), "Two"));
+ EXPECT_TRUE(doesContain(groups.getMembers("Two"), element2));
+
+ element1->removeFromGroup("One");
+ EXPECT_EQ(1L, groups.getGroups().size());
+ EXPECT_TRUE(doesContain(groups.getGroups(), "Two"));
+ EXPECT_TRUE(doesContain(groups.getMembers("Two"), element2));
+
+ element1->addToGroup("One");
+
+
+ std::vector<std::string> groupNames;
+ groupNames.push_back("Three");
+ groupNames.push_back("Four");
+
+ element1->setGroups(groupNames);
+ EXPECT_EQ(4L, groups.getGroups().size());
+
+}
+
}
}
diff --git a/SurgSim/Framework/UnitTests/ThreadPoolTest.cpp b/SurgSim/Framework/UnitTests/ThreadPoolTest.cpp
new file mode 100644
index 0000000..5ea0f0d
--- /dev/null
+++ b/SurgSim/Framework/UnitTests/ThreadPoolTest.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/Framework/ThreadPool.h"
+
+#include <gtest/gtest.h>
+
+namespace
+{
+
+double f1() { return 1.0; }
+int f2(int val) { return val; }
+
+};
+
+namespace SurgSim
+{
+namespace Framework
+{
+
+TEST(ThreadPoolTest, CanConstruct)
+{
+ EXPECT_NO_THROW({ThreadPool pool;});
+ EXPECT_NO_THROW({ThreadPool pool(2);});
+}
+
+TEST(ThreadPoolTest, ExampleUsage)
+{
+ ThreadPool pool;
+
+ // Add a task
+ std::future<double> result1 = pool.enqueue<double>(f1);
+
+ // Add a task using std::bind
+ std::future<int> result2 = pool.enqueue<int>(std::bind(f2, 2));
+
+ // Add a task using a lambda function
+ std::future<std::string> result3 = pool.enqueue<std::string>([]() {return "string"; });
+
+ EXPECT_EQ(1.0, result1.get());
+ EXPECT_EQ(2, result2.get());
+ EXPECT_EQ("string", result3.get());
+}
+
+TEST(ThreadPoolTest, AddLotsOfTasks)
+{
+ ThreadPool pool(2);
+
+ std::vector<std::future<int>> futures;
+ int expectedTotal = 0;
+ for (int i = 0; i < 100; i++)
+ {
+ futures.push_back(pool.enqueue<int>(std::bind(f2, i)));
+ expectedTotal += i;
+ }
+
+ int total = 0;
+ for (auto& future : futures)
+ {
+ total += future.get();
+ }
+ EXPECT_EQ(expectedTotal, total);
+}
+
+};
+};
diff --git a/SurgSim/Framework/UnitTests/TimerTest.cpp b/SurgSim/Framework/UnitTests/TimerTest.cpp
index 35866fe..9a9e78a 100644
--- a/SurgSim/Framework/UnitTests/TimerTest.cpp
+++ b/SurgSim/Framework/UnitTests/TimerTest.cpp
@@ -29,7 +29,8 @@ 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_NO_THROW(timer->getCumulativeTime());
+ EXPECT_NEAR(0.0, timer->getCumulativeTime(), 1e-9);
EXPECT_THROW(timer->getAverageFramePeriod(), SurgSim::Framework::AssertionFailure);
EXPECT_THROW(timer->getAverageFrameRate(), SurgSim::Framework::AssertionFailure);
EXPECT_THROW(timer->getLastFramePeriod(), SurgSim::Framework::AssertionFailure);
@@ -46,6 +47,7 @@ TEST(TimerTest, SettingFrames)
EXPECT_EQ(timer->getLastFramePeriod(), timer->getCumulativeTime());
timer->setMaxNumberOfFrames(3);
+ EXPECT_EQ(3, timer->getMaxNumberOfFrames());
timer->start();
for (auto i = 0; i < 5; ++i)
{
@@ -54,6 +56,7 @@ TEST(TimerTest, SettingFrames)
EXPECT_EQ(timer->getCurrentNumberOfFrames(), 3);
timer->setMaxNumberOfFrames(2);
EXPECT_EQ(timer->getCurrentNumberOfFrames(), 2);
+ EXPECT_TRUE(timer->isBufferFull());
}
TEST(TimerTest, Comparison)
@@ -83,13 +86,12 @@ TEST(TimerTest, GetWithoutAnyFrames)
{
std::shared_ptr<Timer> timer = std::make_shared<Timer>();
- EXPECT_ANY_THROW(timer->getCumulativeTime());
+ EXPECT_NO_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());
+ EXPECT_FALSE(timer->isBufferFull());
}
-
-
diff --git a/SurgSim/Framework/UnitTests/TransferPropertiesBehaviorTests.cpp b/SurgSim/Framework/UnitTests/TransferPropertiesBehaviorTests.cpp
index c452db7..25aa5ad 100644
--- a/SurgSim/Framework/UnitTests/TransferPropertiesBehaviorTests.cpp
+++ b/SurgSim/Framework/UnitTests/TransferPropertiesBehaviorTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -15,14 +15,16 @@
#include <gtest/gtest.h>
-#include "SurgSim/Framework/TransferPropertiesBehavior.h"
#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/TransferPropertiesBehavior.h"
+namespace {
+
class A : public SurgSim::Framework::Accessible
{
public:
- A(int initialA = 0, int initialB = 0) : a(initialA), b(initialB)
+ explicit 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);
@@ -37,6 +39,8 @@ public:
void setB(int val) { b = val; }
};
+};
+
namespace SurgSim
{
namespace Framework
@@ -47,6 +51,26 @@ TEST(TransferPropertiesBehaviorTest, InitTest)
ASSERT_NO_THROW({TransferPropertiesBehavior b("TestName");});
}
+TEST(TransferPropertiesBehaviorTest, GetSetTargetManagerType)
+{
+ auto behavior = std::make_shared<TransferPropertiesBehavior>("test");
+ auto runtime = std::make_shared<Framework::Runtime>();
+
+ EXPECT_EQ(Framework::MANAGER_TYPE_BEHAVIOR, behavior->getTargetManagerType());
+
+ EXPECT_THROW(behavior->setTargetManagerType(-1), Framework::AssertionFailure);
+ EXPECT_THROW(behavior->setTargetManagerType(Framework::MANAGER_TYPE_COUNT), Framework::AssertionFailure);
+
+ EXPECT_NO_THROW(behavior->setTargetManagerType(Framework::MANAGER_TYPE_PHYSICS));
+ EXPECT_EQ(Framework::MANAGER_TYPE_PHYSICS, behavior->getTargetManagerType());
+
+ EXPECT_NO_THROW(behavior->setTargetManagerType(Framework::MANAGER_TYPE_BEHAVIOR));
+ EXPECT_EQ(Framework::MANAGER_TYPE_BEHAVIOR, behavior->getTargetManagerType());
+
+ behavior->initialize(runtime);
+ EXPECT_THROW(behavior->setTargetManagerType(Framework::MANAGER_TYPE_GRAPHICS), Framework::AssertionFailure);
+}
+
TEST(TransferPropertiesBehaviorTest, ValidConnections)
{
auto a = std::make_shared<A>();
diff --git a/SurgSim/Graphics/AxesRepresentation.h b/SurgSim/Graphics/AxesRepresentation.h
index a235f89..5231b83 100644
--- a/SurgSim/Graphics/AxesRepresentation.h
+++ b/SurgSim/Graphics/AxesRepresentation.h
@@ -32,7 +32,7 @@ public:
/// Constructor
explicit AxesRepresentation(const std::string& name) : Representation(name)
{
-
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(AxesRepresentation, double, Size, getSize, setSize);
};
/// Sets the size of the shown axes.
diff --git a/SurgSim/Graphics/CMakeLists.txt b/SurgSim/Graphics/CMakeLists.txt
index 19e0e5e..cdc2d62 100644
--- a/SurgSim/Graphics/CMakeLists.txt
+++ b/SurgSim/Graphics/CMakeLists.txt
@@ -14,37 +14,41 @@
# limitations under the License.
include_directories(
- ${OSG_INCLUDE_DIR}
${OPENTHREADS_INCLUDE_DIR}
)
set(SURGSIM_GRAPHICS_SOURCES
Camera.cpp
+ CurveRepresentation.cpp
Group.cpp
Manager.cpp
Mesh.cpp
MeshPlyReaderDelegate.cpp
- MeshUtilities.cpp
OsgAxesRepresentation.cpp
OsgBoxRepresentation.cpp
OsgCamera.cpp
OsgCapsuleRepresentation.cpp
+ OsgCurveRepresentation.cpp
OsgCylinderRepresentation.cpp
+ OsgFont.cpp
OsgGroup.cpp
OsgLight.cpp
OsgLog.cpp
OsgManager.cpp
OsgMaterial.cpp
OsgMeshRepresentation.cpp
+ OsgModel.cpp
OsgOctreeRepresentation.cpp
OsgPlaneRepresentation.cpp
OsgPointCloudRepresentation.cpp
+ OsgProgram.cpp
OsgRepresentation.cpp
OsgSceneryRepresentation.cpp
OsgScreenSpacePass.cpp
OsgScreenSpaceQuadRepresentation.cpp
- OsgShader.cpp
+ OsgSkeletonRepresentation.cpp
OsgSphereRepresentation.cpp
+ OsgTextRepresentation.cpp
OsgTexture.cpp
OsgTexture1d.cpp
OsgTexture2d.cpp
@@ -58,9 +62,14 @@ set(SURGSIM_GRAPHICS_SOURCES
OsgVectorFieldRepresentation.cpp
OsgView.cpp
OsgViewElement.cpp
+ PaintBehavior.cpp
PointCloudRepresentation.cpp
RenderPass.cpp
Representation.cpp
+ SceneryRepresentation.cpp
+ TangentSpaceGenerator.cpp
+ TextRepresentation.cpp
+ Texture.cpp
TriangleNormalGenerator.cpp
View.cpp
ViewElement.cpp
@@ -71,7 +80,9 @@ set(SURGSIM_GRAPHICS_HEADERS
BoxRepresentation.h
Camera.h
CapsuleRepresentation.h
+ CurveRepresentation.h
CylinderRepresentation.h
+ Font.h
Group.h
Light.h
Manager.h
@@ -80,14 +91,16 @@ set(SURGSIM_GRAPHICS_HEADERS
Mesh-inl.h
MeshPlyReaderDelegate.h
MeshRepresentation.h
- MeshUtilities.h
+ Model.h
OctreeRepresentation.h
OsgAxesRepresentation.h
OsgBoxRepresentation.h
OsgCamera.h
OsgCapsuleRepresentation.h
OsgConversions.h
+ OsgCurveRepresentation.h
OsgCylinderRepresentation.h
+ OsgFont.h
OsgGroup.h
OsgLight.h
OsgLog.h
@@ -95,10 +108,12 @@ set(SURGSIM_GRAPHICS_HEADERS
OsgMaterial.h
OsgMatrixConversions.h
OsgMeshRepresentation.h
+ OsgModel.h
OsgOctreeRepresentation.h
OsgPlane.h
OsgPlaneRepresentation.h
OsgPointCloudRepresentation.h
+ OsgProgram.h
OsgQuaternionConversions.h
OsgRenderTarget.h
OsgRenderTarget-inl.h
@@ -107,8 +122,9 @@ set(SURGSIM_GRAPHICS_HEADERS
OsgSceneryRepresentation.h
OsgScreenSpacePass.h
OsgScreenSpaceQuadRepresentation.h
- OsgShader.h
+ OsgSkeletonRepresentation.h
OsgSphereRepresentation.h
+ OsgTextRepresentation.h
OsgTexture.h
OsgTexture1d.h
OsgTexture2d.h
@@ -131,15 +147,19 @@ set(SURGSIM_GRAPHICS_HEADERS
OsgVectorFieldRepresentation.h
OsgView.h
OsgViewElement.h
+ PaintBehavior.h
PlaneRepresentation.h
PointCloudRepresentation.h
+ Program.h
RenderPass.h
RenderTarget.h
Representation.h
SceneryRepresentation.h
ScreenSpaceQuadRepresentation.h
- Shader.h
+ SkeletonRepresentation.h
SphereRepresentation.h
+ TangentSpaceGenerator.h
+ TextRepresentation.h
Texture.h
Texture1d.h
Texture2d.h
@@ -154,12 +174,12 @@ set(SURGSIM_GRAPHICS_HEADERS
View.h
ViewElement.h
)
+surgsim_create_library_header(Graphics.h "${SURGSIM_GRAPHICS_HEADERS}")
surgsim_add_library(
SurgSimGraphics
"${SURGSIM_GRAPHICS_SOURCES}"
"${SURGSIM_GRAPHICS_HEADERS}"
- "SurgSim/Graphics"
)
SET(LIBS
diff --git a/SurgSim/Graphics/Camera.cpp b/SurgSim/Graphics/Camera.cpp
index 27ed882..bea4a2e 100644
--- a/SurgSim/Graphics/Camera.cpp
+++ b/SurgSim/Graphics/Camera.cpp
@@ -13,7 +13,9 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
#include "SurgSim/Graphics/Camera.h"
+#include "SurgSim/Graphics/Group.h"
#include "SurgSim/Math/MathConvert.h"
namespace SurgSim
@@ -23,54 +25,158 @@ 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, std::vector<std::string>, RenderGroupReferences,
+ getRenderGroupReferences, setRenderGroupReferences);
+ SURGSIM_ADD_SETTER(Camera, std::string, RenderGroupReference, setRenderGroupReference);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(Camera, SurgSim::Math::Vector4d, AmbientColor,
getAmbientColor, setAmbientColor);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Camera, bool, MainCamera, isMainCamera, setMainCamera);
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);
+ SURGSIM_ADD_RO_PROPERTY(Camera, SurgSim::Math::Matrix44f, FloatInverseProjectionMatrix, getInverseProjectionMatrix);
+
+ {
+ typedef std::array<double, 2> ParamType;
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Camera, ParamType, ViewportSize,
+ getViewportSize, setViewportSize);
+ }
+
+ {
+ typedef std::array<double, 4> ParamType;
+ SURGSIM_ADD_SETTER(Camera, ParamType, PerspectiveProjection, setPerspectiveProjection);
+ }
+ {
+ typedef std::array<double, 6> ParamType;
+ SURGSIM_ADD_SETTER(Camera, ParamType, OrthogonalProjection, setOrthogonalProjection);
+ }
+ {
+ typedef std::array<int, 4> ParamType;
+
+ // Deal with the overloaded function, by casting to explicit function type
+ auto getter = (ParamType(Camera::*)(void) const)&Camera::getViewport;
+ auto setter = (void(Camera::*)(ParamType))&Camera::setViewport;
+
+ setAccessors("Viewport", std::bind(getter, this),
+ std::bind(setter, this, std::bind(SurgSim::Framework::convert<ParamType>, std::placeholders::_1)));
+
+ setSerializable("Viewport",
+ std::bind(&YAML::convert<ParamType>::encode, std::bind(getter, this)),
+ std::bind(setter, this, std::bind(&YAML::Node::as<ParamType>, std::placeholders::_1)));
+ }
}
void Camera::setRenderGroupReference(const std::string& name)
{
- removeGroupReference(name);
- m_renderGroupReference = name;
+ std::vector<std::string> references(1, name);
+ setRenderGroupReferences(references);
}
-std::string Camera::getRenderGroupReference() const
+void Camera::setRenderGroupReferences(const std::vector<std::string>& names)
{
- return m_renderGroupReference;
+ m_renderGroupReferences.clear();
+ for (const auto& name : names)
+ {
+ removeGroupReference(name);
+ addRenderGroupReference(name);
+ }
+}
+
+std::vector<std::string> Camera::getRenderGroupReferences() const
+{
+ return m_renderGroupReferences;
+}
+
+void Camera::addRenderGroupReference(const std::string& name)
+{
+ if (std::find(m_renderGroupReferences.begin(), m_renderGroupReferences.end(),
+ name) == m_renderGroupReferences.end())
+ {
+ m_renderGroupReferences.push_back(name);
+ }
}
bool Camera::setRenderGroup(std::shared_ptr<Group> group)
{
- m_group = group;
- return true;
+ std::vector<std::shared_ptr<Group>> groups(1, group);
+ return setRenderGroups(groups);
}
-std::shared_ptr<Group> Camera::getRenderGroup() const
+bool Camera::setRenderGroups(const std::vector<std::shared_ptr<Group>>& groups)
{
- return m_group;
+ bool result = false;
+ m_renderGroups.clear();
+ for (const auto& group : groups)
+ {
+ result = addRenderGroup(group);
+ if (!result)
+ {
+ break;
+ }
+ }
+ return result;
+}
+
+bool Camera::addRenderGroup(std::shared_ptr<Group> group)
+{
+ if (std::find(m_renderGroups.begin(), m_renderGroups.end(), group) == m_renderGroups.end())
+ {
+ m_renderGroups.push_back(group);
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+std::vector<std::shared_ptr<Group>> Camera::getRenderGroups() const
+{
+ return m_renderGroups;
}
bool Camera::addGroupReference(const std::string& name)
{
bool result = false;
- if (name != m_renderGroupReference)
+ if (std::find(m_renderGroupReferences.begin(), m_renderGroupReferences.end(),
+ name) == m_renderGroupReferences.end())
{
result = Representation::addGroupReference(name);
}
return result;
}
+void Camera::setPerspectiveProjection(const std::array<double, 4>& val)
+{
+ setPerspectiveProjection(val[0], val[1], val[2], val[3]);
+}
+
+void Camera::setOrthogonalProjection(const std::array<double, 6>& val)
+{
+
+ setOrthogonalProjection(val[0], val[1], val[2], val[3], val[4], val[5]);
+}
+
+void Camera::setViewport(std::array<int, 4> val)
+{
+ setViewport(val[0], val[1], val[2], val[3]);
+}
+
+std::array<int, 4> Camera::getViewport() const
+{
+ std::array<int, 4> result;
+ getViewport(&result[0], &result[1], &result[2], &result[3]);
+ return result;
+}
+
bool Camera::doInitialize()
{
- SURGSIM_ASSERT(m_renderGroupReference != "")
+ SURGSIM_ASSERT(!m_renderGroupReferences.empty())
<< "Can't have a camera without a RenderGroupReference.";
return true;
}
diff --git a/SurgSim/Graphics/Camera.h b/SurgSim/Graphics/Camera.h
index 39758ab..b8da761 100644
--- a/SurgSim/Graphics/Camera.h
+++ b/SurgSim/Graphics/Camera.h
@@ -21,6 +21,8 @@
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Math/Vector.h"
+#include <array>
+
namespace SurgSim
{
@@ -35,7 +37,7 @@ class RenderTarget;
///
/// 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).
+/// To disable a camera: setLocalActive(false). To re-enable, setLocalActive(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.
@@ -67,9 +69,17 @@ public:
/// \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;
+ /// Set the collections of group references that this camera wants to use for rendering.
+ /// \param names Vector of names of the groups used for rendering
+ void setRenderGroupReferences(const std::vector<std::string>& names);
+
+ /// Gets the collection of names of render groups used for rendering
+ /// \return Vector of names of the groups used for rendering
+ std::vector<std::string> getRenderGroupReferences() const;
+
+ /// Adds a single group reference to the collection of group references for rendering
+ /// \param name Name of the group to be used for rendering
+ void addRenderGroupReference(const std::string& name);
/// 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.
@@ -78,10 +88,19 @@ public:
/// \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.
+ /// Sets the representation groups that will be seen by this camera.
+ /// \param groups Vector of groups of representations
+ /// \return True if it succeeded, false if it failed
+ virtual bool setRenderGroups(const std::vector<std::shared_ptr<Group>>& groups);
+
+ /// Add a group of representations that will seen by this camera.
+ /// \param group Group of representations
+ virtual bool addRenderGroup(std::shared_ptr<Group> group);
+
+ /// Gets all groups 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;
+ /// \return Vector of groups of representations
+ std::vector<std::shared_ptr<Group>> getRenderGroups() const;
/// Gets the view matrix of the camera
/// \return View matrix
@@ -95,10 +114,45 @@ public:
/// \param matrix Projection matrix
virtual void setProjectionMatrix(const SurgSim::Math::Matrix44d& matrix) = 0;
+ /// Sets the viewport size for this camera
+ /// \param x,y location of the viewport in screen space
+ /// \param width, height size of the viewport in screen space
+ virtual void setViewport(int x, int y, int width, int height) = 0;
+
+ /// collect the viewport values
+ /// \param x,y,width,height [out] non-nullptr parameters to write the viewport parameters
+ virtual void getViewport(int* x, int* y, int* width, int* height) const = 0;
+
+ /// Sets the width and height of the viewport
+ /// \param dimensions size of the viewport in screen space
+ virtual void setViewportSize(std::array<double, 2> dimensions) = 0;
+
+ /// Gets the dimensions of the viewport
+ virtual std::array<double, 2> getViewportSize() const = 0;
+
+ /// Set the projection matrix with the appropriate perspective projection parameters
+ /// \param fovy Field of view along the y-axis
+ /// \param aspect Aspect ration between y and x axis in the viewport
+ /// \param near, far near and far clipping planes
+ virtual void setPerspectiveProjection(double fovy, double aspect, double near, double far) = 0;
+
+ /// Set the projection matrix with the appropriate orthogonal projection parameters
+ /// \param left, right left and right bounds of the view volume
+ /// \param bottom, top bottom and top bounds of the view volume
+ /// \param near, far near and far bounds of the view volume
+ virtual void setOrthogonalProjection(
+ double left, double right,
+ double bottom, double top,
+ double near, double far) = 0;
+
/// Gets the projection matrix of the camera
/// \return Projection matrix
virtual const SurgSim::Math::Matrix44d& getProjectionMatrix() const = 0;
+ /// Gets the inverse projection matrix of the camera
+ /// \return Inverse Projection matrix
+ virtual SurgSim::Math::Matrix44d getInverseProjectionMatrix() 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;
@@ -115,7 +169,7 @@ public:
/// determined.
virtual void setRenderOrder(RenderOrder order, int value) = 0;
- virtual bool addGroupReference(const std::string& name) override;
+ 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
@@ -124,18 +178,43 @@ public:
/// \return the ambient light that gets added to the scene
virtual SurgSim::Math::Vector4d getAmbientColor() = 0;
+ /// Marks the camera as a main view camera, this means that view dependent passes should follow this camera with
+ /// their appropriate calculations, for this purpose when isMainCamera() is true, the camera provides a uniform
+ /// struct with it's transforms. This function will most likely be called by the view.
+ /// \code
+ /// struct MainCamera {
+ /// mat4 viewMatrix;
+ /// mat4 inverseViewMatrix;
+ /// mat4 projectionMatrix;
+ /// mat4 mainProjectionMatrix;
+ /// };
+ /// uniform MainCamera mainCamera;
+ /// \endcode
+ virtual void setMainCamera(bool val) = 0;
+
+ /// \return whether this is used as a main camera
+ virtual bool isMainCamera() = 0;
private:
- virtual bool doInitialize() override;
+ void setPerspectiveProjection(const std::array<double, 4>& val);
+
+ void setOrthogonalProjection(const std::array<double, 6>& val);
+
+ void setViewport(std::array<int, 4> val);
+
+ std::array<int, 4> getViewport() const;
+
+
+ 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;
+ std::vector<std::shared_ptr<Group>> m_renderGroups;
- /// The name of the group that the camera wants to use for rendering, the graphics manager will actually assign
+ /// The names of the groups that the camera wants to use for rendering, the graphics manager will actually assign
/// this group
- std::string m_renderGroupReference;
+ std::vector<std::string> m_renderGroupReferences;
};
}; // namespace Graphics
diff --git a/SurgSim/Graphics/CurveRepresentation.cpp b/SurgSim/Graphics/CurveRepresentation.cpp
new file mode 100644
index 0000000..9464054
--- /dev/null
+++ b/SurgSim/Graphics/CurveRepresentation.cpp
@@ -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.
+
+#include "SurgSim/Graphics/CurveRepresentation.h"
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Math/MathConvert.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+CurveRepresentation::CurveRepresentation(const std::string& name) : Representation(name)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CurveRepresentation, size_t, Subdivisions, getSubdivisions, setSubdivisions);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CurveRepresentation, double, Tension, getTension, setTension);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CurveRepresentation, Math::Vector4d, Color, getColor, setColor);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CurveRepresentation, double, Width, getWidth, setWidth);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CurveRepresentation, bool, AntiAliasing, isAntiAliasing, setAntiAliasing);
+
+ // Provide a common entry point accepting VerticesPlain under the label "Vertices"
+ // this can be used by behaviors to address this structure and the point cloud the same way
+ auto converter = std::bind(SurgSim::Framework::convert<DataStructures::VerticesPlain>, std::placeholders::_1);
+ auto functor = std::bind((void(CurveRepresentation::*)(const DataStructures::VerticesPlain&))
+ &CurveRepresentation::updateControlPoints, this, converter);
+
+ setSetter("Vertices", functor);
+}
+
+void CurveRepresentation::updateControlPoints(const DataStructures::VerticesPlain& vertices)
+{
+ SURGSIM_ASSERT(vertices.getNumVertices() > 1) << "Need at least 2 control points.";
+ m_locker.set(vertices);
+}
+
+void CurveRepresentation::updateControlPoints(DataStructures::VerticesPlain&& vertices)
+{
+ SURGSIM_ASSERT(vertices.getNumVertices() > 1) << "Need at least 2 control points.";
+ m_locker.set(std::move(vertices));
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Graphics/CurveRepresentation.h b/SurgSim/Graphics/CurveRepresentation.h
new file mode 100644
index 0000000..f0ca1a8
--- /dev/null
+++ b/SurgSim/Graphics/CurveRepresentation.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_CURVEREPRESENTATION_H
+#define SURGSIM_GRAPHICS_CURVEREPRESENTATION_H
+
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Framework/LockedContainer.h"
+#include "SurgSim/Graphics/Representation.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// This implements a graphical object to draw an interpolated curve, it accepts a series of control points, the
+/// number of segments in the curve will depend on the number of control points and the value returned from
+/// getSubdivisions().
+/// This class also provides the ad-hoc "Vertices" property, this means it can receive a
+/// \sa DataStructures::VerticesPlain structure as a property via setValue()
+class CurveRepresentation : public virtual Representation
+{
+public:
+ /// Constructor
+ /// \param name the name of the representation
+ explicit CurveRepresentation(const std::string& name);
+
+ /// Sets the number of intermediate points the get generated between each two control points
+ /// \param num number of interpolated points
+ virtual void setSubdivisions(size_t num) = 0;
+
+ /// \return the number of interpolated points between control points
+ virtual size_t getSubdivisions() const = 0;
+
+ /// Sets the tension (tau) parameter of the Catmull Rom interpolation, needs to be between 0.0 and 1.0
+ /// \param tension the tension
+ virtual void setTension(double tension) = 0;
+
+ /// \return the tension currently used
+ virtual double getTension() const = 0;
+
+ /// Sets the color for the curve
+ /// \param color the new color to be used
+ virtual void setColor(const SurgSim::Math::Vector4d& color) = 0;
+
+ /// \return the current color for this curve
+ virtual Math::Vector4d getColor() const = 0;
+
+ /// Sets the line width to be used for drawing this curve
+ /// \param width the new width to be used
+ virtual void setWidth(double width) = 0;
+
+ /// \return the current width
+ virtual double getWidth() const = 0;
+
+ /// Sets up whether to use anti aliasing on the curve or not
+ /// \param val if true anti aliasing will be turned on
+ virtual void setAntiAliasing(bool val) = 0;
+
+ /// \return whether the curve is currently being drawn with anti aliasing
+ virtual bool isAntiAliasing() const = 0;
+
+ /// Updates the control points for this class, this will cause a new curve to be generated on the next update
+ /// \note this method is threadsafe
+ /// \throws if the number of control points is < 2
+ /// \param vertices new vertices to be used as control points
+ void updateControlPoints(const DataStructures::VerticesPlain& vertices);
+
+ /// Updates the control points for this class, this will cause a new curve to be generated on the next update
+ /// move support.
+ /// \note this method is threadsafe
+ /// \throws if the number of control points is < 2
+ /// \param vertices new vertices to be used as control points
+ void updateControlPoints(DataStructures::VerticesPlain&& vertices);
+
+protected:
+
+ /// Container control points, threadsafe access when updating.
+ Framework::LockedContainer<DataStructures::VerticesPlain> m_locker;
+
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Graphics/Font.h b/SurgSim/Graphics/Font.h
new file mode 100644
index 0000000..06ae33d
--- /dev/null
+++ b/SurgSim/Graphics/Font.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_GRAPHICS_FONT_H
+#define SURGSIM_GRAPHICS_FONT_H
+
+#include "SurgSim/Framework/Asset.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+/// Abstract base class for the Font Asset, fonts are typefaces that can be used to render
+/// text on screen they would usually be loaded from disk
+class Font : public SurgSim::Framework::Asset
+{
+
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Graphics/Group.h b/SurgSim/Graphics/Group.h
index 7cceea0..b109e47 100644
--- a/SurgSim/Graphics/Group.h
+++ b/SurgSim/Graphics/Group.h
@@ -45,11 +45,11 @@ public:
virtual ~Group();
/// Sets whether the group is currently visible
- /// \param visible True for visible, false for invisible
+ /// \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
+ /// \return visible True for visible, false for invisible
virtual bool isVisible() const = 0;
/// Adds an representation
diff --git a/SurgSim/Graphics/Light.h b/SurgSim/Graphics/Light.h
index 2319783..03a244d 100644
--- a/SurgSim/Graphics/Light.h
+++ b/SurgSim/Graphics/Light.h
@@ -17,6 +17,7 @@
#define SURGSIM_GRAPHICS_LIGHT_H
#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Math/MathConvert.h"
#include "SurgSim/Math/Vector.h"
namespace SurgSim
@@ -51,6 +52,18 @@ public:
/// Constructor
explicit Light(const std::string& name) : Representation(name)
{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Light, SurgSim::Math::Vector4d, DiffuseColor,
+ getDiffuseColor, setDiffuseColor);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Light, SurgSim::Math::Vector4d, SpecularColor,
+ getSpecularColor, setSpecularColor);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Light, double, ConstantAttenuation,
+ getConstantAttenuation, setConstantAttenuation);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Light, double, LinearAttenuation,
+ getLinearAttenuation, setLinearAttenuation);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Light, double, QuadraticAttenuation,
+ getQuadraticAttenuation, setQuadraticAttenuation);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Light, std::string, LightGroupReference,
+ getLightGroupReference, setLightGroupReference);
}
virtual ~Light()
diff --git a/SurgSim/Graphics/Manager.cpp b/SurgSim/Graphics/Manager.cpp
index 96ffdfd..156cb69 100644
--- a/SurgSim/Graphics/Manager.cpp
+++ b/SurgSim/Graphics/Manager.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -30,6 +30,7 @@ using SurgSim::Graphics::View;
Manager::Manager() : ComponentManager("Graphics Manager")
{
+ setRate(60.0);
}
Manager::~Manager()
@@ -90,7 +91,12 @@ bool Manager::addRepresentation(std::shared_ptr<Representation> representation)
auto camera = std::dynamic_pointer_cast<Camera>(representation);
if (camera != nullptr)
{
- camera->setRenderGroup(getOrCreateGroup(camera->getRenderGroupReference()));
+ std::vector<std::shared_ptr<Group>> groups;
+ for (auto reference : camera->getRenderGroupReferences())
+ {
+ groups.push_back(getOrCreateGroup(reference));
+ }
+ camera->setRenderGroups(groups);
}
auto light = std::dynamic_pointer_cast<Light>(representation);
@@ -99,12 +105,12 @@ bool Manager::addRepresentation(std::shared_ptr<Representation> representation)
light->setGroup(getOrCreateGroup(light->getLightGroupReference()));
}
- SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Added representation " << representation->getName();
+ SURGSIM_LOG_INFO(m_logger) << "Added representation " << representation->getFullName();
result = true;
}
else
{
- SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Duplicate representation " << representation->getName();
+ SURGSIM_LOG_INFO(m_logger) << "Cannot add duplicate representation " << representation->getFullName();
}
return result;
}
@@ -115,12 +121,12 @@ bool Manager::addView(std::shared_ptr<View> view)
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();
+ SURGSIM_LOG_INFO(m_logger) << "Added view " << view->getFullName();
result = true;
}
else
{
- SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Duplicate view " << view->getName();
+ SURGSIM_LOG_INFO(m_logger) << "Cannot add duplicate view " << view->getFullName();
}
return result;
}
@@ -139,12 +145,12 @@ bool Manager::removeRepresentation(std::shared_ptr<Representation> representatio
if (it != m_representations.end())
{
m_representations.erase(it);
- SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Removed representation " << representation->getName();
+ SURGSIM_LOG_INFO(m_logger) << "Removed representation " << representation->getFullName();
result = true;
}
else
{
- SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Representation not found " << representation->getName();
+ SURGSIM_LOG_INFO(m_logger) << "Representation not found for removal, " << representation->getFullName();
}
return result;
}
@@ -156,12 +162,12 @@ bool Manager::removeView(std::shared_ptr<View> view)
if (it != m_views.end())
{
m_views.erase(it);
- SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Removed view " << view->getName();
+ SURGSIM_LOG_INFO(m_logger) << "Removed view " << view->getFullName();
result = true;
}
else
{
- SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " View not found " << view->getName();
+ SURGSIM_LOG_INFO(m_logger) << "View not found for removal, " << view->getFullName();
}
return result;
}
@@ -178,23 +184,18 @@ bool Manager::doStartUp()
bool Manager::doUpdate(double dt)
{
- processComponents();
processBehaviors(dt);
- for (auto it = m_representations.begin(); it != m_representations.end(); ++it)
+ processComponents();
+
+ for (auto& view : m_views)
{
- if ((*it)->isActive())
- {
- (*it)->update(dt);
- }
+ view->update(dt);
}
- for (auto it = m_views.begin(); it != m_views.end(); ++it)
+ for (auto& representation : m_representations)
{
- if ((*it)->isActive())
- {
- (*it)->update(dt);
- }
+ representation->update(dt);
}
return true;
@@ -208,7 +209,16 @@ int Manager::getType() const
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.";
+ SURGSIM_ASSERT(oldGroup == m_groups.end()) <<
+ "Tried to add a group that has already been added, " << group->getName();
m_groups[group->getName()] = group;
- SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Added group " << group->getName();
+ SURGSIM_LOG_INFO(m_logger) << "Added group " << group->getName();
+}
+
+void Manager::doBeforeStop()
+{
+ retireComponents(m_views);
+ retireComponents(m_representations);
+
+ ComponentManager::doBeforeStop();
}
diff --git a/SurgSim/Graphics/Manager.h b/SurgSim/Graphics/Manager.h
index a53e64d..41ec754 100644
--- a/SurgSim/Graphics/Manager.h
+++ b/SurgSim/Graphics/Manager.h
@@ -102,7 +102,7 @@ protected:
virtual bool doUpdate(double dt);
/// Overrides ComponentManager::getType()
- virtual int getType() const override;
+ 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.
@@ -114,6 +114,7 @@ protected:
/// \param group The group to be added.
virtual void addGroup(std::shared_ptr<Group> group);
+ void doBeforeStop() override;
private:
diff --git a/SurgSim/Graphics/Material.h b/SurgSim/Graphics/Material.h
index fb07217..34e80bf 100644
--- a/SurgSim/Graphics/Material.h
+++ b/SurgSim/Graphics/Material.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -28,7 +28,7 @@ namespace Graphics
{
class UniformBase;
-class Shader;
+class Program;
/// Base class that defines the interface for graphics materials.
///
@@ -51,13 +51,12 @@ public:
/// 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;
+ virtual void 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;
+ virtual void addUniform(const std::string& type, const std::string& name) = 0;
/// Removes a uniform from this material.
/// \param uniform Uniform to remove.
@@ -88,21 +87,21 @@ public:
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;
+ /// Sets the program used by this material.
+ /// \param program Shader program.
+ /// \return True if program was set successfully, otherwise false.
+ virtual bool setProgram(std::shared_ptr<Program> program) = 0;
- /// Gets the shader used by this material.
+ /// Gets the program used by this material.
/// \return Shader program.
- virtual std::shared_ptr<Shader> getShader() const = 0;
+ virtual std::shared_ptr<Program> getProgram() const = 0;
/// Removes the shader from the material, falling back to fixed-function pipeline.
- virtual void clearShader() = 0;
+ virtual void clearProgram() = 0;
};
}; // namespace Graphics
}; // namespace SurgSim
-#endif // SURGSIM_GRAPHICS_MATERIAL_H
\ No newline at end of file
+#endif // SURGSIM_GRAPHICS_MATERIAL_H
diff --git a/SurgSim/Graphics/Mesh-inl.h b/SurgSim/Graphics/Mesh-inl.h
index bda629a..ea18456 100644
--- a/SurgSim/Graphics/Mesh-inl.h
+++ b/SurgSim/Graphics/Mesh-inl.h
@@ -22,11 +22,9 @@ 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)
+template <class V, class E, class T>
+Mesh::Mesh(const TriangleMesh<V, E, T>& other)
+ : DataStructures::TriangleMesh<VertexData, DataStructures::EmptyData, DataStructures::EmptyData>(other)
{
}
diff --git a/SurgSim/Graphics/Mesh.cpp b/SurgSim/Graphics/Mesh.cpp
index 6b3c751..6e6592d 100644
--- a/SurgSim/Graphics/Mesh.cpp
+++ b/SurgSim/Graphics/Mesh.cpp
@@ -19,18 +19,35 @@
#include "SurgSim/Graphics/Mesh.h"
#include "SurgSim/Graphics/MeshPlyReaderDelegate.h"
-
using SurgSim::DataStructures::EmptyData;
+
+template<>
+std::string SurgSim::DataStructures::TriangleMesh<SurgSim::Graphics::VertexData, EmptyData, EmptyData>
+::m_className = "SurgSim::Graphics::Mesh";
+
namespace SurgSim
{
namespace Graphics
{
-Mesh::Mesh()
+SURGSIM_REGISTER(SurgSim::Framework::Asset, SurgSim::Graphics::Mesh, Mesh);
+
+Mesh::Mesh() :
+ m_updateCount(1)
{
}
+Mesh::Mesh( const Mesh& other ) : BaseType(other)
+{
+
+}
+
+Mesh::Mesh( Mesh&& other ) : BaseType(std::move(other))
+{
+
+}
+
void Mesh::initialize(
const std::vector<SurgSim::Math::Vector3d>& vertices,
const std::vector<SurgSim::Math::Vector4d>& colors,
@@ -88,5 +105,48 @@ void Mesh::initialize(
}
}
+bool Mesh::doLoad(const std::string& fileName)
+{
+ SurgSim::DataStructures::PlyReader reader(fileName);
+ if (! reader.isValid())
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger())
+ << "'" << fileName << "' is an invalid .ply file.";
+ return false;
+ }
+
+ auto delegate = std::make_shared<MeshPlyReaderDelegate>(std::dynamic_pointer_cast<Mesh>(shared_from_this()));
+ if (! reader.parseWithDelegate(delegate))
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger())
+ << "The input file '" << fileName << "' does not have the property required by triangle mesh.";
+ return false;
+ }
+
+ return true;
+}
+
+void Mesh::dirty()
+{
+ ++m_updateCount;
+}
+
+size_t Mesh::getUpdateCount() const
+{
+ return m_updateCount;
+}
+
+Mesh& Mesh::operator=( const Mesh& other )
+{
+ BaseType::operator=(other);
+ return *this;
+}
+
+Mesh& Mesh::operator=( Mesh&& other )
+{
+ BaseType::operator=(std::move(other));
+ return *this;
+}
+
}; // Graphics
}; // SurgSim
diff --git a/SurgSim/Graphics/Mesh.h b/SurgSim/Graphics/Mesh.h
index 46f54b4..496b611 100644
--- a/SurgSim/Graphics/Mesh.h
+++ b/SurgSim/Graphics/Mesh.h
@@ -19,7 +19,7 @@
#include <vector>
#include "SurgSim/DataStructures/EmptyData.h"
-#include "SurgSim/DataStructures/TriangleMeshBase.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
#include "SurgSim/DataStructures/OptionalValue.h"
#include "SurgSim/Math/Vector.h"
@@ -52,21 +52,41 @@ struct VertexData
}
};
-class Mesh : public SurgSim::DataStructures::TriangleMeshBase<VertexData, SurgSim::DataStructures::EmptyData,
+SURGSIM_STATIC_REGISTRATION(Mesh);
+
+class Mesh : public SurgSim::DataStructures::TriangleMesh<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.
+ typedef TriangleMesh<VertexData, DataStructures::EmptyData, DataStructures::EmptyData> BaseType;
+
+ /// Copy constructor when the template data is a different type
+ /// \tparam V Type of extra data stored in each vertex
+ /// \tparam E Type of extra data stored in each edge
+ /// \tparam T Type of extra data stored in each triangle
+ /// \param other The mesh to be copied from. Vertex, edge and triangle data will not be copied
/// \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);
+ template <class V, class E, class T>
+ explicit Mesh(const TriangleMesh<V, E, T>& other);
+
+ /// Copy Constructor
+ /// \param other Constructor source
+ Mesh(const Mesh& other);
+
+ /// Move Constructor
+ /// \param other Constructor source
+ Mesh(Mesh&& other);
+
+ /// Copy Assignment
+ /// \param other Assignment source
+ Mesh& operator=(const Mesh& other);
+
+ /// Move Assignment
+ /// \param other Assignment source
+ Mesh& operator=(Mesh&& other);
/// Utility function to initialize a mesh with plain data,
/// \param vertices An array of vertex coordinates.
@@ -80,6 +100,20 @@ public:
const std::vector<SurgSim::Math::Vector4d>& colors,
const std::vector<SurgSim::Math::Vector2d>& textures,
const std::vector<size_t>& triangles);
+
+ /// Increase the update count, this indicates that the mesh has been changed, if used in a mesh representation
+ /// the mesh representation will still only update the data members that have been marked for updating
+ void dirty();
+
+ /// Return the update count, please note that it will silently roll over when the range of size_t has been exceeded
+ size_t getUpdateCount() const;
+
+
+protected:
+ bool doLoad(const std::string& fileName) override;
+
+ /// For checking whether the mesh has changed
+ size_t m_updateCount;
};
@@ -88,4 +122,4 @@ public:
#include "SurgSim/Graphics/Mesh-inl.h"
-#endif // SURGSIM_GRAPHICS_MESH_H
\ No newline at end of file
+#endif // SURGSIM_GRAPHICS_MESH_H
diff --git a/SurgSim/Graphics/MeshPlyReaderDelegate.h b/SurgSim/Graphics/MeshPlyReaderDelegate.h
index 90112cc..ab3306c 100644
--- a/SurgSim/Graphics/MeshPlyReaderDelegate.h
+++ b/SurgSim/Graphics/MeshPlyReaderDelegate.h
@@ -40,7 +40,7 @@ public:
/// \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;
+ void processVertex(const std::string& elementName) override;
};
diff --git a/SurgSim/Graphics/MeshRepresentation.h b/SurgSim/Graphics/MeshRepresentation.h
index a0af003..285539a 100644
--- a/SurgSim/Graphics/MeshRepresentation.h
+++ b/SurgSim/Graphics/MeshRepresentation.h
@@ -16,13 +16,19 @@
#ifndef SURGSIM_GRAPHICS_MESHREPRESENTATION_H
#define SURGSIM_GRAPHICS_MESHREPRESENTATION_H
+#include "SurgSim/Framework/Asset.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Graphics/Mesh.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
@@ -44,27 +50,31 @@ public:
/// \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, std::shared_ptr<SurgSim::Framework::Asset>, Mesh, getMesh,
+ setMesh);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(MeshRepresentation, int, UpdateOptions, getUpdateOptions, setUpdateOptions);
+ SURGSIM_ADD_SETTER(MeshRepresentation, std::string, MeshFileName, loadMesh);
}
+ /// Destructor
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;
+ /// Sets the mesh.
+ /// \param mesh the graphics mesh to be used
+ virtual void setMesh(std::shared_ptr<SurgSim::Framework::Asset> mesh) = 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;
+ /// Convenience function to trigger the load of the mesh with the given filename
+ /// \param fileName name of the file to be loaded
+ virtual void loadMesh(const std::string& fileName) = 0;
+
+ /// Sets the shape of the representation
+ /// param shape the shape of this representation
+ /// \note only shapes of type MeshShape are supported
+ virtual void setShape(std::shared_ptr<SurgSim::Math::Shape> shape) = 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
@@ -75,6 +85,8 @@ public:
/// Gets update options for this mesh.
/// \return The update options.
virtual int getUpdateOptions() const = 0;
+
+ virtual void updateMesh(const Mesh& mesh) = 0;
};
}; // Graphics
diff --git a/SurgSim/Graphics/MeshUtilities.cpp b/SurgSim/Graphics/MeshUtilities.cpp
deleted file mode 100644
index 2fc1963..0000000
--- a/SurgSim/Graphics/MeshUtilities.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 2cdd0f8..0000000
--- a/SurgSim/Graphics/MeshUtilities.h
+++ /dev/null
@@ -1,39 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/Model.h b/SurgSim/Graphics/Model.h
new file mode 100644
index 0000000..2add2ac
--- /dev/null
+++ b/SurgSim/Graphics/Model.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_GRAPHICS_MODEL_H
+#define SURGSIM_GRAPHICS_MODEL_H
+
+#include "SurgSim/Framework/Asset.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+// Wraps the Graphics Model class
+class Model : public SurgSim::Framework::Asset
+{
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Graphics/OsgAxesRepresentation.cpp b/SurgSim/Graphics/OsgAxesRepresentation.cpp
index 9bc8ef7..28016b5 100644
--- a/SurgSim/Graphics/OsgAxesRepresentation.cpp
+++ b/SurgSim/Graphics/OsgAxesRepresentation.cpp
@@ -24,6 +24,8 @@ namespace SurgSim
namespace Graphics
{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgAxesRepresentation, OsgAxesRepresentation);
+
OsgAxesRepresentation::OsgAxesRepresentation(const std::string& name) :
Representation(name),
OsgRepresentation(name),
@@ -48,7 +50,7 @@ std::shared_ptr<OsgUnitAxes> OsgAxesRepresentation::getShareUnitAxes()
void OsgAxesRepresentation::setSize(double val)
{
m_size = val;
- m_transform->setScale(osg::Vec3d(val,val,val));
+ m_transform->setScale(osg::Vec3d(val, val, val));
}
double OsgAxesRepresentation::getSize()
diff --git a/SurgSim/Graphics/OsgAxesRepresentation.h b/SurgSim/Graphics/OsgAxesRepresentation.h
index 230a1b3..e99f5f9 100644
--- a/SurgSim/Graphics/OsgAxesRepresentation.h
+++ b/SurgSim/Graphics/OsgAxesRepresentation.h
@@ -31,6 +31,8 @@ namespace SurgSim
namespace Graphics
{
+SURGSIM_STATIC_REGISTRATION(OsgAxesRepresentation);
+
/// Osg axes representation implementation for the AxesRepresentation interface in graphics.
class OsgAxesRepresentation : public OsgRepresentation, public AxesRepresentation
{
@@ -40,13 +42,15 @@ public:
explicit OsgAxesRepresentation(const std::string& name);
~OsgAxesRepresentation();
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgAxesRepresentation);
+
/// Sets the size of the shown axes.
/// \param val The value.
- virtual void setSize(double val) override;
+ void setSize(double val) override;
/// Gets the current size.
/// \return The size.
- virtual double getSize() override;
+ double getSize() override;
private:
diff --git a/SurgSim/Graphics/OsgBoxRepresentation.h b/SurgSim/Graphics/OsgBoxRepresentation.h
index 189fbee..e5780c1 100644
--- a/SurgSim/Graphics/OsgBoxRepresentation.h
+++ b/SurgSim/Graphics/OsgBoxRepresentation.h
@@ -51,27 +51,27 @@ public:
/// Sets the size along X-axis of the box
/// \param sizeX Size along X-axis of the box
- virtual void setSizeX(double sizeX) override;
+ 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;
+ 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;
+ 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;
+ 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;
+ 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;
+ double getSizeZ() const override;
/// Sets the size of the box
/// \param sizeX Size along X-axis of the box
@@ -86,11 +86,11 @@ public:
/// Sets the size of the box
/// \param size Size of the box
- virtual void setSize(const SurgSim::Math::Vector3d& size) override;
+ 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;
+ SurgSim::Math::Vector3d getSize() const override;
private:
/// The OSG box shape is a unit box and this transform scales it to the size set.
diff --git a/SurgSim/Graphics/OsgCamera.cpp b/SurgSim/Graphics/OsgCamera.cpp
index c72405e..d6c487a 100644
--- a/SurgSim/Graphics/OsgCamera.cpp
+++ b/SurgSim/Graphics/OsgCamera.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -62,6 +62,78 @@ const osg::Camera::RenderOrder RenderOrderEnums[3] =
osg::Camera::POST_RENDER
};
+/// Update the main camera uniforms to reflect the state of the rendering camera, this is used when rendering
+/// stereo via osg, the mainCamera.* uniforms will carry the values of the currently rendering camera, i.e. the left
+/// and the right view camera.
+class UniformUpdater : public osg::NodeCallback
+{
+public:
+ UniformUpdater(osg::Uniform* projection, osg::Uniform* inverseProjection,
+ osg::Uniform* view, osg::Uniform* inverseView) :
+ m_projection(projection),
+ m_inverseProjection(inverseProjection),
+ m_view(view),
+ m_inverseView(inverseView)
+ {
+
+ }
+
+ virtual void operator()(osg::Node* node, osg::NodeVisitor* nodeVisitor)
+ {
+ osgUtil::CullVisitor* cv = dynamic_cast<osgUtil::CullVisitor*>(nodeVisitor);
+ if (cv != nullptr)
+ {
+ auto camera = cv->getCurrentCamera();
+ const auto& projection = camera->getProjectionMatrix();
+ m_projection->set(projection);
+ m_inverseProjection->set(osg::Matrix::inverse(projection));
+ m_view->set(camera->getViewMatrix());
+ m_inverseView->set(camera->getInverseViewMatrix());
+ }
+ traverse(node, nodeVisitor);
+ }
+
+private:
+ osg::ref_ptr<osg::Uniform> m_projection;
+ osg::ref_ptr<osg::Uniform> m_inverseProjection;
+ osg::ref_ptr<osg::Uniform> m_view;
+ osg::ref_ptr<osg::Uniform> m_inverseView;
+};
+
+osg::Uniform* addMatrixUniform(osg::Node* node, const std::string& name)
+{
+ auto uniform = new osg::Uniform;
+
+ uniform->setName(name);
+ uniform->setType(osg::Uniform::FLOAT_MAT4);
+
+ osg::Matrix matrix;
+ uniform->set(matrix);
+
+ node->getOrCreateStateSet()->addUniform(uniform);
+
+ return uniform;
+}
+
+osg::Switch* createUniformUpdateNode(long mask) // NOLINT
+{
+ auto node = new osg::Switch;
+
+ auto projection = addMatrixUniform(node, "mainCamera.projectionMatrix");
+ auto inverseProjection = addMatrixUniform(node, "mainCamera.inverseProjectionMatrix");
+ auto view = addMatrixUniform(node, "mainCamera.viewMatrix");
+ auto inverseView = addMatrixUniform(node, "mainCamera.inverseViewMatrix");
+
+ auto callback = new UniformUpdater(projection, inverseProjection, view, inverseView);
+ node->addCullCallback(callback);
+ node->setNodeMask(mask);
+
+ return node;
+}
+
+const long CullMask = 0xfffffff1; // NOLINT
+const long CullMaskLeft = 0x1; // NOLINT
+const long CullMaskRight = 0x2; // NOLINT
};
@@ -77,10 +149,10 @@ OsgCamera::OsgCamera(const std::string& 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_ambientColorUniform(std::make_shared<OsgUniform<Vector4f>>("ambientColor")),
+ m_isMainCamera(false)
{
m_switch->removeChildren(0, m_switch->getNumChildren());
m_camera->setName(name + " Camera");
@@ -90,10 +162,16 @@ OsgCamera::OsgCamera(const std::string& name) :
m_materialProxy->setName(name + " MaterialProxy");
m_camera->setViewMatrix(toOsg(getLocalPose().inverse().matrix()));
- m_camera->setProjectionMatrixAsPerspective(45.0, 1.0, 0.01, 10.0);
+ setPerspectiveProjection(45.0, 1.0, 0.01, 10.0);
m_camera->setComputeNearFarMode(osgUtil::CullVisitor::DO_NOT_COMPUTE_NEAR_FAR);
+ // Hopefully this will ignore the left and right nodes when rendering mono and
+ // pick the appropriate node when rendering stereo
+ m_camera->setCullMask(CullMask);
+ m_camera->setCullMaskLeft(CullMaskLeft);
+ m_camera->setCullMaskRight(CullMaskRight);
+
/// Update storage of view and projection matrices
m_projectionMatrix = fromOsg(m_camera->getProjectionMatrix());
@@ -104,36 +182,54 @@ OsgCamera::OsgCamera(const std::string& name) :
m_ambientColorUniform->addToStateSet(state);
setAmbientColor(Vector4d(0.0, 0.0, 0.0, 0.0));
+
+ // We will want this in all cases
+ m_camera->getOrCreateStateSet()->setMode(GL_TEXTURE_CUBE_MAP_SEAMLESS, osg::StateAttribute::ON);
}
-bool OsgCamera::setRenderGroup(std::shared_ptr<SurgSim::Graphics::Group> group)
+bool OsgCamera::setRenderGroup(std::shared_ptr<Group> group)
{
+ std::vector<std::shared_ptr<Group>> groups(1, group);
+ return setRenderGroups(groups);
+}
- 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))
+bool OsgCamera::setRenderGroups(const std::vector<std::shared_ptr<Group>>& groups)
+{
+ bool result = false;
+ int numChildren = m_materialProxy->getNumChildren();
+ for (const auto& group : groups)
{
- m_materialProxy->removeChildren(0, m_camera->getNumChildren()); /// Remove any previous group
- m_materialProxy->addChild(osgGroup->getOsgGroup());
- return true;
+ std::vector<std::string> groupReferences = Camera::getRenderGroupReferences();
+ SURGSIM_ASSERT(std::find(groupReferences.begin(), groupReferences.end(),
+ group->getName()) != groupReferences.end())
+ << "Trying to set the wrong group in the camera with group name <" << group->getName() << ">.";
+
+ std::shared_ptr<OsgGroup> osgGroup = std::dynamic_pointer_cast<OsgGroup>(group);
+ if (osgGroup)
+ {
+ result = true;
+ m_materialProxy->addChild(osgGroup->getOsgGroup());
+ }
+ else
+ {
+ result = false;
+ break;
+ }
}
- else
+
+ if (result)
{
- return false;
+ m_materialProxy->removeChildren(0, numChildren);
+ result = Graphics::Camera::setRenderGroups(groups);
}
-}
-void OsgCamera::setVisible(bool visible)
-{
- m_switch->setChildValue(m_camera, visible);
+ return result;
}
-bool OsgCamera::isVisible() const
+void OsgCamera::setLocalActive(bool val)
{
- return m_switch->getChildValue(m_camera);
+ Component::setLocalActive(val);
+ m_switch->setChildValue(m_camera, isActive());
}
SurgSim::Math::Matrix44d OsgCamera::getViewMatrix() const
@@ -152,19 +248,29 @@ const SurgSim::Math::Matrix44d& OsgCamera::getProjectionMatrix() const
return m_projectionMatrix;
}
+SurgSim::Math::Matrix44d OsgCamera::getInverseProjectionMatrix() const
+{
+ return m_projectionMatrix.inverse();
+}
+
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());
+ setVisible(isActive());
- auto viewMatrix = getViewMatrix();
- auto floatMatrix = viewMatrix.cast<float>();
- m_camera->setViewMatrix(toOsg(viewMatrix));
- m_viewMatrixUniform->set(floatMatrix);
- m_inverseViewMatrixUniform->set(floatMatrix.inverse());
+ if (isActive())
+ {
+ // 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)
@@ -197,7 +303,6 @@ bool OsgCamera::setRenderTarget(std::shared_ptr<RenderTarget> renderTarget)
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;
}
@@ -216,7 +321,7 @@ std::shared_ptr<RenderTarget> OsgCamera::getRenderTarget() const
}
-bool OsgCamera::setMaterial(std::shared_ptr<Material> material)
+bool OsgCamera::setMaterial(std::shared_ptr<SurgSim::Framework::Component> material)
{
std::shared_ptr<OsgMaterial> osgMaterial = std::dynamic_pointer_cast<OsgMaterial>(material);
bool result = false;
@@ -255,6 +360,101 @@ void OsgCamera::detachCurrentRenderTarget()
m_renderTarget = nullptr;
}
+void OsgCamera::setViewport(int x, int y, int width, int height)
+{
+ m_camera->setViewport(x, y, width, height);
+}
+
+void OsgCamera::getViewport(int* x, int* y, int* width, int* height) const
+{
+ SURGSIM_ASSERT(x != nullptr && y != nullptr && width != nullptr && height != nullptr)
+ << "Parameter can't be nullptr.";
+
+ auto viewPort = m_camera->getViewport();
+
+ SURGSIM_ASSERT(viewPort != nullptr) << "Trying to access viewport before it has been established.";
+
+ *x = viewPort->x();
+ *y = viewPort->y();
+ *width = viewPort->width();
+ *height = viewPort->height();
+}
+
+void OsgCamera::setViewportSize(std::array<double, 2> dimensions)
+{
+ auto viewPort = m_camera->getViewport();
+
+ if (viewPort != nullptr)
+ {
+ double aspectRatioChange = (dimensions[0] / viewPort->width()) / (dimensions[1] / viewPort->height());
+ m_camera->setViewport(viewPort->x(), viewPort->y(), dimensions[0], dimensions[1]);
+ m_camera->getProjectionMatrix() *= osg::Matrix::scale(1.0 / aspectRatioChange, aspectRatioChange,1.0);
+ m_projectionMatrix = fromOsg(m_camera->getProjectionMatrix());
+ }
+ else
+ {
+ m_camera->setViewport(0, 0, dimensions[0], dimensions[1]);
+ }
+}
+
+std::array<double, 2> OsgCamera::getViewportSize() const
+{
+ auto viewPort = m_camera->getViewport();
+
+ SURGSIM_ASSERT(viewPort != nullptr) << "Trying to access viewport before it has been established.";
+
+ std::array<double, 2> dimensions = {viewPort->width(), viewPort->height()};
+
+ return dimensions;
+}
+
+void OsgCamera::setMainCamera(bool val)
+{
+ if (val != m_isMainCamera)
+ {
+ if (val)
+ {
+ m_camera->removeChild(m_materialProxy);
+
+ std::array<long, 2> masks = {CullMaskLeft, CullMaskRight}; // NOLINT
+
+ // Insert two nodes into the camera hierarchy, they will update the global uniforms with the correct
+ // value. Also attach the material proxy to each of the nodes.
+ for (auto& mask : masks)
+ {
+ auto node = createUniformUpdateNode(mask);
+ m_camera->addChild(node);
+ node->addChild(m_materialProxy);
+ }
+ }
+ else
+ {
+ // Remove the update nodes and hook the material proxy node back up
+ m_camera->removeChildren(0, m_camera->getNumChildren());
+ m_camera->addChild(m_materialProxy);
+ }
+ m_isMainCamera = val;
+ }
+}
+
+bool OsgCamera::isMainCamera()
+{
+ return m_isMainCamera;
+}
+
+void OsgCamera::setPerspectiveProjection(double fovy, double aspect, double near, double far)
+{
+ m_camera->setProjectionMatrixAsPerspective(fovy, aspect, near, far);
+ m_projectionMatrix = fromOsg(m_camera->getProjectionMatrix());
+}
+
+
+void OsgCamera::setOrthogonalProjection(double left, double right, double bottom, double top, double near, double far)
+{
+ m_camera->setProjectionMatrixAsOrtho(left, right, bottom, top, near, far);
+ m_projectionMatrix = fromOsg(m_camera->getProjectionMatrix());
+}
+
void OsgCamera::attachRenderTargetTexture(osg::Camera::BufferComponent buffer, std::shared_ptr<Texture> texture)
{
if (texture == nullptr)
@@ -263,8 +463,8 @@ void OsgCamera::attachRenderTargetTexture(osg::Camera::BufferComponent buffer, s
}
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";
+ SURGSIM_ASSERT(osgTexture != nullptr)
+ << "RenderTarget used a texture that was not an OsgTexture subclass";
osg::Texture* actualTexture = osgTexture->getOsgTexture();
SURGSIM_ASSERT(actualTexture != nullptr) <<
@@ -307,7 +507,10 @@ SurgSim::Math::Vector4d OsgCamera::getAmbientColor()
return m_ambientColor;
}
-
+void OsgCamera::setGenerateTangents(bool value)
+{
+ SURGSIM_ASSERT(value == false) << "Generate Tangents is not supported on Cameras.";
+}
}; // namespace Graphics
}; // namespace SurgSim
diff --git a/SurgSim/Graphics/OsgCamera.h b/SurgSim/Graphics/OsgCamera.h
index 66f760a..2d99405 100644
--- a/SurgSim/Graphics/OsgCamera.h
+++ b/SurgSim/Graphics/OsgCamera.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -63,21 +63,23 @@ public:
SURGSIM_CLASSNAME(SurgSim::Graphics::OsgCamera);
- virtual bool setRenderGroup(std::shared_ptr<Group> group) override;
+ bool setRenderGroup(std::shared_ptr<Group> group) override;
- virtual void setVisible(bool visible) override;
+ bool setRenderGroups(const std::vector<std::shared_ptr<Group>>& groups) override;
- virtual bool isVisible() const override;
+ void setLocalActive(bool val) override;
virtual SurgSim::Math::Matrix44d getViewMatrix() const;
virtual SurgSim::Math::Matrix44d getInverseViewMatrix() const;
- virtual void setProjectionMatrix(const SurgSim::Math::Matrix44d& matrix) override;
+ void setProjectionMatrix(const SurgSim::Math::Matrix44d& matrix) override;
- virtual const SurgSim::Math::Matrix44d& getProjectionMatrix() const override;
+ const SurgSim::Math::Matrix44d& getProjectionMatrix() const override;
- virtual void update(double dt) override;
+ SurgSim::Math::Matrix44d getInverseProjectionMatrix() const override;
+
+ void update(double dt) override;
/// \return the OSG camera node
osg::ref_ptr<osg::Camera> getOsgCamera() const;
@@ -85,26 +87,47 @@ public:
/// \return the OSG parent node for this object
osg::ref_ptr<osg::Node> getOsgNode() const;
- virtual bool setRenderTarget(std::shared_ptr<RenderTarget> renderTarget) override;
+ bool setRenderTarget(std::shared_ptr<RenderTarget> renderTarget) override;
+
+ std::shared_ptr<RenderTarget> getRenderTarget() const override;
+
+ bool setMaterial(std::shared_ptr<SurgSim::Framework::Component> material) override;
+
+ std::shared_ptr<Material> getMaterial() const override;
+
+ void clearMaterial() override;
+
+ void setRenderOrder(RenderOrder order, int value) override;
- virtual std::shared_ptr<RenderTarget> getRenderTarget() const override;
+ void setAmbientColor(const SurgSim::Math::Vector4d& color) override;
- virtual bool setMaterial(std::shared_ptr<Material> material) override;
+ SurgSim::Math::Vector4d getAmbientColor() override;
- virtual std::shared_ptr<Material> getMaterial() const override;
+ void setGenerateTangents(bool value) override;
- virtual void clearMaterial() override;
+ void setPerspectiveProjection(double fovy, double aspect, double near, double far) override;
- virtual void setRenderOrder(RenderOrder order, int value) override;
+ void setOrthogonalProjection(
+ double left, double right,
+ double bottom, double top,
+ double near, double far) override;
- virtual void setAmbientColor(const SurgSim::Math::Vector4d& color) override;
- virtual SurgSim::Math::Vector4d getAmbientColor() override;
+ void setViewport(int x, int y, int width, int height) override;
+
+ void getViewport(int* x, int* y, int* width, int* height) const override;
+
+ void setViewportSize(std::array<double, 2> dimensions) override;
+
+ std::array<double, 2> getViewportSize() const override;
+
+ void setMainCamera(bool val) override;
+
+ bool isMainCamera() 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;
@@ -132,7 +155,7 @@ private:
/// Value for ambient color
SurgSim::Math::Vector4d m_ambientColor;
-
+ bool m_isMainCamera;
};
}; // namespace Graphics
diff --git a/SurgSim/Graphics/OsgCapsuleRepresentation.h b/SurgSim/Graphics/OsgCapsuleRepresentation.h
index e244264..837ddd7 100644
--- a/SurgSim/Graphics/OsgCapsuleRepresentation.h
+++ b/SurgSim/Graphics/OsgCapsuleRepresentation.h
@@ -52,33 +52,33 @@ public:
/// Sets the radius of the capsule
/// \param radius Radius of the capsule
- virtual void setRadius(double radius) override;
+ 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;
+ double getRadius() const override;
/// Sets the height of the capsule
/// \param height Height of the capsule
- virtual void setHeight(double height) override;
+ void setHeight(double height) override;
/// Returns the height of the capsule
/// \return Height along Y-axis of the capsule
- virtual double getHeight() const override;
+ 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;
+ 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;
+ 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;
+ 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;
+ SurgSim::Math::Vector2d getSize() const override;
private:
/// The OSG Capsule shape consist of one unit cylinder and two unit spheres
diff --git a/SurgSim/Graphics/OsgCurveRepresentation.cpp b/SurgSim/Graphics/OsgCurveRepresentation.cpp
new file mode 100644
index 0000000..89ead82
--- /dev/null
+++ b/SurgSim/Graphics/OsgCurveRepresentation.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/Graphics/OsgCurveRepresentation.h"
+
+#include <array>
+
+#include <osg/Array>
+#include <osg/Geode>
+#include <osg/Geometry>
+#include <osg/Hint>
+#include <osg/LineWidth>
+#include <osg/PositionAttitudeTransform>
+
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Graphics/OsgConversions.h"
+#include "SurgSim/Math/CardinalSplines.h"
+#include "SurgSim/Math/Geometry.h"
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgCurveRepresentation, OsgCurveRepresentation)
+
+OsgCurveRepresentation::OsgCurveRepresentation(const std::string& name) :
+ Representation(name),
+ OsgRepresentation(name),
+ CurveRepresentation(name),
+ m_subdivision(100),
+ m_tension(0.4)
+{
+ osg::Geode* geode = new osg::Geode();
+ m_geometry = new osg::Geometry();
+ m_geometry->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+
+ // At this stage there are no vertices in there
+ m_drawArrays = new osg::DrawArrays(osg::PrimitiveSet::LINE_STRIP, 0, 0);
+ m_geometry->addPrimitiveSet(m_drawArrays);
+ m_geometry->setUseDisplayList(false);
+ m_geometry->setUseVertexBufferObjects(true);
+ m_geometry->setDataVariance(osg::Object::DYNAMIC);
+
+ m_vertexData = new osg::Vec3Array;
+ m_geometry->setVertexArray(m_vertexData);
+
+ m_normalData = new osg::Vec3Array;
+ m_geometry->setNormalArray(m_normalData, osg::Array::BIND_PER_VERTEX);
+
+ setColor(Math::Vector4d(1.0, 1.0, 1.0, 1.0));
+ setWidth(1.5);
+ setAntiAliasing(true);
+
+ geode->addDrawable(m_geometry);
+ m_transform->addChild(geode);
+}
+
+OsgCurveRepresentation::~OsgCurveRepresentation()
+{
+}
+
+bool OsgCurveRepresentation::doInitialize()
+{
+ return true;
+}
+
+bool OsgCurveRepresentation::doWakeUp()
+{
+ return true;
+}
+
+void OsgCurveRepresentation::doUpdate(double dt)
+{
+ DataStructures::VerticesPlain controlPoints;
+ if (m_locker.tryTakeChanged(&controlPoints))
+ {
+ updateGraphics(controlPoints);
+ }
+}
+
+void OsgCurveRepresentation::setSubdivisions(size_t num)
+{
+ m_subdivision = num;
+}
+
+size_t OsgCurveRepresentation::getSubdivisions() const
+{
+ return m_subdivision;
+}
+
+void OsgCurveRepresentation::setTension(double tension)
+{
+ m_tension = tension;
+}
+
+double OsgCurveRepresentation::getTension() const
+{
+ return m_tension;
+}
+
+void OsgCurveRepresentation::updateGraphics(const DataStructures::VerticesPlain& controlPoints)
+{
+ const double stepsize = 1.0 / (m_subdivision + 1);
+
+ Math::CardinalSplines::extendControlPoints(controlPoints, &m_controlPoints);
+
+ size_t numPoints = static_cast<size_t>(static_cast<double>(m_controlPoints.size() - 3) / stepsize);
+
+ m_vertices.clear();
+ m_vertices.reserve(numPoints);
+
+ Math::CardinalSplines::interpolate(getSubdivisions(), m_controlPoints, &m_vertices, getTension());
+
+ size_t vertexCount = m_vertices.size();
+ if (m_vertexData->size() != vertexCount)
+ {
+ m_vertexData->resize(vertexCount);
+ m_normalData->resize(vertexCount);
+
+ m_drawArrays->set(osg::PrimitiveSet::LINE_STRIP, 0, vertexCount);
+ m_drawArrays->dirty();
+ }
+
+ // #performance
+ // Calculate the bounding box while iterating over the vertices, this will save osg time in the update traversal
+ for (size_t i = 0; i < vertexCount; ++i)
+ {
+ const auto& vertex0 = m_vertices[i];
+ const auto& vertex1 = (i < vertexCount - 1) ? m_vertices[i + 1] : m_controlPoints.back();
+
+ // Assign the segment into the normal for use in the shader
+ const auto& normal = vertex1 - vertex0;
+
+ (*m_normalData)[i][0] = static_cast<float>(normal[0]);
+ (*m_normalData)[i][1] = static_cast<float>(normal[1]);
+ (*m_normalData)[i][2] = static_cast<float>(normal[2]);
+
+ (*m_vertexData)[i][0] = static_cast<float>(vertex0[0]);
+ (*m_vertexData)[i][1] = static_cast<float>(vertex0[1]);
+ (*m_vertexData)[i][2] = static_cast<float>(vertex0[2]);
+ }
+
+ m_vertexData->dirty();
+ m_normalData->dirty();
+ m_geometry->dirtyBound();
+}
+
+void OsgCurveRepresentation::setWidth(double width)
+{
+ auto lineWidth = new osg::LineWidth(width);
+ m_geometry->getOrCreateStateSet()->setAttribute(lineWidth, osg::StateAttribute::ON);
+ m_width = width;
+}
+
+double OsgCurveRepresentation::getWidth() const
+{
+ return m_width;
+}
+
+void OsgCurveRepresentation::setAntiAliasing(bool val)
+{
+ auto state = m_geometry->getOrCreateStateSet();
+ if (val && state->getMode(GL_BLEND) == osg::StateAttribute::INHERIT)
+ {
+ state->setMode(GL_BLEND, osg::StateAttribute::ON);
+ state->setMode(GL_LINE_SMOOTH, osg::StateAttribute::ON);
+ }
+ else
+ {
+ state->setMode(GL_BLEND, osg::StateAttribute::INHERIT);
+ state->setMode(GL_LINE_SMOOTH, osg::StateAttribute::INHERIT);
+ }
+}
+
+bool OsgCurveRepresentation::isAntiAliasing() const
+{
+ return m_geometry->getOrCreateStateSet()->getMode(GL_BLEND) == osg::StateAttribute::ON;
+}
+
+void OsgCurveRepresentation::setColor(const SurgSim::Math::Vector4d& color)
+{
+ 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;
+}
+
+Math::Vector4d OsgCurveRepresentation::getColor() const
+{
+ return m_color;
+}
+
+}
+}
diff --git a/SurgSim/Graphics/OsgCurveRepresentation.h b/SurgSim/Graphics/OsgCurveRepresentation.h
new file mode 100644
index 0000000..f6ebac5
--- /dev/null
+++ b/SurgSim/Graphics/OsgCurveRepresentation.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_OSGCURVEREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGCURVEREPRESENTATION_H
+
+#include "SurgSim/Graphics/CurveRepresentation.h"
+#include "SurgSim/Graphics/OsgRepresentation.h"
+
+#include <osg/Array>
+#include <osg/ref_ptr>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace osg
+{
+class Geometry;
+class DrawArrays;
+}
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+SURGSIM_STATIC_REGISTRATION(OsgCurveRepresentation)
+
+/// Implements the CurveRepresentation for OpenSceneGraph, it uses Catmull Rom interpolation, to draw the line
+/// as a GL_LINESTRIP. use the material_curve.vert shader for rendering. This class will also deposit the information
+/// of the segment in the normal information for the vertex.
+class OsgCurveRepresentation : public OsgRepresentation, public CurveRepresentation
+{
+public:
+ /// Constructor
+ /// \param name Name of the representation
+ explicit OsgCurveRepresentation(const std::string& name);
+
+ ~OsgCurveRepresentation();
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgCurveRepresentation);
+
+ bool doInitialize() override;
+
+ bool doWakeUp() override;
+
+ void doUpdate(double dt) override;
+
+ void setSubdivisions(size_t num) override;
+
+ size_t getSubdivisions() const override;
+
+ void setTension(double tension) override;
+
+ double getTension() const override;
+
+ void setColor(const SurgSim::Math::Vector4d& color) override;
+
+ Math::Vector4d getColor() const override;
+
+ void setWidth(double width) override;
+
+ double getWidth() const override;
+
+ void setAntiAliasing(bool val) override;
+
+ bool isAntiAliasing() const override;
+
+private:
+
+ /// Update the OSG structure with the information of the control points
+ /// \param controlPoints to use
+ void updateGraphics(const DataStructures::VerticesPlain& controlPoints);
+
+ ///@{
+ /// OSG handles for updating
+ osg::ref_ptr<osg::Geometry> m_geometry;
+ osg::ref_ptr<osg::Vec3Array> m_vertexData;
+ osg::ref_ptr<osg::Vec3Array> m_normalData;
+ osg::ref_ptr<osg::DrawArrays> m_drawArrays;
+ ///@}
+
+ /// @{
+ /// Members for the CurveRepresentation properties
+ Math::Vector4d m_color;
+ size_t m_subdivision;
+ double m_tension;
+ double m_width;
+ ///@}
+
+ ///@{
+ /// Local structures to keep allocations to a minimum
+ std::vector<Math::Vector3d> m_controlPoints;
+ std::vector<Math::Vector3d> m_vertices;
+ ///@}
+
+};
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+}
+}
+
+#endif
diff --git a/SurgSim/Graphics/OsgCylinderRepresentation.h b/SurgSim/Graphics/OsgCylinderRepresentation.h
index c42b988..89e9c1a 100644
--- a/SurgSim/Graphics/OsgCylinderRepresentation.h
+++ b/SurgSim/Graphics/OsgCylinderRepresentation.h
@@ -48,33 +48,33 @@ public:
/// Sets the radius of the cylinder
/// \param radius Radius along X-axis and Z-axis of the cylinder
- virtual void setRadius(double radius) override;
+ 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;
+ 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;
+ void setHeight(double height) override;
/// Returns the height of the cylinder
/// \return Height along Y-axis of the cylinder
- virtual double getHeight() const override;
+ 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;
+ 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;
+ 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;
+ 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;
+ SurgSim::Math::Vector2d getSize() const override;
private:
/// The OSG Cylinder shape is a unit Cylinder and this transform scales it to the size set.
diff --git a/SurgSim/Graphics/OsgFont.cpp b/SurgSim/Graphics/OsgFont.cpp
new file mode 100644
index 0000000..0a4e586
--- /dev/null
+++ b/SurgSim/Graphics/OsgFont.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/Graphics/OsgFont.h"
+
+#include <osgText/Font>
+
+#include "SurgSim/Framework/Log.h"
+
+SURGSIM_REGISTER(SurgSim::Framework::Asset, SurgSim::Graphics::OsgFont, OsgFont);
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+
+OsgFont::OsgFont()
+{
+
+}
+
+OsgFont::~OsgFont()
+{
+
+}
+
+bool OsgFont::doLoad(const std::string& filePath)
+{
+ std::string osgFilePath = osgText::findFontFile(filePath);
+
+ m_font = osgText::readFontFile(osgFilePath);
+ bool result = m_font.valid();
+ if (!result)
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getLogger("Graphics"))
+ << "The font could not be loaded from " << osgFilePath << ".";
+ }
+
+ return result;
+}
+
+osg::ref_ptr<osgText::Font> OsgFont::getOsgFont() const
+{
+ return m_font;
+}
+
+}
+}
+
diff --git a/SurgSim/Graphics/OsgFont.h b/SurgSim/Graphics/OsgFont.h
new file mode 100644
index 0000000..508231d
--- /dev/null
+++ b/SurgSim/Graphics/OsgFont.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_OSGFONT_H
+#define SURGSIM_GRAPHICS_OSGFONT_H
+
+#include "SurgSim/Graphics/Font.h"
+
+#include <osg/ref_ptr>
+
+namespace osgText
+{
+class Font;
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// Osg specialization of the Font class, supports osgText::Font
+class OsgFont : public SurgSim::Graphics::Font
+{
+public:
+ /// Constructor
+ OsgFont();
+
+ /// Destructor
+ virtual ~OsgFont();
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgFont);
+
+ /// \return the appropriate osgText::Font reference
+ osg::ref_ptr<osgText::Font> getOsgFont() const;
+
+
+protected:
+
+ bool doLoad(const std::string& filePath) override;
+
+private:
+
+ /// Osg pointer to the font structure
+ osg::ref_ptr<osgText::Font> m_font;
+
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Graphics/OsgGroup.h b/SurgSim/Graphics/OsgGroup.h
index 0a89e1e..8a38cee 100644
--- a/SurgSim/Graphics/OsgGroup.h
+++ b/SurgSim/Graphics/OsgGroup.h
@@ -38,33 +38,33 @@ public:
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;
+ /// \param visible True for visible, false for invisible
+ void setVisible(bool visible) override;
/// Gets whether this group is currently visible
- /// \return True for visible, false for invisible
- virtual bool isVisible() const override;
+ /// \return True for visible, false for invisible
+ 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;
+ 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;
+ 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;
+ bool remove(std::shared_ptr<Representation> representation) override;
/// Removes all representations
- virtual void clear() override;
+ void clear() override;
/// Returns the root OSG group node
osg::ref_ptr<osg::Group> getOsgGroup() const;
diff --git a/SurgSim/Graphics/OsgLight.cpp b/SurgSim/Graphics/OsgLight.cpp
index 99d2f10..47547f6 100644
--- a/SurgSim/Graphics/OsgLight.cpp
+++ b/SurgSim/Graphics/OsgLight.cpp
@@ -42,6 +42,8 @@ namespace SurgSim
namespace Graphics
{
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgLight, OsgLight);
+
/// \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
@@ -66,21 +68,21 @@ OsgLight::OsgLight(const std::string& name) :
m_switch->addChild(m_lightSource);
- m_uniforms[POSITION] = new osg::Uniform(Uniform::FLOAT_VEC4, prefix + "position");
+ m_uniforms[POSITION] = new osg::Uniform(osg::Uniform::FLOAT_VEC4, prefix + "position");
- m_uniforms[DIFFUSE_COLOR] = new osg::Uniform(Uniform::FLOAT_VEC4, prefix + "diffuse");
+ m_uniforms[DIFFUSE_COLOR] = new osg::Uniform(osg::Uniform::FLOAT_VEC4, prefix + "diffuse");
setDiffuseColor(m_diffuseColor);
- m_uniforms[SPECULAR_COLOR] = new osg::Uniform(Uniform::FLOAT_VEC4, prefix + "specular");
+ m_uniforms[SPECULAR_COLOR] = new osg::Uniform(osg::Uniform::FLOAT_VEC4, prefix + "specular");
setSpecularColor(m_specularColor);
- m_uniforms[CONSTANT_ATTENUATION] = new osg::Uniform(Uniform::FLOAT, prefix + "constantAttenuation");
+ m_uniforms[CONSTANT_ATTENUATION] = new osg::Uniform(osg::Uniform::FLOAT, prefix + "constantAttenuation");
setConstantAttenuation(m_constantAttenuation);
- m_uniforms[LINEAR_ATTENUATION] = new osg::Uniform(Uniform::FLOAT, prefix + "linearAttenuation");
+ m_uniforms[LINEAR_ATTENUATION] = new osg::Uniform(osg::Uniform::FLOAT, prefix + "linearAttenuation");
setLinearAttenuation(m_linearAttenuation);
- m_uniforms[QUADRATIC_ATTENUATION] = new osg::Uniform(Uniform::FLOAT, prefix + "quadraticAttenuation");
+ m_uniforms[QUADRATIC_ATTENUATION] = new osg::Uniform(osg::Uniform::FLOAT, prefix + "quadraticAttenuation");
setQuadraticAttenuation(m_quadraticAttenuation);
// By default light the main group
diff --git a/SurgSim/Graphics/OsgLight.h b/SurgSim/Graphics/OsgLight.h
index 1022edb..bd965c5 100644
--- a/SurgSim/Graphics/OsgLight.h
+++ b/SurgSim/Graphics/OsgLight.h
@@ -47,6 +47,8 @@ namespace Graphics
class OsgGroup;
+SURGSIM_STATIC_REGISTRATION(OsgLight);
+
/// OpenScenegraph implementation for the Light interface
class OsgLight : public OsgRepresentation, public Light
{
@@ -60,37 +62,39 @@ public:
explicit OsgLight(const std::string& name);
virtual ~OsgLight();
- virtual bool setGroup(std::shared_ptr<SurgSim::Graphics::Group> group) override;
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgLight);
+
+ bool setGroup(std::shared_ptr<SurgSim::Graphics::Group> group) override;
- virtual void setLightGroupReference(const std::string& name) override;
+ void setLightGroupReference(const std::string& name) override;
- virtual std::string getLightGroupReference() override;
+ std::string getLightGroupReference() override;
- virtual std::shared_ptr<SurgSim::Graphics::Group> getGroup() override;
+ std::shared_ptr<SurgSim::Graphics::Group> getGroup() override;
- virtual void setDiffuseColor(const SurgSim::Math::Vector4d& color) override;
+ void setDiffuseColor(const SurgSim::Math::Vector4d& color) override;
- virtual SurgSim::Math::Vector4d getDiffuseColor() override;
+ SurgSim::Math::Vector4d getDiffuseColor() override;
- virtual void setSpecularColor(const SurgSim::Math::Vector4d& color) override;
+ void setSpecularColor(const SurgSim::Math::Vector4d& color) override;
- virtual SurgSim::Math::Vector4d getSpecularColor() override;
+ SurgSim::Math::Vector4d getSpecularColor() override;
- virtual void setConstantAttenuation(double val) override;
+ void setConstantAttenuation(double val) override;
- virtual double getConstantAttenuation() override;
+ double getConstantAttenuation() override;
- virtual void setLinearAttenuation(double val) override;
+ void setLinearAttenuation(double val) override;
- virtual double getLinearAttenuation() override;
+ double getLinearAttenuation() override;
- virtual void setQuadraticAttenuation(double val) override;
+ void setQuadraticAttenuation(double val) override;
- virtual double getQuadraticAttenuation() override;
+ double getQuadraticAttenuation() override;
private:
- virtual void doUpdate(double dt) override;
+ void doUpdate(double dt) override;
/// Applies all the lights variables to the given StateSet
void apply(osg::ref_ptr<osg::StateSet> stateSet);
diff --git a/SurgSim/Graphics/OsgLog.h b/SurgSim/Graphics/OsgLog.h
index 677a221..ef2ce3f 100644
--- a/SurgSim/Graphics/OsgLog.h
+++ b/SurgSim/Graphics/OsgLog.h
@@ -47,7 +47,7 @@ public:
/// \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;
+ void notify(osg::NotifySeverity severity, const char *message) override;
private:
std::shared_ptr<SurgSim::Framework::Logger> m_logger;
diff --git a/SurgSim/Graphics/OsgManager.cpp b/SurgSim/Graphics/OsgManager.cpp
index f1ff717..263f966 100644
--- a/SurgSim/Graphics/OsgManager.cpp
+++ b/SurgSim/Graphics/OsgManager.cpp
@@ -29,12 +29,74 @@
#include <osgViewer/Scene>
#include <osgDB/WriteFile>
+#include <osg/NodeVisitor>
+#include <osg/NodeCallback>
+#include <osg/Matrixf>
+#include <osg/Uniform>
using SurgSim::Graphics::OsgRepresentation;
using SurgSim::Graphics::OsgCamera;
using SurgSim::Graphics::OsgGroup;
using SurgSim::Graphics::OsgManager;
+namespace
+{
+
+/// Class to update the "modelMatrix" uniform for all transforms in the scenegraph
+/// #performance This could be change to use a stack of matrices rather than query
+/// the nodepath for every transform
+class TransformUpdater : public osg::NodeCallback
+{
+public:
+ explicit TransformUpdater(osg::Uniform* uniform) : m_uniform(uniform)
+ {
+
+ }
+
+ void operator()(osg::Node* node, osg::NodeVisitor* nodeVisitor) override
+ {
+ osgUtil::CullVisitor* cv = dynamic_cast<osgUtil::CullVisitor*>(nodeVisitor);
+ if (cv != nullptr)
+ {
+ auto mat = osg::computeLocalToWorld(nodeVisitor->getNodePath(), true);
+ m_uniform->set(mat);
+ }
+ traverse(node, nodeVisitor);
+ }
+
+private:
+ osg::ref_ptr<osg::Uniform> m_uniform;
+};
+
+/// Class to find all transform nodes in the added scenegraph, and add the "modelMatrix" uniform
+/// to the stateset, also ads the appropriate callback to the node
+class TransformModifier : public osg::NodeVisitor
+{
+public:
+ TransformModifier() : osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ALL_CHILDREN)
+ {
+
+ }
+
+ void apply(osg::Transform& node) override
+ {
+ auto state = node.getOrCreateStateSet();
+ auto uniform = new osg::Uniform;
+ uniform->setName("modelMatrix");
+ uniform->setType(osg::Uniform::FLOAT_MAT4);
+
+ osg::Matrix matrix;
+ uniform->set(matrix);
+
+ state->addUniform(uniform);
+
+ auto callback = new TransformUpdater(uniform);
+ node.addCullCallback(callback);
+ }
+};
+
+}
+
namespace SurgSim
{
namespace Graphics
@@ -42,12 +104,30 @@ namespace Graphics
OsgManager::OsgManager() : SurgSim::Graphics::Manager(),
m_viewer(new osgViewer::CompositeViewer())
{
+ setMultiThreading(true);
}
OsgManager::~OsgManager()
{
}
+void OsgManager::setMultiThreading(bool val)
+{
+ if (val == true)
+ {
+ m_viewer->setThreadingModel(osgViewer::ViewerBase::DrawThreadPerContext);
+ }
+ else
+ {
+ m_viewer->setThreadingModel(osgViewer::ViewerBase::SingleThreaded);
+ }
+}
+
+bool OsgManager::isMultiThreading() const
+{
+ return m_viewer->getThreadingModel() != osgViewer::ViewerBase::SingleThreaded;
+}
+
std::shared_ptr<Group> OsgManager::getOrCreateGroup(const std::string& name)
{
std::shared_ptr<Group> result;
@@ -76,6 +156,11 @@ bool OsgManager::addRepresentation(std::shared_ptr<SurgSim::Graphics::Representa
if (osgRepresentation)
{
result = Manager::addRepresentation(osgRepresentation);
+ if (result)
+ {
+ TransformModifier modifier;
+ osgRepresentation->getOsgNode()->accept(modifier);
+ }
}
else
{
@@ -166,9 +251,13 @@ bool OsgManager::doUpdate(double dt)
void OsgManager::doBeforeStop()
{
+#ifdef OSS_DEBUG
dumpDebugInfo();
+#endif
// Delete the viewer so that the graphics context will be released in the manager's thread
m_viewer = nullptr;
+
+ Graphics::Manager::doBeforeStop();
}
osg::ref_ptr<osgViewer::CompositeViewer> OsgManager::getOsgCompositeViewer() const
diff --git a/SurgSim/Graphics/OsgManager.h b/SurgSim/Graphics/OsgManager.h
index f134b9d..47f7905 100644
--- a/SurgSim/Graphics/OsgManager.h
+++ b/SurgSim/Graphics/OsgManager.h
@@ -51,40 +51,47 @@ public:
/// Destructor
virtual ~OsgManager();
+ /// Change the threading model of the graphics manager viewer
+ /// \param val enables or disables multithreading in the viewer
+ void setMultiThreading(bool val);
+
+ /// \return true when the viewer is multithreading
+ bool isMultiThreading() const;
+
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;
+ void dumpDebugInfo() const override;
protected:
- virtual bool doUpdate(double dt) override;
+ bool doUpdate(double dt) override;
- virtual bool doInitialize() override;
+ bool doInitialize() override;
- virtual bool doStartUp() override;
+ 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;
+ 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;
+ 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;
+ bool removeView(std::shared_ptr<View> view) override;
- virtual std::shared_ptr<Group> getOrCreateGroup(const std::string& name) override;
+ std::shared_ptr<Group> getOrCreateGroup(const std::string& name) override;
private:
diff --git a/SurgSim/Graphics/OsgMaterial.cpp b/SurgSim/Graphics/OsgMaterial.cpp
index 86477f7..3f822e1 100644
--- a/SurgSim/Graphics/OsgMaterial.cpp
+++ b/SurgSim/Graphics/OsgMaterial.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -15,21 +15,22 @@
#include "SurgSim/Graphics/OsgMaterial.h"
+#include <algorithm>
+#include <boost/any.hpp>
+#include <functional>
+
#include "SurgSim/Framework/Accessible.h"
+#include "SurgSim/Framework/ApplicationData.h"
#include "SurgSim/Framework/Log.h"
-#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Graphics/OsgProgram.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
{
@@ -42,44 +43,42 @@ OsgMaterial::OsgMaterial(const std::string& name) :
}
-bool OsgMaterial::addUniform(std::shared_ptr<UniformBase> uniform)
+void 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);
+ SURGSIM_ASSERT(osgUniform != nullptr) << "Uniform must be an 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;
+ if (isInitialized())
+ {
+ osgUniform->addToStateSet(m_stateSet);
}
- return didSucceed;
+ 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");
}
-bool OsgMaterial::addUniform(const std::string& type, const std::string& name)
+void OsgMaterial::addUniform(const std::string& type, const std::string& name)
{
- OsgUniformFactory factory;
+ static OsgUniformFactory factory;
- bool result = false;
if (factory.isRegistered(type))
{
- result = addUniform(factory.create(type, name));
+ addUniform(factory.create(type, name));
}
else
{
- SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
- << "Type " << type << " not supported.";
+ SURGSIM_FAILURE() << "OsgUniform type " << type << " not supported.";
}
- return result;
+}
+
+void OsgMaterial::addUniform(const std::string& type, const std::string& name, const boost::any& value)
+{
+ addUniform(type, name);
+ setValue(name, value);
}
bool OsgMaterial::removeUniform(std::shared_ptr<UniformBase> uniform)
@@ -161,37 +160,37 @@ bool OsgMaterial::hasUniform(const std::string& name) const
return (getUniform(name) != nullptr);
}
-bool OsgMaterial::setShader(std::shared_ptr<Shader> shader)
+bool OsgMaterial::setProgram(std::shared_ptr<Program> program)
{
bool didSucceed = false;
- std::shared_ptr<OsgShader> osgShader = std::dynamic_pointer_cast<OsgShader>(shader);
- if (osgShader)
+ std::shared_ptr<OsgProgram> osgProgram = std::dynamic_pointer_cast<OsgProgram>(program);
+ if (osgProgram)
{
- if (m_shader)
+ if (m_program)
{
- m_shader->removeFromStateSet(m_stateSet.get());
+ m_program->removeFromStateSet(m_stateSet.get());
}
- osgShader->addToStateSet(m_stateSet);
- m_shader = osgShader;
+ osgProgram->addToStateSet(m_stateSet);
+ m_program = osgProgram;
didSucceed = true;
}
return didSucceed;
}
-std::shared_ptr<Shader> OsgMaterial::getShader() const
+std::shared_ptr<Program> OsgMaterial::getProgram() const
{
- return m_shader;
+ return m_program;
}
-void OsgMaterial::clearShader()
+void OsgMaterial::clearProgram()
{
- if (m_shader)
+ if (m_program)
{
- m_shader->removeFromStateSet(m_stateSet.get());
+ m_program->removeFromStateSet(m_stateSet.get());
}
- m_shader = nullptr;
+ m_program = nullptr;
}
bool OsgMaterial::doInitialize()
@@ -212,6 +211,42 @@ osg::ref_ptr<osg::StateSet> OsgMaterial::getOsgStateSet() const
{
return m_stateSet;
}
+
+std::shared_ptr<OsgMaterial> buildMaterial(const std::string& vertexShaderName, const std::string& fragmentShaderName)
+{
+ bool result = true;
+
+ std::shared_ptr<OsgMaterial> material;
+
+ auto program = std::make_shared<OsgProgram>();
+ std::string fileName;
+ fileName = Framework::Runtime::getApplicationData()->findFile(vertexShaderName);
+ if (!program->loadVertexShader(fileName))
+ {
+ SURGSIM_LOG_WARNING(Framework::Logger::getLogger("Graphics"))
+ << "Shader " << vertexShaderName << ", could not "
+ << ((fileName == "") ? "find shader file" : "compile " + fileName) << ".";
+ result = false;
+ }
+
+ fileName = Framework::Runtime::getApplicationData()->findFile(fragmentShaderName);
+ if (!program->loadFragmentShader(fileName))
+ {
+ SURGSIM_LOG_WARNING(Framework::Logger::getLogger("Graphics"))
+ << "Shader " << fragmentShaderName << " , could not "
+ << ((fileName == "") ? "find shader file" : "compile " + fileName) << ".";
+ result = false;
+ }
+
+ if (result)
+ {
+ material = std::make_shared<OsgMaterial>("material");
+ material->setProgram(program);
+ }
+
+ return material;
+}
+
}; // namespace Graphics
}; // namespace SurgSim
diff --git a/SurgSim/Graphics/OsgMaterial.h b/SurgSim/Graphics/OsgMaterial.h
index ddc8854..fdb9699 100644
--- a/SurgSim/Graphics/OsgMaterial.h
+++ b/SurgSim/Graphics/OsgMaterial.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,13 +16,14 @@
#ifndef SURGSIM_GRAPHICS_OSGMATERIAL_H
#define SURGSIM_GRAPHICS_OSGMATERIAL_H
-#include "SurgSim/Graphics/Material.h"
-
+#include <boost/any.hpp>
#include <osg/Material>
#include <osg/StateSet>
-
#include <set>
+#include "SurgSim/Graphics/Material.h"
+
+
namespace SurgSim
{
@@ -30,7 +31,7 @@ namespace Graphics
{
class MaterialFace;
-class OsgShader;
+class OsgProgram;
class OsgUniformBase;
/// OSG-based implementation of a graphics material.
@@ -48,46 +49,53 @@ public:
/// \post The material has no uniforms and no shader.
explicit OsgMaterial(const std::string& name);
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgMaterial);
+
/// 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;
+ void addUniform(std::shared_ptr<UniformBase> uniform) override;
- virtual bool addUniform(const std::string& type, const std::string& name) override;
+ void addUniform(const std::string& type, const std::string& name) override;
+
+ /// Adds and a uniform to this material and set its value
+ /// \param type the type of the uniform
+ /// \param name Name used in shader code to access this uniform
+ /// \param value The value for this uniform
+ void addUniform(const std::string& type, const std::string& name, const boost::any& value);
/// 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;
+ bool removeUniform(std::shared_ptr<UniformBase> uniform) override;
- virtual bool removeUniform(const std::string& name) override;
+ bool removeUniform(const std::string& name) override;
- virtual size_t getNumUniforms() const override;
+ size_t getNumUniforms() const override;
- virtual std::shared_ptr<UniformBase> getUniform(size_t index) const override;
+ std::shared_ptr<UniformBase> getUniform(size_t index) const override;
- virtual std::shared_ptr<UniformBase> getUniform(const std::string& name) const override;
+ std::shared_ptr<UniformBase> getUniform(const std::string& name) const override;
- virtual bool hasUniform(const std::string& name) const override;
+ 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;
+ /// \param program Shader program
+ /// \return True if program was set successfully, otherwise false
+ /// \note OsgMaterial only accepts subclasses of OsgProgram
+ bool setProgram(std::shared_ptr<Program> program) override;
- virtual std::shared_ptr<Shader> getShader() const override;
+ std::shared_ptr<Program> getProgram() const override;
- virtual void clearShader() override;
+ void clearProgram() override;
/// \return the OSG state set with the material properties
osg::ref_ptr<osg::StateSet> getOsgStateSet() const;
- virtual bool doInitialize() override;
+ bool doInitialize() override;
- virtual bool doWakeUp() override;
+ bool doWakeUp() override;
private:
/// OSG state set which provides material properties in the scenegraph
@@ -97,11 +105,19 @@ private:
std::vector<std::shared_ptr<OsgUniformBase>> m_uniforms;
/// Shader used by this material
- std::shared_ptr<OsgShader> m_shader;
+ std::shared_ptr<OsgProgram> m_program;
};
+/// 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 fragment shader to be used, needs to be available on the path.
+/// \return a valid material if all the shaders are found, nullptr otherwise
+std::shared_ptr<OsgMaterial> buildMaterial(
+ const std::string& vertexShaderName,
+ const std::string& fragmentShaderName);
+
}; // namespace Graphics
}; // namespace SurgSim
-#endif // SURGSIM_GRAPHICS_OSGMATERIAL_H
\ No newline at end of file
+#endif // SURGSIM_GRAPHICS_OSGMATERIAL_H
diff --git a/SurgSim/Graphics/OsgMeshRepresentation.cpp b/SurgSim/Graphics/OsgMeshRepresentation.cpp
index ceffa9e..08ddf80 100644
--- a/SurgSim/Graphics/OsgMeshRepresentation.cpp
+++ b/SurgSim/Graphics/OsgMeshRepresentation.cpp
@@ -18,18 +18,21 @@
#include <osg/Array>
#include <osg/Geode>
#include <osg/Geometry>
+#include <osg/Switch>
#include <osg/PositionAttitudeTransform>
#include <osg/Vec3f>
#include <osgUtil/SmoothingVisitor>
#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Asset.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"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/Shape.h"
namespace SurgSim
{
@@ -42,47 +45,33 @@ OsgMeshRepresentation::OsgMeshRepresentation(const std::string& name) :
OsgRepresentation(name),
MeshRepresentation(name),
m_updateOptions(UPDATE_OPTION_VERTICES),
- m_mesh(std::make_shared<Mesh>()),
- m_filename()
+ m_updateCount(0)
{
- // 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();
+ m_meshSwitch = new osg::Switch();
+ m_transform->addChild(m_meshSwitch);
- // 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);
+ setMesh(std::make_shared<Mesh>());
+}
- osg::ref_ptr<osg::Geode> geode = new osg::Geode();
- geode->addDrawable(m_geometry);
+OsgMeshRepresentation::~OsgMeshRepresentation()
+{
+}
- m_transform->addChild(geode);
+void OsgMeshRepresentation::loadMesh(const std::string& fileName)
+{
+ auto mesh = std::make_shared<Mesh>();
+ mesh->load(fileName);
+ setMesh(mesh);
}
-OsgMeshRepresentation::~OsgMeshRepresentation()
+void OsgMeshRepresentation::setMesh(std::shared_ptr<SurgSim::Framework::Asset> mesh)
{
+ auto graphicsMesh = std::dynamic_pointer_cast<Mesh>(mesh);
+ SURGSIM_ASSERT(graphicsMesh != nullptr) << "Mesh for OsgMeshRepresentation needs to be a SurgSim::Graphics::Mesh";
+ m_mesh = graphicsMesh;
+ m_updateCount = m_mesh->getUpdateCount();
+ m_mesh->dirty();
+ buildGeometry();
}
std::shared_ptr<Mesh> OsgMeshRepresentation::getMesh()
@@ -90,54 +79,74 @@ std::shared_ptr<Mesh> OsgMeshRepresentation::getMesh()
return m_mesh;
}
-void OsgMeshRepresentation::doUpdate(double dt)
+void OsgMeshRepresentation::setShape(std::shared_ptr<SurgSim::Math::Shape> shape)
{
- SURGSIM_ASSERT(m_mesh->isValid()) << "The mesh in the OsgMeshRepresentation " << getName() << " is invalid.";
-
- int updateOptions = updateOsgArrays();
- updateOptions |= m_updateOptions;
+ SURGSIM_ASSERT(shape->getType() == SurgSim::Math::SHAPE_TYPE_MESH ||
+ shape->getType() == SurgSim::Math::SHAPE_TYPE_SURFACEMESH)
+ << "Shape for OsgMeshRepresentation needs to be a SurgSim::Math::MeshShape/SurgSim::Math::SurfaceMeshShape";
+ auto meshShape = std::static_pointer_cast<SurgSim::Math::MeshShape>(shape);
+ m_mesh = std::make_shared<Mesh>(*meshShape);
+}
- if ((updateOptions & (UPDATE_OPTION_VERTICES | UPDATE_OPTION_TEXTURES | UPDATE_OPTION_COLORS)) != 0)
+void OsgMeshRepresentation::doUpdate(double dt)
+{
+ size_t updateCount = m_mesh->getUpdateCount();
+ if (m_updateCount != updateCount)
{
- updateVertices(updateOptions);
- m_geometry->dirtyDisplayList();
- m_geometry->dirtyBound();
+ // The update was done through shared data (might not be threadsafe)
+ // #threadsafety
+ m_updateCount = updateCount;
+ privateUpdateMesh(*m_mesh);
}
-
- if ((updateOptions & UPDATE_OPTION_TRIANGLES) != 0)
+ else
{
- updateTriangles();
- m_triangles->dirty();
+ // The update was done through the threadsafe Locked container
+ Mesh tempMesh;
+ if (m_writeBuffer.tryTakeChanged(&tempMesh))
+ {
+ privateUpdateMesh(tempMesh);
+ }
}
}
-bool OsgMeshRepresentation::doInitialize()
+void OsgMeshRepresentation::privateUpdateMesh(const Mesh& mesh)
{
- bool result = true;
+ SURGSIM_ASSERT(mesh.isValid()) << "The mesh in the OsgMeshRepresentation " << getName() << " is invalid.";
- if (!m_filename.empty())
+ // Early exit if there are no vertices
+ if (mesh.getNumVertices() == 0)
{
- std::string filePath = getRuntime()->getApplicationData()->findFile(m_filename);
+ m_meshSwitch->setAllChildrenOff();
+ return;
+ }
- 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;
- }
+ m_meshSwitch->setSingleChildOn(0);
+
+
+ int updateOptions = updateOsgArrays(mesh, m_geometry);
+ updateOptions |= m_updateOptions;
+
+ if ((updateOptions & UPDATE_OPTION_TRIANGLES) != 0)
+ {
+ updateTriangles(mesh, m_geometry);
}
- return result;
+ if ((updateOptions & (UPDATE_OPTION_VERTICES | UPDATE_OPTION_TEXTURES | UPDATE_OPTION_COLORS)) != 0)
+ {
+ updateVertices(mesh, m_geometry, updateOptions);
+ updateTangents();
+ m_geometry->dirtyDisplayList();
+ m_geometry->dirtyBound();
+ m_geometry->getBound();
+ }
}
-void OsgMeshRepresentation::updateVertices(int updateOptions)
+bool OsgMeshRepresentation::doInitialize()
+{
+ return true;
+}
+
+void OsgMeshRepresentation::updateVertices(const Mesh& mesh, osg::Geometry* geometry, int updateOptions)
{
static osg::Vec4d defaultColor(0.8, 0.2, 0.2, 1.0);
static osg::Vec2d defaultTextureCoord(0.0, 0.0);
@@ -145,104 +154,127 @@ void OsgMeshRepresentation::updateVertices(int updateOptions)
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)
+ auto vertices = static_cast<osg::Vec3Array*>(geometry->getVertexArray());
+ auto colors = static_cast<osg::Vec4Array*>(geometry->getColorArray());
+ auto textureCoords = static_cast<osg::Vec2Array*>(geometry->getTexCoordArray(0));
+
+ size_t index = 0;
+ for (const auto& vertex : mesh.getVertices())
{
- Mesh::VertexType vertex = m_mesh->getVertex(i);
if (updateVertices)
{
- (*m_vertices)[i].set(toOsg(vertex.position));
+ (*vertices)[index].set(toOsg(vertex.position));
}
if (updateColors)
{
- (*m_colors)[i] = (vertex.data.color.hasValue()) ? toOsg(vertex.data.color.getValue()) : defaultColor;
+ (*colors)[index] = (vertex.data.color.hasValue()) ? toOsg(vertex.data.color.getValue()) : defaultColor;
}
if (updateTextures)
{
- (*m_textureCoordinates)[i] =
+ (*textureCoords)[index] =
(vertex.data.texture.hasValue()) ? toOsg(vertex.data.texture.getValue()) : defaultTextureCoord;
}
+ ++index;
}
if (updateVertices)
{
- updateNormals();
+ updateNormals(geometry);
}
+ vertices->dirty();
}
-void OsgMeshRepresentation::updateNormals()
+void OsgMeshRepresentation::updateNormals(osg::Geometry* geometry)
{
// Generate normals from geometry
- auto normalGenerator = createNormalGenerator(m_vertices, m_normals);
- m_geometry->accept(normalGenerator);
+ auto vertices = static_cast<osg::Vec3Array*>(geometry->getVertexArray());
+ auto normals = static_cast<osg::Vec3Array*>(geometry->getNormalArray());
+ auto normalGenerator = createNormalGenerator(vertices, normals);
+ geometry->accept(normalGenerator);
normalGenerator.normalize();
+ normals->dirty();
}
-void OsgMeshRepresentation::updateTriangles()
+void OsgMeshRepresentation::updateTriangles(const Mesh& mesh, osg::Geometry* geometry)
{
- int i = 0;
- m_triangles->resize(m_mesh->getNumTriangles() * 3);
- for (auto const& triangle : m_mesh->getTriangles())
+ osg::Geometry::DrawElementsList drawElements;
+ geometry->getDrawElementsList(drawElements);
+ auto triangles = static_cast<osg::DrawElementsUInt*>(drawElements[0]);
+
+ size_t i = 0;
+ for (auto const& triangle : mesh.getTriangles())
{
if (triangle.isValid)
{
- (*m_triangles)[i++] = triangle.verticesId[0];
- (*m_triangles)[i++] = triangle.verticesId[1];
- (*m_triangles)[i++] = triangle.verticesId[2];
+ (*triangles)[i++] = triangle.verticesId[0];
+ (*triangles)[i++] = triangle.verticesId[1];
+ (*triangles)[i++] = triangle.verticesId[2];
}
}
+ triangles->dirty();
}
-int OsgMeshRepresentation::updateOsgArrays()
+int OsgMeshRepresentation::updateOsgArrays(const Mesh& mesh, osg::Geometry* geometry)
{
int result = 0;
- size_t numVertices = m_mesh->getNumVertices();
+ size_t numVertices = mesh.getNumVertices();
- if (numVertices > m_vertices->size())
+ auto vertices = static_cast<osg::Vec3Array*>(geometry->getVertexArray());
+ auto normals = static_cast<osg::Vec3Array*>(geometry->getNormalArray());
+ if (numVertices != vertices->size())
{
- m_vertices->resize(numVertices);
- m_normals->resize(numVertices);
-
- m_vertices->setDataVariance(getDataVariance(UPDATE_OPTION_VERTICES));
- m_normals->setDataVariance(getDataVariance(UPDATE_OPTION_VERTICES));
+ vertices->resize(numVertices);
+ normals->resize(numVertices);
result |= UPDATE_OPTION_VERTICES;
}
+ vertices->setDataVariance(getDataVariance(UPDATE_OPTION_VERTICES));
+ normals->setDataVariance(getDataVariance(UPDATE_OPTION_VERTICES));
// The first vertex determines what values the mesh should have ...
- Mesh::VertexType vertex = m_mesh->getVertex(0);
+ Mesh::VertexType vertex = mesh.getVertex(0);
- if (vertex.data.color.hasValue() && numVertices > m_colors->size())
+ auto colors = static_cast<osg::Vec4Array*>(geometry->getColorArray());
+ if (vertex.data.color.hasValue() && numVertices > colors->size())
{
- if (m_colors->size() > 1)
+ if (colors->size() > 1)
{
- m_colors->setDataVariance(getDataVariance(UPDATE_OPTION_COLORS));
- m_geometry->setColorArray(m_colors, osg::Array::BIND_PER_VERTEX);
+ geometry->setColorArray(colors, osg::Array::BIND_PER_VERTEX);
}
- m_colors->resize(numVertices);
+ colors->resize(numVertices);
result |= UPDATE_OPTION_COLORS;
}
+ colors->setDataVariance(getDataVariance(UPDATE_OPTION_COLORS));
- if (vertex.data.texture.hasValue() && numVertices > m_textureCoordinates->size())
+ auto textureCoords = static_cast<osg::Vec2Array*>(geometry->getTexCoordArray(0));
+ if (vertex.data.texture.hasValue())
{
- bool setTextureArray = m_textureCoordinates->size() == 0;
- m_textureCoordinates->resize(numVertices);
- if (setTextureArray)
+ if (textureCoords == nullptr)
{
- m_geometry->setTexCoordArray(0, m_textureCoordinates, osg::Array::BIND_PER_VERTEX);
- m_textureCoordinates->setDataVariance(getDataVariance(UPDATE_OPTION_TEXTURES));
+ textureCoords = new osg::Vec2Array(0);
+ geometry->setTexCoordArray(0, textureCoords, osg::Array::BIND_PER_VERTEX);
}
+ textureCoords->resize(numVertices);
result |= UPDATE_OPTION_TEXTURES;
}
+ if (textureCoords != nullptr)
+ {
+ textureCoords->setDataVariance(getDataVariance(UPDATE_OPTION_TEXTURES));
+ }
+
+ osg::Geometry::DrawElementsList drawElements;
+ geometry->getDrawElementsList(drawElements);
+ auto triangles = static_cast<osg::DrawElementsUInt*>(drawElements[0]);
- if (m_mesh->getNumTriangles() * 3 > m_triangles->size())
+ if (mesh.getNumTriangles() * 3 != triangles->size())
{
- m_triangles->resize(m_mesh->getNumTriangles() * 3);
- m_triangles->setDataVariance(getDataVariance(UPDATE_OPTION_TRIANGLES));
+ triangles->resize(mesh.getNumTriangles() * 3);
result |= UPDATE_OPTION_TRIANGLES;
}
+ triangles->setDataVariance(getDataVariance(UPDATE_OPTION_TRIANGLES));
+
return result;
}
@@ -259,25 +291,55 @@ int OsgMeshRepresentation::getUpdateOptions() const
return m_updateOptions;
}
-osg::ref_ptr<osg::Geometry> OsgMeshRepresentation::getOsgGeometry()
+osg::ref_ptr<osg::Geometry> OsgMeshRepresentation::getOsgGeometry() const
{
return m_geometry;
}
-osg::Object::DataVariance OsgMeshRepresentation::getDataVariance(int updateOption)
+void OsgMeshRepresentation::updateMesh(const SurgSim::Graphics::Mesh& mesh)
{
- return ((m_updateOptions & updateOption) != 0) ? osg::Object::DYNAMIC : osg::Object::STATIC;
+ m_writeBuffer.set(mesh);
}
-void OsgMeshRepresentation::setFilename(std::string filename)
+osg::Object::DataVariance OsgMeshRepresentation::getDataVariance(int updateOption)
{
- m_filename = filename;
+ return ((m_updateOptions & updateOption) != 0) ? osg::Object::DYNAMIC : osg::Object::STATIC;
}
-std::string OsgMeshRepresentation::getFilename() const
+void OsgMeshRepresentation::buildGeometry()
{
- return m_filename;
+ // Remove old Geometry nodes
+ m_meshSwitch->removeChildren(0, m_meshSwitch->getNumChildren());
+
+ m_geometry = new osg::Geometry;
+
+ m_geometry->setUseDisplayList(false);
+
+ // Create Standard arrays zero size, updateOsgArrays will take care of the correct sizing and
+ // setting the correct data variance on them
+
+ // Set up vertices array
+ auto vertices = new osg::Vec3Array();
+ m_geometry->setVertexArray(vertices);
+
+ // Create normals
+ auto normals = new osg::Vec3Array();
+ m_geometry->setNormalArray(normals, osg::Array::BIND_PER_VERTEX);
+
+ // Set up color array with default color
+ auto colors = new osg::Vec4Array(1);
+ (*colors)[0] = osg::Vec4(0.8f, 0.8f, 1.0f, 1.0f);
+ m_geometry->setColorArray(colors, osg::Array::BIND_OVERALL);
+
+ // Set up primitive set for triangles
+ auto triangles = new osg::DrawElementsUInt(osg::PrimitiveSet::TRIANGLES);
+ m_geometry->addPrimitiveSet(triangles);
+
+ auto geode = new osg::Geode;
+ geode->addDrawable(m_geometry);
+ m_meshSwitch->setAllChildrenOff();
+ m_meshSwitch->addChild(geode);
}
}; // Graphics
-}; // SurgSim
\ No newline at end of file
+}; // SurgSim
diff --git a/SurgSim/Graphics/OsgMeshRepresentation.h b/SurgSim/Graphics/OsgMeshRepresentation.h
index 534dd8f..00ba5c2 100644
--- a/SurgSim/Graphics/OsgMeshRepresentation.h
+++ b/SurgSim/Graphics/OsgMeshRepresentation.h
@@ -25,6 +25,7 @@
#include "SurgSim/Framework/ObjectFactory.h"
#include "SurgSim/Graphics/OsgRepresentation.h"
#include "SurgSim/Graphics/MeshRepresentation.h"
+#include "SurgSim/Framework/LockedContainer.h"
#if defined(_MSC_VER)
#pragma warning(push)
@@ -33,6 +34,7 @@
namespace osg
{
+class Geode;
class Geometry;
class DrawElementsUInt;
}
@@ -58,21 +60,28 @@ public:
SURGSIM_CLASSNAME(SurgSim::Graphics::OsgMeshRepresentation);
- virtual std::shared_ptr<Mesh> getMesh() override;
+ std::shared_ptr<Mesh> getMesh() override;
- virtual void setUpdateOptions(int val) override;
- virtual int getUpdateOptions() const override;
+ void setMesh(std::shared_ptr<SurgSim::Framework::Asset> mesh) override;
- osg::ref_ptr<osg::Geometry> getOsgGeometry();
+ void loadMesh(const std::string& fileName) override;
- virtual void setFilename(std::string filename) override;
- virtual std::string getFilename() const override;
+ void setShape(std::shared_ptr<SurgSim::Math::Shape> shape) override;
+
+ void setUpdateOptions(int val) override;
+ int getUpdateOptions() const override;
+
+ osg::ref_ptr<osg::Geometry> getOsgGeometry() const;
+
+ void updateMesh(const SurgSim::Graphics::Mesh& mesh) override;
protected:
- virtual void doUpdate(double dt) override;
+ 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;
+ bool doInitialize() override;
+
+ void privateUpdateMesh(const SurgSim::Graphics::Mesh& mesh);
private:
/// Indicates which elements of the mesh should be updated on every frame
@@ -86,34 +95,46 @@ private:
///@{
/// Osg structures
+ osg::ref_ptr<osg::Switch> m_meshSwitch;
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
+ /// \param mesh The mesh used to update
+ /// \param geometry [out] The geometry that carries the data
/// \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();
+ int updateOsgArrays(const Mesh& mesh, osg::Geometry* geometry);
/// 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 mesh The mesh used to update
+ /// \param geometry [out] The geometry that carries the data
/// \param updateOptions Set of flags indicating whether a specific vertex attribute should be updated
- void updateVertices(int updateOptions);
+ void updateVertices(const Mesh& mesh, osg::Geometry* geometry, int updateOptions);
/// Updates the normals.
- void updateNormals();
+ /// \param geometry [out] The geometry that carries the data
+ void updateNormals(osg::Geometry* geometry);
/// Updates the triangles.
- void updateTriangles();
+ /// \param mesh The mesh used to update
+ /// \param geometry [out] The geometry that carries the data
+ void updateTriangles(const Mesh& mesh, osg::Geometry* geometry);
/// Gets data variance for a given update option.
/// \param updateOption The update option.
/// \return The data variance.
osg::Object::DataVariance getDataVariance(int updateOption);
+
+ /// Create the appropriate geometry nodes
+ void buildGeometry();
+
+ /// Cache for the update count pull from the mesh
+ size_t m_updateCount;
+
+ Framework::LockedContainer<Mesh> m_writeBuffer;
+
};
#if defined(_MSC_VER)
@@ -123,4 +144,4 @@ private:
}; // Graphics
}; // SurgSim
-#endif // SURGSIM_GRAPHICS_OSGMESHREPRESENTATION_H
\ No newline at end of file
+#endif // SURGSIM_GRAPHICS_OSGMESHREPRESENTATION_H
diff --git a/SurgSim/Graphics/OsgModel.cpp b/SurgSim/Graphics/OsgModel.cpp
new file mode 100644
index 0000000..0d5b0c1
--- /dev/null
+++ b/SurgSim/Graphics/OsgModel.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/Graphics/OsgModel.h"
+#include "SurgSim/Framework/Log.h"
+
+#include <osgDB/ReadFile>
+
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Asset, SurgSim::Graphics::OsgModel, OsgModel);
+
+OsgModel::OsgModel()
+{
+}
+
+OsgModel::~OsgModel()
+{
+
+}
+
+osg::ref_ptr<osg::Node> OsgModel::getOsgNode()
+{
+ return m_root;
+}
+
+bool OsgModel::doLoad(const std::string& filePath)
+{
+ m_root = osgDB::readNodeFile(filePath);
+ SURGSIM_ASSERT(m_root.valid()) << "Could not load file " << filePath << std::endl;
+ return true;
+}
+
+
+}
+}
+
diff --git a/SurgSim/Graphics/OsgModel.h b/SurgSim/Graphics/OsgModel.h
new file mode 100644
index 0000000..22f4fd3
--- /dev/null
+++ b/SurgSim/Graphics/OsgModel.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_OSGMODEL_H
+#define SURGSIM_GRAPHICS_OSGMODEL_H
+
+
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Graphics/Model.h"
+#include <osg/ref_ptr>
+
+namespace osg
+{
+class Node;
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// Osg implementation of the Model class, inheriting from Asset, this class knows how to load models that can be
+/// handled by osg.
+class OsgModel : public Model
+{
+public:
+
+ /// Constructor
+ OsgModel();
+
+ virtual ~OsgModel();
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgModel);
+
+ /// \return the Node that is the root of the loaded model, nullptr if no model is loaded
+ osg::ref_ptr<osg::Node> getOsgNode();
+
+private:
+
+ bool doLoad(const std::string& filePath) override;
+
+ osg::ref_ptr<osg::Node> m_root;
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Graphics/OsgOctreeRepresentation.cpp b/SurgSim/Graphics/OsgOctreeRepresentation.cpp
index eb28d6f..4192140 100644
--- a/SurgSim/Graphics/OsgOctreeRepresentation.cpp
+++ b/SurgSim/Graphics/OsgOctreeRepresentation.cpp
@@ -61,7 +61,7 @@ void OsgOctreeRepresentation::doUpdate(double dt)
|0__|__4|/
*/
void OsgOctreeRepresentation::buildOctree(osg::ref_ptr<osg::PositionAttitudeTransform> transformNode,
- std::shared_ptr<SurgSim::Math::OctreeShape::NodeType> octree)
+ 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();
@@ -70,7 +70,7 @@ void OsgOctreeRepresentation::buildOctree(osg::ref_ptr<osg::PositionAttitudeTran
if (octree->hasChildren())
{
auto octreeChildren = octree->getChildren();
- for(int i = 0; i < 8; ++i)
+ for (int i = 0; i < 8; ++i)
{
buildOctree(osgTransform, octreeChildren[i]);
}
@@ -94,7 +94,7 @@ void OsgOctreeRepresentation::setOctreeShape(const std::shared_ptr<SurgSim::Math
SURGSIM_ASSERT(octreeShape != nullptr) << "OsgOctreeRepresentation can only accept an OctreeShape.";
m_octreeShape = octreeShape;
- buildOctree(m_transform, m_octreeShape->getRootNode());
+ buildOctree(m_transform, m_octreeShape->getOctree());
}
std::shared_ptr<SurgSim::Math::OctreeShape> OsgOctreeRepresentation::getOctreeShape() const
@@ -107,10 +107,10 @@ void OsgOctreeRepresentation::setNodeVisible(const SurgSim::DataStructures::Octr
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)
+ for (auto index = std::begin(path); index != std::end(path); ++index)
{
SURGSIM_ASSERT(result->getNumChildren() > 1) <<
- "OsgOctreeRepresentation::setNodeVisible(): Invalid OctreePath";
+ "OsgOctreeRepresentation::setNodeVisible(): Invalid OctreePath";
result = result->getChild(*index)->asGroup();
}
diff --git a/SurgSim/Graphics/OsgOctreeRepresentation.h b/SurgSim/Graphics/OsgOctreeRepresentation.h
index 9a1e27e..435469b 100644
--- a/SurgSim/Graphics/OsgOctreeRepresentation.h
+++ b/SurgSim/Graphics/OsgOctreeRepresentation.h
@@ -59,15 +59,15 @@ public:
/// Executes the update operation
/// \param dt The time step
- virtual void doUpdate(double dt) override;
+ 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;
+ void setOctreeShape(const std::shared_ptr<SurgSim::Math::Shape>& shape) override;
+ 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;
+ void setNodeVisible(const SurgSim::DataStructures::OctreePath& path, bool visibility) override;
private:
/// Draw the Octree associated with this OSG representation.
diff --git a/SurgSim/Graphics/OsgPlane.h b/SurgSim/Graphics/OsgPlane.h
index 99296a5..2a4803a 100644
--- a/SurgSim/Graphics/OsgPlane.h
+++ b/SurgSim/Graphics/OsgPlane.h
@@ -36,7 +36,7 @@ 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) :
+ explicit OsgPlane(float length = 1000.0f, float width = 1000.0f) :
m_geode(new osg::Geode())
{
osg::ref_ptr<osg::Geometry> plane = osg::createTexturedQuadGeometry(
diff --git a/SurgSim/Graphics/OsgPointCloudRepresentation.cpp b/SurgSim/Graphics/OsgPointCloudRepresentation.cpp
index 4f33107..9b9bc14 100644
--- a/SurgSim/Graphics/OsgPointCloudRepresentation.cpp
+++ b/SurgSim/Graphics/OsgPointCloudRepresentation.cpp
@@ -47,15 +47,14 @@ OsgPointCloudRepresentation::OsgPointCloudRepresentation(const std::string& name
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_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 );
+ m_geometry->getOrCreateStateSet()->setAttribute(m_point, osg::StateAttribute::ON);
geode->addDrawable(m_geometry);
m_transform->addChild(geode);
@@ -68,27 +67,48 @@ OsgPointCloudRepresentation::~OsgPointCloudRepresentation()
void OsgPointCloudRepresentation::doUpdate(double dt)
{
- auto vertices = m_vertices->getVertices();
+ DataStructures::VerticesPlain vertices;
+
+ // #performance
+ // This is an intermediary step, it keeps the old non-threadsafe interface intact but also supports the
+ // threadsafe update (btw, this is not any worse than what we did before) once we deprecate the non-threadsafe
+ // access to the shared pointer we can remove the else branch
+ // HS-2015-08-11
+ if (m_locker.tryTakeChanged(&vertices))
+ {
+ updateGeometry(vertices);
+ }
+ else
+ {
+ updateGeometry(*m_vertices);
+ }
+}
+
+void OsgPointCloudRepresentation::updateGeometry(const DataStructures::VerticesPlain& vertexData)
+{
+ auto& vertices = vertexData.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())
+ if (count != m_vertexData->size())
{
m_vertexData->resize(count);
}
- m_drawArrays->set(osg::PrimitiveSet::POINTS,0,count);
+ m_drawArrays->set(osg::PrimitiveSet::POINTS, 0, count);
m_drawArrays->dirty();
}
+ // #performance
+ // Calculate the bounding box while iterating over the vertices, this will save osg time in the update traversal
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]);
+ const auto& vertex = vertices[i];
+ (*m_vertexData)[i][0] = static_cast<float>(vertex.position[0]);
+ (*m_vertexData)[i][1] = static_cast<float>(vertex.position[1]);
+ (*m_vertexData)[i][2] = static_cast<float>(vertex.position[2]);
}
m_geometry->dirtyBound();
diff --git a/SurgSim/Graphics/OsgPointCloudRepresentation.h b/SurgSim/Graphics/OsgPointCloudRepresentation.h
index 0f067a4..1340142 100644
--- a/SurgSim/Graphics/OsgPointCloudRepresentation.h
+++ b/SurgSim/Graphics/OsgPointCloudRepresentation.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -57,31 +57,20 @@ public:
SURGSIM_CLASSNAME(SurgSim::Graphics::OsgPointCloudRepresentation);
- /// Gets the vertices.
- /// \return The vertices.
- virtual std::shared_ptr<PointCloud> getVertices() const override;
+ std::shared_ptr<PointCloud> getVertices() const override;
- /// Sets point size.
- /// \param val The value.
- virtual void setPointSize(double val) override;
+ void setPointSize(double val) override;
- /// Gets point size.
- /// \return The point size.
- virtual double getPointSize() const override;
+ double getPointSize() const override;
- /// Executes the update operation.
- /// \param dt The dt.
- virtual void doUpdate(double dt) override;
+ void doUpdate(double dt) override;
- /// Sets a color.
- /// \param color The color.
- virtual void setColor(const SurgSim::Math::Vector4d& color) override;
+ void setColor(const SurgSim::Math::Vector4d& color) override;
- /// Gets the color.
- /// \return The current color.
- virtual SurgSim::Math::Vector4d getColor() const override;
+ SurgSim::Math::Vector4d getColor() const override;
private:
+
/// Local pointer to vertices with data
std::shared_ptr<PointCloud> m_vertices;
@@ -99,6 +88,10 @@ private:
/// Color backing variable
SurgSim::Math::Vector4d m_color;
+
+ /// Update the geometry
+ /// \param vertices new vertices
+ void updateGeometry(const DataStructures::VerticesPlain& vertices);
};
#if defined(_MSC_VER)
diff --git a/SurgSim/Graphics/OsgProgram.cpp b/SurgSim/Graphics/OsgProgram.cpp
new file mode 100644
index 0000000..c5c5842
--- /dev/null
+++ b/SurgSim/Graphics/OsgProgram.cpp
@@ -0,0 +1,266 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/OsgProgram.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Log.h"
+
+namespace
+{
+static const osg::Shader::Type OsgShaderTypes[3] = {osg::Shader::VERTEX, osg::Shader::FRAGMENT, osg::Shader::GEOMETRY};
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+OsgProgram::OsgProgram() : Program(),
+ m_program(new osg::Program()),
+ m_globalScope(false)
+{
+
+}
+
+void OsgProgram::addToStateSet(osg::StateSet* stateSet)
+{
+ if (stateSet != nullptr)
+ {
+ int attribute = osg::StateAttribute::ON | ((m_globalScope) ? osg::StateAttribute::OVERRIDE : 0);
+ stateSet->setAttributeAndModes(m_program, attribute);
+ }
+}
+
+void OsgProgram::removeFromStateSet(osg::StateSet* stateSet)
+{
+ if (stateSet != nullptr)
+ {
+ stateSet->removeAttribute(m_program);
+ }
+}
+
+bool OsgProgram:: hasVertexShader() const
+{
+ return hasShader(SHADER_TYPE_VERTEX);
+}
+
+void OsgProgram::clearVertexShader()
+{
+ clearShader(SHADER_TYPE_VERTEX);
+}
+
+bool OsgProgram::loadVertexShader(const std::string& filePath)
+{
+ return loadShaderSource(filePath, SHADER_TYPE_VERTEX);
+}
+
+void OsgProgram::setVertexShaderSource(const std::string& source)
+{
+ setShaderSource(source, SHADER_TYPE_VERTEX);
+}
+
+bool OsgProgram::getVertexShaderSource(std::string* source) const
+{
+ return getShaderSource(SHADER_TYPE_VERTEX, source);
+}
+
+bool OsgProgram::hasGeometryShader() const
+{
+ return hasShader(SHADER_TYPE_GEOMETRY);
+}
+
+void OsgProgram::clearGeometryShader()
+{
+ return clearShader(SHADER_TYPE_GEOMETRY);
+}
+
+bool OsgProgram::loadGeometryShader(const std::string& filePath)
+{
+ return loadShaderSource(filePath, SHADER_TYPE_GEOMETRY);
+}
+
+void OsgProgram::setGeometryShaderSource(const std::string& source)
+{
+ return setShaderSource(source, SHADER_TYPE_GEOMETRY);
+}
+
+bool OsgProgram::getGeometryShaderSource(std::string* source) const
+{
+ return getShaderSource(SHADER_TYPE_GEOMETRY, source);
+}
+
+bool OsgProgram::hasFragmentShader() const
+{
+ return hasShader(SHADER_TYPE_FRAGMENT);
+}
+
+void OsgProgram::clearFragmentShader()
+{
+ clearShader(SHADER_TYPE_FRAGMENT);
+}
+
+bool OsgProgram::loadFragmentShader(const std::string& filePath)
+{
+ return loadShaderSource(filePath, SHADER_TYPE_FRAGMENT);
+}
+
+void OsgProgram::setFragmentShaderSource(const std::string& source)
+{
+ setShaderSource(source, SHADER_TYPE_FRAGMENT);
+}
+
+bool OsgProgram::getFragmentShaderSource(std::string* source) const
+{
+ return getShaderSource(SHADER_TYPE_FRAGMENT, source);
+}
+
+osg::ref_ptr<osg::Program> OsgProgram::getOsgProgram() const
+{
+ return m_program;
+}
+
+void OsgProgram::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 OsgProgram::isGlobalScope() const
+{
+ return m_globalScope;
+}
+
+bool OsgProgram::hasShader(int shaderType) const
+{
+ bool result = true;
+ if (shaderType < SHADER_TYPE_COUNT)
+ {
+ result = m_osgShaders[shaderType].valid();
+ }
+ return result;
+}
+
+void OsgProgram::clearShader(int shaderType)
+{
+ if (hasShader(shaderType))
+ {
+ m_program->removeShader(m_osgShaders[shaderType]);
+ m_osgShaders[shaderType] = nullptr;
+ }
+}
+
+bool OsgProgram::loadShaderSource(const std::string& filePath, int shaderType)
+{
+ bool result = false;
+ auto shader = getOrCreateOsgShader(shaderType);
+ if (shader->loadShaderSourceFromFile(filePath))
+ {
+ shader->setName(filePath);
+ result = true;
+ }
+
+ return result;
+}
+
+void OsgProgram::setShaderSource(const std::string& source, int shaderType)
+{
+ auto shader = getOrCreateOsgShader(shaderType);
+ shader->setShaderSource(source);
+}
+
+bool OsgProgram::getShaderSource(int shaderType, std::string* source) const
+{
+ if (hasShader(shaderType))
+ {
+ *source = m_osgShaders[shaderType]->getShaderSource();
+ return true;
+ }
+ else
+ {
+ *source = "";
+ return false;
+ }
+}
+
+osg::ref_ptr<osg::Shader> OsgProgram::getOrCreateOsgShader(int shaderType)
+{
+ osg::ref_ptr<osg::Shader> result;
+ if (!hasShader(shaderType))
+ {
+ result = new osg::Shader(OsgShaderTypes[shaderType]);
+ m_program->addShader(result);
+ m_osgShaders[shaderType] = result;
+ }
+ else
+ {
+ result = m_osgShaders[shaderType];
+ }
+ return result;
+}
+
+std::shared_ptr<OsgProgram> loadProgram(const SurgSim::Framework::ApplicationData& data, const std::string& name)
+{
+ return loadProgram(data, name + ".vert", name + ".frag");
+}
+
+std::shared_ptr<OsgProgram> loadProgram(const SurgSim::Framework::ApplicationData& data,
+ const std::string& vertexShaderName, const std::string& fragmentShaderName)
+{
+ std::string filename;
+
+ auto program(std::make_shared<SurgSim::Graphics::OsgProgram>());
+ 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 (!program->loadVertexShader(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 (!program->loadFragmentShader(filename))
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Could not load fragment shader " << fragmentShaderName;
+ success = false;
+ }
+
+ if (!success)
+ {
+ program = nullptr;
+ }
+
+ return program;
+}
+
+}
+}
diff --git a/SurgSim/Graphics/OsgProgram.h b/SurgSim/Graphics/OsgProgram.h
new file mode 100644
index 0000000..1646fba
--- /dev/null
+++ b/SurgSim/Graphics/OsgProgram.h
@@ -0,0 +1,164 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_OSGPROGRAM_H
+#define SURGSIM_GRAPHICS_OSGPROGRAM_H
+
+#include "SurgSim/Graphics/Program.h"
+
+#include <osg/Program>
+#include <osg/StateSet>
+
+#include <string>
+#include <memory>
+#include <array>
+
+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 OsgProgram : public Program
+{
+public:
+ /// Constructor
+ /// \post No shader code is set, so the fixed-function pipeline is used.
+ OsgProgram();
+
+ bool hasVertexShader() const override;
+
+ void clearVertexShader() override;
+
+ bool loadVertexShader(const std::string& filePath) override;
+
+ void setVertexShaderSource(const std::string& source) override;
+
+ bool getVertexShaderSource(std::string* source) const override;
+
+ bool hasGeometryShader() const override;
+
+ void clearGeometryShader() override;
+
+ bool loadGeometryShader(const std::string& filePath) override;
+
+ void setGeometryShaderSource(const std::string& source) override;
+
+ bool getGeometryShaderSource(std::string* source) const override;
+
+ bool hasFragmentShader() const override;
+
+ void clearFragmentShader() override;
+
+ bool loadFragmentShader(const std::string& filePath) override;
+
+ void setFragmentShaderSource(const std::string& source) override;
+
+ bool getFragmentShaderSource(std::string* source) const override;
+
+ void setGlobalScope(bool val) override;
+
+ bool isGlobalScope() const override;
+
+ /// \return the OSG program attribute
+ osg::ref_ptr<osg::Program> getOsgProgram() const;
+
+ /// Adds this shader to the OSG state set
+ /// \param stateSet OSG state set
+ void addToStateSet(osg::StateSet* stateSet);
+
+ /// Removes this uniform from the OSG state set
+ /// \param stateSet OSG state set
+ void removeFromStateSet(osg::StateSet* stateSet);
+
+private:
+ /// OSG program attribute
+ osg::ref_ptr<osg::Program> m_program;
+
+ // Type of shader, internal use only
+ enum ShaderType
+ {
+ SHADER_TYPE_VERTEX = 0,
+ SHADER_TYPE_FRAGMENT,
+ SHADER_TYPE_GEOMETRY,
+ SHADER_TYPE_COUNT
+ };
+
+ /// Storage of the osg objects
+ std::array<osg::ref_ptr<osg::Shader>, SHADER_TYPE_COUNT> m_osgShaders;
+
+ /// Check whether there is a shader in use for the given type
+ /// \param shaderType Type of the shader
+ /// \return true if the shader has been set, otherwise false.
+ bool hasShader(int shaderType) const;
+
+ /// Removes the geometry shader, returning that portion of the shader program to fixed-function.
+ /// \param shaderType Type of the shader
+ void clearShader(int shaderType);
+
+ /// Loads the shader source code from a file
+ /// \param filePath Path to file containing shader source code
+ /// \param shaderType Type of the shader
+ /// \return True if the source is successfully loaded, otherwise false.
+ bool loadShaderSource(const std::string& filePath, int shaderType);
+
+ /// Set the shader source code
+ /// \param source Shader source code
+ /// \param shaderType Type of the shader
+ virtual void setShaderSource(const std::string& source, int shaderType);
+
+ /// Gets the shader source code
+ /// \return Shader source code
+ virtual bool getShaderSource(int shaderType, std::string* source) const;
+
+ /// Fetches the appropriate shader if it exists, creates it otherwise
+ /// \param shaderType Type of the shader
+ /// \return the shader with the given type
+ osg::ref_ptr<osg::Shader> getOrCreateOsgShader(int shaderType);
+
+ /// Is the shader supposed to be used globally
+ bool m_globalScope;
+
+};
+
+/// Utility function, load a program from a set of shader files
+/// \param data Application data object
+/// \param name the base name of the shader files to be used '.vert' and '.frag' will be added automatically
+/// \return a valid program if all the shaders are found, nullptr otherwise
+std::shared_ptr<SurgSim::Graphics::OsgProgram> loadProgram(const SurgSim::Framework::ApplicationData& data,
+ const std::string& name);
+
+/// Utility function, load a program from a set of shader files
+/// \param data Application data object
+/// \param vertexShaderName name of the vertex shader to be used
+/// \param fragmentShaderName name of the fragment shader to be used
+/// \return a valid program if all the shaders are found, nullptr otherwise
+std::shared_ptr<SurgSim::Graphics::OsgProgram> loadProgram(const SurgSim::Framework::ApplicationData& data,
+ const std::string& vertexShaderName, const std::string& fragmentShaderName);
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_OSGPROGRAM_H
diff --git a/SurgSim/Graphics/OsgRenderTarget-inl.h b/SurgSim/Graphics/OsgRenderTarget-inl.h
index 67bb0be..84ae32d 100644
--- a/SurgSim/Graphics/OsgRenderTarget-inl.h
+++ b/SurgSim/Graphics/OsgRenderTarget-inl.h
@@ -36,11 +36,11 @@ OsgRenderTarget<T>::OsgRenderTarget() :
template <class T>
OsgRenderTarget<T>::OsgRenderTarget(
- int width,
- int height,
- double scale,
- int colorCount,
- bool useDepth) :
+ int width,
+ int height,
+ double scale,
+ int colorCount,
+ bool useDepth) :
m_width(width * scale),
m_height(height * scale),
m_colorTargetCount(0),
@@ -73,9 +73,9 @@ int OsgRenderTarget<T>::setColorTargetCount(int count)
// 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)
+ for (int i = m_colorTargetCount; i < result; ++i)
{
- setupTexture(TARGETTYPE_COLORBASE+i);
+ setupTexture(TARGETTYPE_COLORBASE + i);
}
m_colorTargetCount = result;
return result;
@@ -154,8 +154,8 @@ void OsgRenderTarget<T>::setupTexture(int type)
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->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)
@@ -163,8 +163,9 @@ void OsgRenderTarget<T>::setupTexture(int type)
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);
+ osgTexture->setWrap(osg::Texture::WRAP_S, osg::Texture::CLAMP_TO_BORDER);
+ osgTexture->setWrap(osg::Texture::WRAP_T, osg::Texture::CLAMP_TO_BORDER);
+ osgTexture->setBorderColor(osg::Vec4(1.0, 1.0, 1.0, 1.0));
}
if (type >= TARGETTYPE_COLORBASE)
{
diff --git a/SurgSim/Graphics/OsgRenderTarget.h b/SurgSim/Graphics/OsgRenderTarget.h
index db69e93..73171cc 100644
--- a/SurgSim/Graphics/OsgRenderTarget.h
+++ b/SurgSim/Graphics/OsgRenderTarget.h
@@ -71,15 +71,15 @@ public:
/// Gets a size.
/// \param [out] width, height The width and height of the RenderTarget textures.
- virtual void getSize(int* width, int* height) const override;
+ void getSize(int* width, int* height) const override;
/// \return The number of color targets that are available.
- virtual int getColorTargetCount() const override;
+ 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;
+ 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.
@@ -88,11 +88,11 @@ public:
/// Determines if RenderTarget does use a depth target.
/// \return true if it does.
- virtual bool doesUseDepthTarget() const override;
+ bool doesUseDepthTarget() const override;
/// Generic accessor for the depth Target.
/// \return The depth target.
- virtual std::shared_ptr<Texture> getDepthTarget() const override;
+ std::shared_ptr<Texture> getDepthTarget() const override;
/// Accessor for the depth target as an OsgTexture.
/// \return The depth target as an osg specific class.
diff --git a/SurgSim/Graphics/OsgRepresentation.cpp b/SurgSim/Graphics/OsgRepresentation.cpp
index 3fab621..aa12a0e 100644
--- a/SurgSim/Graphics/OsgRepresentation.cpp
+++ b/SurgSim/Graphics/OsgRepresentation.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,10 +16,9 @@
#include "SurgSim/Graphics/OsgRepresentation.h"
#include <algorithm>
-
#include <boost/thread/locks.hpp>
-
#include <osg/Geode>
+#include <osg/Group>
#include <osg/Switch>
#include <osg/PolygonMode>
#include <osg/PositionAttitudeTransform>
@@ -28,6 +27,9 @@
#include "SurgSim/Graphics/OsgMaterial.h"
#include "SurgSim/Graphics/OsgRigidTransformConversions.h"
#include "SurgSim/Graphics/OsgUnitBox.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/OsgUniformFactory.h"
+#include "SurgSim/Graphics/TangentSpaceGenerator.h"
namespace SurgSim
{
@@ -36,15 +38,20 @@ namespace Graphics
OsgRepresentation::OsgRepresentation(const std::string& name) :
Representation(name),
- m_drawAsWireFrame(false)
+ m_drawAsWireFrame(false),
+ m_modelMatrixUniform(std::make_shared<OsgUniform<SurgSim::Math::Matrix44f>>("modelMatrix"))
{
m_switch = new osg::Switch;
m_switch->setName(name + " Representation Switch");
+ m_modelMatrixUniform->addToStateSet(m_switch->getOrCreateStateSet());
+
+ m_materialProxy = new osg::Group;
+ m_switch->addChild(m_materialProxy);
m_transform = new osg::PositionAttitudeTransform();
m_transform->setName(name + " Transform");
- m_switch->addChild(m_transform);
+ m_materialProxy->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));
@@ -55,33 +62,32 @@ OsgRepresentation::~OsgRepresentation()
}
-void OsgRepresentation::setVisible(bool visible)
+void OsgRepresentation::update(double dt)
{
- m_switch->setChildValue(m_transform, isActive() && visible);
- m_isVisible = visible;
-}
-bool OsgRepresentation::isVisible() const
-{
- return m_switch->getChildValue(m_transform);
-}
+ if (isActive())
+ {
+ std::pair<osg::Quat, osg::Vec3d> pose = toOsg(getPose());
+ m_transform->setAttitude(pose.first);
+ m_transform->setPosition(pose.second);
+ doUpdate(dt);
+ setVisible(true);
+ }
+ else
+ {
+ setVisible(false);
+ }
-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 OsgRepresentation::setMaterial(std::shared_ptr<SurgSim::Framework::Component> material)
{
bool didSucceed = false;
std::shared_ptr<OsgMaterial> osgMaterial = std::dynamic_pointer_cast<OsgMaterial>(material);
if (osgMaterial != nullptr)
{
- m_transform->setStateSet(osgMaterial->getOsgStateSet());
+ m_materialProxy->setStateSet(osgMaterial->getOsgStateSet());
didSucceed = true;
m_material = osgMaterial;
}
@@ -95,7 +101,7 @@ std::shared_ptr<Material> OsgRepresentation::getMaterial() const
void OsgRepresentation::clearMaterial()
{
- m_transform->setStateSet(new osg::StateSet()); // Reset to empty state set
+ m_materialProxy->setStateSet(new osg::StateSet()); // Reset to empty state set
m_material = nullptr;
}
@@ -117,7 +123,7 @@ void OsgRepresentation::setDrawAsWireFrame(bool val)
osg::ref_ptr<osg::PolygonMode> polygonMode;
if (val)
{
- polygonMode = new osg::PolygonMode(osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::LINE);
+ polygonMode = new osg::PolygonMode(osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::LINE);
}
else
{
@@ -132,7 +138,62 @@ bool OsgRepresentation::getDrawAsWireFrame() const
return m_drawAsWireFrame;
}
+void OsgRepresentation::setVisible(bool val)
+{
+ m_switch->setChildValue(m_materialProxy, val);
+}
+
+
+
+void OsgRepresentation::setGenerateTangents(bool value)
+{
+ if (value && m_tangentGenerator == nullptr)
+ {
+ m_tangentGenerator =
+ new TangentSpaceGenerator(DIFFUSE_TEXTURE_UNIT, TANGENT_VERTEX_ATTRIBUTE_ID, BITANGENT_VERTEX_ATTRIBUTE_ID);
+ m_tangentGenerator->setBasisOrthonormality(true);
+ }
+ if (!value)
+ {
+ m_tangentGenerator = nullptr;
+ }
+}
+
+bool OsgRepresentation::isGeneratingTangents() const
+{
+ return m_tangentGenerator != nullptr;
+}
+
+void OsgRepresentation::updateTangents()
+{
+ if (m_tangentGenerator != nullptr)
+ {
+ m_switch->accept(*m_tangentGenerator);
+ }
+}
+
+void OsgRepresentation::addUniform(std::shared_ptr<UniformBase> uniform)
+{
+ auto osgUniform = std::dynamic_pointer_cast<OsgUniformBase>(uniform);
+ SURGSIM_ASSERT(osgUniform != nullptr) << "Uniform must be an OsgUniform";
+ m_transform->getOrCreateStateSet()->addUniform(osgUniform->getOsgUniform());
+}
+
+void OsgRepresentation::addUniform(const std::string& type, const std::string& name, const boost::any& value)
+{
+ static OsgUniformFactory factory;
+ if (factory.isRegistered(type))
+ {
+ auto uniform = factory.create(type, name);
+ uniform->setValue("Value", value);
+ addUniform(uniform);
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "OsgUniform type " << type << " not supported.";
+ }
+}
}; // Graphics
diff --git a/SurgSim/Graphics/OsgRepresentation.h b/SurgSim/Graphics/OsgRepresentation.h
index e4506a9..ed7cefe 100644
--- a/SurgSim/Graphics/OsgRepresentation.h
+++ b/SurgSim/Graphics/OsgRepresentation.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -21,9 +21,11 @@
#include <osg/ref_ptr>
#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Graphics/OsgUniform.h"
namespace osg
{
+class Group;
class Node;
class PositionAttitudeTransform;
class Switch;
@@ -35,6 +37,17 @@ namespace Graphics
{
class OsgMaterial;
+class TangentSpaceGenerator;
+class UniformBase;
+
+/// OSS default value for opengl values that have to be assigned a fixed number
+///@{
+static const int TANGENT_VERTEX_ATTRIBUTE_ID = 6;
+static const int BITANGENT_VERTEX_ATTRIBUTE_ID = 7;
+static const int DIFFUSE_TEXTURE_UNIT = 0;
+static const int NORMAL_TEXTURE_UNIT = 1;
+static const int SHADOW_TEXTURE_UNIT = 8;
+///@}
/// Base OSG implementation of a graphics representation.
///
@@ -50,51 +63,83 @@ public:
/// 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;
+ bool setMaterial(std::shared_ptr<SurgSim::Framework::Component> material) override;
/// Gets the material that defines the visual appearance of the representation
/// \return Graphics material
- virtual std::shared_ptr<Material> getMaterial() const override;
+ std::shared_ptr<Material> getMaterial() const override;
/// Removes the material from the representation
- virtual void clearMaterial() override;
+ void clearMaterial() override;
+
+ void setDrawAsWireFrame(bool val) override;
+
+ bool getDrawAsWireFrame() const override;
- virtual void setDrawAsWireFrame(bool val) override;
- virtual bool getDrawAsWireFrame() const override;
+ /// Enable the generation of tangents
+ /// When enabled it is up to the subclasses responsibility to react to changes and trigger the regeneration of
+ /// Tangents. Tangents will be stored for every geometry node that contains a vertex, normal and texture array.
+ /// \note the \sa TangentSpaceGenerator is used to create the appropriate vertex attribute arrays.
+ /// These are stored as vertex attribute arrays at the indices indicated by TANGENT_ARRAY_ATTRIBUTE_ID,
+ /// and BITANGENT_ARRAY_ATTRIBUTE_ID with the format osg::ArrayVec4.
+ /// The tangents will be made orthonormal by default.
+ /// tangents via \sa updateTangents()
+ void setGenerateTangents(bool value) override;
+
+ bool isGeneratingTangents() const override;
/// Updates the representation.
/// \param dt The time in seconds of the preceding timestep.
- virtual void update(double dt) override;
+ void update(double dt) override;
+
+ /// Adds a uniform to this representation.
+ /// \param uniform Uniform to add.
+ void addUniform(std::shared_ptr<SurgSim::Graphics::UniformBase> uniform);
+
+ /// Adds and a uniform to this representation and set its value
+ /// \param type the type of the uniform
+ /// \param name Name used in shader code to access this uniform
+ /// \param value The value for this uniform
+ void addUniform(const std::string& type, const std::string& name, const boost::any& value);
protected:
virtual void doUpdate(double dt);
+ /// Set the visibility of this representation
+ /// \param val The visibility
+ void setVisible(bool val);
+
+ /// Causes the tangents to be recalculated if tangent generation is enabled
+ /// Subclasses should call this whenever the state changes in a way that need tangents to be recalculated
+ void updateTangents();
+
/// 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;
+ /// Holder for attributes coming from OsgMaterial
+ osg::ref_ptr<osg::Group> m_materialProxy;
+
/// Material defining the visual appearance of the representation
std::shared_ptr<OsgMaterial> m_material;
+ /// Visitor to generate tangents
+ osg::ref_ptr<TangentSpaceGenerator> m_tangentGenerator;
+
/// Indicates if the representation is rendered as a wireframe.
bool m_drawAsWireFrame;
+
+ std::shared_ptr<OsgUniform<SurgSim::Math::Matrix44f>> m_modelMatrixUniform;
+
};
}; // Graphics
}; // SurgSim
-#endif // SURGSIM_GRAPHICS_OSGREPRESENTATION_H
\ No newline at end of file
+#endif // SURGSIM_GRAPHICS_OSGREPRESENTATION_H
diff --git a/SurgSim/Graphics/OsgSceneryRepresentation.cpp b/SurgSim/Graphics/OsgSceneryRepresentation.cpp
index f6d4ec1..8884de8 100644
--- a/SurgSim/Graphics/OsgSceneryRepresentation.cpp
+++ b/SurgSim/Graphics/OsgSceneryRepresentation.cpp
@@ -16,9 +16,11 @@
#include <osg/PositionAttitudeTransform>
#include <osgDB/ReadFile>
+#include "SurgSim/Framework/Assert.h"
#include "SurgSim/Framework/ApplicationData.h"
#include "SurgSim/Framework/Runtime.h"
#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
+#include "SurgSim/Graphics/OsgModel.h"
namespace SurgSim
{
@@ -31,36 +33,62 @@ OsgSceneryRepresentation::OsgSceneryRepresentation(const std::string& name) :
Representation(name),
OsgRepresentation(name),
SceneryRepresentation(name),
- m_sceneryRepresentation(nullptr),
+ m_osgNode(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();
+ return true;
+}
- std::string objectPath = applicationData->findFile(m_fileName);
- SURGSIM_ASSERT(!objectPath.empty()) << "Could not find file " << m_fileName << std::endl;
+void OsgSceneryRepresentation::loadModel(const std::string& fileName)
+{
+ auto model = std::make_shared<OsgModel>();
+ model->load(fileName);
+ setModel(model);
+}
- m_sceneryRepresentation = osgDB::readNodeFile(objectPath);
- SURGSIM_ASSERT(m_sceneryRepresentation.valid()) << "Could not load file " << objectPath << std::endl;
+void OsgSceneryRepresentation::setModel(std::shared_ptr<SurgSim::Framework::Asset> model)
+{
+ auto osgModel = std::dynamic_pointer_cast<OsgModel>(model);
- m_transform->addChild(m_sceneryRepresentation);
- return true;
+ SURGSIM_ASSERT(model == nullptr || osgModel != nullptr) << "OsgSceneryRepresentation expects an OsgModel.";
+
+ if (m_osgNode.valid())
+ {
+ m_transform->removeChild(m_osgNode);
+ }
+ if (osgModel != nullptr)
+ {
+ SURGSIM_ASSERT(osgModel->getOsgNode().valid())
+ << "OsgSceneryRepresentation was passed a model that did not have any geometry assigned to it.";
+ m_osgNode = osgModel->getOsgNode();
+ m_transform->addChild(m_osgNode);
+ }
+
+ m_model = osgModel;
+ updateTangents();
+}
+
+std::shared_ptr<Model> OsgSceneryRepresentation::getModel() const
+{
+ return m_model;
}
-std::string OsgSceneryRepresentation::getFileName() const
+osg::ref_ptr<osg::Node> OsgSceneryRepresentation::getModelNode() const
{
- return m_fileName;
+ return m_osgNode;
}
-void OsgSceneryRepresentation::setFileName(const std::string& fileName)
+void OsgSceneryRepresentation::setGenerateTangents(bool value)
{
- SURGSIM_ASSERT(!isInitialized()) << "Can't set the filename after the object has been initialized.";
- m_fileName = fileName;
+ OsgRepresentation::setGenerateTangents(value);
+ if (m_osgNode.valid())
+ {
+ updateTangents();
+ }
}
}; // namespace Graphics
diff --git a/SurgSim/Graphics/OsgSceneryRepresentation.h b/SurgSim/Graphics/OsgSceneryRepresentation.h
index 4add15b..78cb917 100644
--- a/SurgSim/Graphics/OsgSceneryRepresentation.h
+++ b/SurgSim/Graphics/OsgSceneryRepresentation.h
@@ -28,8 +28,14 @@
namespace SurgSim
{
+namespace Framework
+{
+class Asset;
+}
namespace Graphics
{
+class Model;
+
SURGSIM_STATIC_REGISTRATION(OsgSceneryRepresentation);
/// A OsgSceneryRepresentation is used to load osg object/node from file
@@ -44,19 +50,23 @@ public:
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;
+ void loadModel(const std::string& fileName) override;
+ void setModel(std::shared_ptr<SurgSim::Framework::Asset> model) override;
-private:
- virtual bool doInitialize() override;
+ std::shared_ptr<Model> getModel() const override;
+
+ /// \return the osg node that carries the information of the loaded model
+ osg::ref_ptr<osg::Node> getModelNode() const;
+ void setGenerateTangents(bool value) override;
+
+private:
+ bool doInitialize() override;
/// A osg::Node to hold the objet loaded from file
- osg::ref_ptr<osg::Node> m_sceneryRepresentation;
+ osg::ref_ptr<osg::Node> m_osgNode;
+
+ std::shared_ptr<Model> m_model;
/// Name of the object file to be loaded
std::string m_fileName;
diff --git a/SurgSim/Graphics/OsgScreenSpacePass.h b/SurgSim/Graphics/OsgScreenSpacePass.h
index d17200b..7abe638 100644
--- a/SurgSim/Graphics/OsgScreenSpacePass.h
+++ b/SurgSim/Graphics/OsgScreenSpacePass.h
@@ -49,7 +49,7 @@ public:
void setViewPort(int width, int height);
/// Initialize this Component
- virtual bool doInitialize() override;
+ bool doInitialize() override;
private:
diff --git a/SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.cpp b/SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.cpp
index 5874f0d..3c55613 100644
--- a/SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.cpp
+++ b/SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.cpp
@@ -15,18 +15,14 @@
#include <memory>
-#include "SurgSim/Framework/ApplicationData.h"
-#include "SurgSim/Framework/Log.h"
-#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/SceneElement.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>
@@ -87,13 +83,20 @@ OsgScreenSpaceQuadRepresentation::OsgScreenSpaceQuadRepresentation(const std::st
m_transform->setCullingActive(false);
m_transform->addChild(m_geode);
- m_switch->addChild(m_transform);
+ // By default use float texture coordinates, this makes this useable without a texture set
+ setTextureCoordinates(0.0, 0.0, 1.0, 1.0);
+
+ m_switch->addChild(m_materialProxy);
+ m_materialProxy->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);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgScreenSpaceQuadRepresentation, bool, Transparent,
+ isTransparent, setTransparent);
}
OsgScreenSpaceQuadRepresentation::~OsgScreenSpaceQuadRepresentation()
@@ -202,6 +205,23 @@ void OsgScreenSpaceQuadRepresentation::getLocation(double* x, double* y)
*y = position.y();
}
+void OsgScreenSpaceQuadRepresentation::setTransparent(bool value)
+{
+ if (value)
+ {
+ m_geode->getOrCreateStateSet()->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
+ }
+ else
+ {
+ m_geode->getOrCreateStateSet()->setRenderingHint(osg::StateSet::DEFAULT_BIN);
+ }
+}
+
+bool OsgScreenSpaceQuadRepresentation::isTransparent()
+{
+ return osg::StateSet::TRANSPARENT_BIN == m_geode->getOrCreateStateSet()->getRenderingHint();
+}
+
void OsgScreenSpaceQuadRepresentation::doUpdate(double dt)
{
@@ -241,48 +261,5 @@ bool OsgScreenSpaceQuadRepresentation::doInitialize()
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
index 4de96f5..75b4ad3 100644
--- a/SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h
+++ b/SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h
@@ -22,6 +22,7 @@
#include "SurgSim/Graphics/Representation.h"
#include "SurgSim/Graphics/ScreenSpaceQuadRepresentation.h"
+#include <array>
#include <osg/Vec3>
#if defined(_MSC_VER)
@@ -68,19 +69,19 @@ public:
/// 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;
+ 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;
+ 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;
+ 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
@@ -96,10 +97,18 @@ public:
/// \return true if it succeeds, false if it fails.
bool setTexture(std::shared_ptr<OsgTextureRectangle> texture);
+ /// Sets whether or not the alpha component of the texture should be used for transparancy blending.
+ /// \param value true will render this last, allowing alpha-based blending to work.
+ void setTransparent(bool value);
+
+ /// Gets whether or not the alpha component of the texture should be used for transparancy blending.
+ /// \return true if this will render last, allowing alpha-based blending to work.
+ bool isTransparent();
+
protected:
- virtual void doUpdate(double dt) override;
+ void doUpdate(double dt) override;
- virtual bool doInitialize() override;
+ bool doInitialize() override;
private:
@@ -139,14 +148,6 @@ private:
/// 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
@@ -156,4 +157,4 @@ private:
#pragma warning(pop)
#endif
-#endif
\ No newline at end of file
+#endif
diff --git a/SurgSim/Graphics/OsgShader.cpp b/SurgSim/Graphics/OsgShader.cpp
deleted file mode 100644
index 45d098f..0000000
--- a/SurgSim/Graphics/OsgShader.cpp
+++ /dev/null
@@ -1,259 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 87415d4..0000000
--- a/SurgSim/Graphics/OsgShader.h
+++ /dev/null
@@ -1,150 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/OsgSkeletonRepresentation.cpp b/SurgSim/Graphics/OsgSkeletonRepresentation.cpp
new file mode 100644
index 0000000..fc98555
--- /dev/null
+++ b/SurgSim/Graphics/OsgSkeletonRepresentation.cpp
@@ -0,0 +1,423 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/OsgSkeletonRepresentation.h"
+
+#include <algorithm>
+#include <boost/thread.hpp>
+#include <map>
+#include <osg/Geode>
+#include <osg/Quat>
+#include <osg/Node>
+#include <osg/PositionAttitudeTransform>
+#include <osg/Shader>
+#include <osg/Vec3>
+#include <osgAnimation/Bone>
+#include <osgAnimation/RigGeometry>
+#include <osgAnimation/RigTransformHardware>
+#include <osgAnimation/Skeleton>
+#include <osgAnimation/StackedQuaternionElement>
+#include <osgAnimation/StackedTranslateElement>
+#include <osgAnimation/UpdateBone>
+#include <osgDB/ReadFile>
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Graphics/OsgModel.h"
+#include "SurgSim/Graphics/OsgRigidTransformConversions.h"
+#include "SurgSim/Graphics/OsgProgram.h"
+
+
+/// Local data structure to store the bones and their references to the transforms.
+struct SurgSim::Graphics::BoneData
+{
+ osg::ref_ptr<osgAnimation::Bone> osgBone;
+ osg::ref_ptr<osgAnimation::StackedQuaternionElement> osgRotation;
+ osg::ref_ptr<osgAnimation::StackedTranslateElement> osgTranslation;
+ SurgSim::Math::RigidTransform3d neutralPose;
+ SurgSim::Math::RigidTransform3d pose;
+
+ BoneData() :
+ osgBone(nullptr),
+ osgRotation(nullptr),
+ osgTranslation(nullptr),
+ neutralPose(SurgSim::Math::RigidTransform3d::Identity()),
+ pose(SurgSim::Math::RigidTransform3d::Identity())
+ {
+ }
+};
+
+namespace
+{
+
+/// Visitor to collect information about the tree that we are using, collect all the
+/// osgAnimation::Bone nodes in it and add stacked transform elements to them so
+/// we can manipulate them, also switches the skinning to Hardware
+class BoneBuilder : public osg::NodeVisitor
+{
+public:
+ /// Constructor
+ /// \param hardwareShader The shader which does the skinning.
+ /// \param bones The data structure to store the found bones
+ BoneBuilder(osg::Shader* hardwareShader,
+ std::shared_ptr<std::map<std::string, SurgSim::Graphics::BoneData>> bones) :
+ NodeVisitor(NodeVisitor::TRAVERSE_ALL_CHILDREN),
+ m_shader(hardwareShader),
+ m_bones(bones)
+ {
+ }
+
+ /// Traverse the given tree and collect the bone data in them.
+ /// Also setup for hardware skinning, if a hardware shader is provided.
+ /// \param node The root node of the skeleton tree that needs to be traversed.
+ virtual void apply(osg::Node& node) override // NOLINT
+ {
+ // Look for the transformation root ..
+ if (m_rootTransform == nullptr)
+ {
+ m_rootTransform = dynamic_cast<osg::MatrixTransform*>(&node);
+ }
+
+ // Parse the bone data.
+ osgAnimation::Bone* bone = dynamic_cast<osgAnimation::Bone*>(&node);
+ if (bone != nullptr)
+ {
+ auto newBone = m_bones->find(bone->getName());
+ if (newBone == m_bones->end())
+ {
+ newBone = m_bones->emplace(std::make_pair(bone->getName(), SurgSim::Graphics::BoneData())).first;
+ }
+ else
+ {
+ if (newBone->second.osgBone != nullptr)
+ {
+ SURGSIM_ASSERT(m_bones->find(bone->getName()) == m_bones->end())
+ << "There already exists a bone with name, " << bone->getName()
+ << ", in this skeleton. Cannot create a duplicate.";
+ }
+ }
+
+ newBone->second.osgBone = bone;
+ newBone->second.osgRotation = new osgAnimation::StackedQuaternionElement("OssRotation");
+ newBone->second.osgRotation->setQuaternion(osg::Quat());
+ newBone->second.osgTranslation = new osgAnimation::StackedTranslateElement("OssTranslation");
+
+ osgAnimation::UpdateMatrixTransform* callback =
+ dynamic_cast<osgAnimation::UpdateMatrixTransform*>(bone->getUpdateCallback());
+ if (callback == nullptr)
+ {
+ bone->setDefaultUpdateCallback();
+ callback = dynamic_cast<osgAnimation::UpdateMatrixTransform*>(bone->getUpdateCallback());
+ }
+ SURGSIM_ASSERT(callback != nullptr) << "Could neither find nor create the appropriate BoneUpdate callback";
+
+ // Push these transformations onto the stack so we can manipulate them
+ callback->getStackedTransforms().push_back(newBone->second.osgRotation);
+ callback->getStackedTransforms().push_back(newBone->second.osgTranslation);
+ }
+
+ // Setup for hardware skinning.
+ if (m_shader != nullptr)
+ {
+ osg::Geode* meshGeode = dynamic_cast<osg::Geode*>(&node);
+ if (nullptr != meshGeode)
+ {
+ osgAnimation::RigTransformHardware* rigTransform = new osgAnimation::RigTransformHardware();
+ osg::Shader* shader = new osg::Shader(*m_shader);
+ rigTransform->setShader(shader);
+
+ for (size_t i = 0; i < meshGeode->getNumDrawables(); ++i)
+ {
+ auto rigGeometry = dynamic_cast<osgAnimation::RigGeometry*>(meshGeode->getDrawable(i));
+ if (nullptr != rigGeometry)
+ {
+ rigGeometry->setRigTransformImplementation(rigTransform);
+ }
+ }
+ }
+ }
+ traverse(node);
+ }
+
+ /// \return The root node of the skeleton on which the transform is set.
+ osg::ref_ptr<osg::MatrixTransform> getRootTransform()
+ {
+ return m_rootTransform;
+ }
+
+private:
+ /// The root bone of the skeleton where the global transform is set.
+ osg::ref_ptr<osg::MatrixTransform> m_rootTransform;
+
+ /// The hardware skinning shader.
+ osg::ref_ptr<osg::Shader> m_shader;
+
+ /// The bone data from the skeleton.
+ std::shared_ptr<std::map<std::string, SurgSim::Graphics::BoneData>> m_bones;
+};
+
+};
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Component,
+ SurgSim::Graphics::OsgSkeletonRepresentation, OsgSkeletonRepresentation);
+
+OsgSkeletonRepresentation::OsgSkeletonRepresentation(const std::string& name) :
+ Representation(name),
+ OsgRepresentation(name),
+ SkeletonRepresentation(name),
+ m_logger(SurgSim::Framework::Logger::getLogger("Graphics/OsgSkeletonRepresentation")),
+ m_bones(std::make_shared<std::map<std::string, BoneData>>())
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgSkeletonRepresentation, std::string,
+ SkinningShaderFileName, getSkinningShaderFileName, setSkinningShaderFileName);
+}
+
+void OsgSkeletonRepresentation::loadModel(const std::string& fileName)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot load model after OsgSkeletonRepresentation had been initialized.";
+
+ auto model = std::make_shared<OsgModel>();
+ model->load(fileName);
+ setModel(model);
+}
+
+void OsgSkeletonRepresentation::setModel(std::shared_ptr<SurgSim::Framework::Asset> model)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot set model after OsgSkeletonRepresentation had been initialized.";
+
+ auto osgModel = std::dynamic_pointer_cast<OsgModel>(model);
+ SURGSIM_ASSERT(model == nullptr || osgModel != nullptr) << "OsgSkeletonRepresentation expects an OsgModel.";
+ m_model = osgModel;
+}
+
+std::shared_ptr<Model> OsgSkeletonRepresentation::getModel() const
+{
+ return m_model;
+}
+
+void OsgSkeletonRepresentation::setSkinningShaderFileName(const std::string& fileName)
+{
+ m_skinningShaderFileName = fileName;
+}
+
+std::string OsgSkeletonRepresentation::getSkinningShaderFileName() const
+{
+ return m_skinningShaderFileName;
+}
+
+void OsgSkeletonRepresentation::setBonePose(const std::string& name, const SurgSim::Math::RigidTransform3d& pose)
+{
+ boost::unique_lock<boost::shared_mutex> lock(m_mutex);
+
+ auto found = m_bones->find(name);
+ if (found != m_bones->end())
+ {
+ found->second.pose = pose;
+ }
+ else if (isInitialized())
+ {
+ SURGSIM_FAILURE() << "Bone with name " << name << " is not present in mesh.";
+ }
+ else
+ {
+ auto newBone = m_bones->emplace(std::make_pair(name, BoneData())).first;
+ newBone->second.pose = pose;
+ }
+}
+
+SurgSim::Math::RigidTransform3d OsgSkeletonRepresentation::getBonePose(const std::string& name) const
+{
+ SurgSim::Math::RigidTransform3d pose = SurgSim::Math::RigidTransform3d::Identity();
+ boost::shared_lock<boost::shared_mutex> lock(m_mutex);
+
+ auto found = m_bones->find(name);
+ if (found != m_bones->end())
+ {
+ pose = found->second.pose;
+ }
+ else if (isInitialized())
+ {
+ SURGSIM_FAILURE() << "Bone with name " << name << " is not present in mesh.";
+ }
+
+ return std::move(pose);
+}
+
+void OsgSkeletonRepresentation::setNeutralBonePose(const std::string& name,
+ const SurgSim::Math::RigidTransform3d& pose)
+{
+ boost::unique_lock<boost::shared_mutex> lock(m_mutex);
+
+ auto found = m_bones->find(name);
+ if (found != m_bones->end())
+ {
+ found->second.neutralPose = pose;
+ }
+ else if (isInitialized())
+ {
+ SURGSIM_FAILURE() << "Bone with name " << name << " is not present in mesh.";
+ }
+ else
+ {
+ auto newBone = m_bones->emplace(std::make_pair(name, BoneData())).first;
+ newBone->second.neutralPose = pose;
+ }
+}
+
+SurgSim::Math::RigidTransform3d OsgSkeletonRepresentation::getNeutralBonePose(const std::string& name) const
+{
+ SurgSim::Math::RigidTransform3d pose = SurgSim::Math::RigidTransform3d::Identity();
+ boost::shared_lock<boost::shared_mutex> lock(m_mutex);
+
+ auto found = m_bones->find(name);
+ if (found != m_bones->end())
+ {
+ pose = found->second.neutralPose;
+ }
+ else if (isInitialized())
+ {
+ SURGSIM_FAILURE() << "Bone with name " << name << " is not present in mesh.";
+ }
+
+ return std::move(pose);
+}
+
+void OsgSkeletonRepresentation::setNeutralBonePoses(const std::map<std::string, SurgSim::Math::RigidTransform3d>& poses)
+{
+ for (auto& pose : poses)
+ {
+ setNeutralBonePose(pose.first, pose.second);
+ }
+}
+
+std::map<std::string, SurgSim::Math::RigidTransform3d> OsgSkeletonRepresentation::getNeutralBonePoses() const
+{
+ std::map<std::string, SurgSim::Math::RigidTransform3d> neutralBonePoses;
+ boost::shared_lock<boost::shared_mutex> lock(m_mutex);
+
+ for (auto& bone : *m_bones)
+ {
+ neutralBonePoses[bone.first] = bone.second.neutralPose;
+ }
+ return std::move(neutralBonePoses);
+}
+
+void OsgSkeletonRepresentation::doUpdate(double dt)
+{
+ {
+ boost::shared_lock<boost::shared_mutex> lock(m_mutex);
+ for (auto& bone : *m_bones)
+ {
+ std::pair<osg::Quat, osg::Vec3d> pose = toOsg(bone.second.pose * bone.second.neutralPose);
+ bone.second.osgRotation->setQuaternion(pose.first);
+ bone.second.osgTranslation->setTranslate(pose.second);
+ }
+ }
+
+ // Update the position of the rest of the bones.
+ m_base->accept(*m_updateVisitor);
+ ++m_frameCount;
+ m_updateVisitor->setTraversalNumber(m_frameCount);
+}
+
+bool OsgSkeletonRepresentation::doInitialize()
+{
+ std::string shaderFilename;
+ if (m_skinningShaderFileName.empty())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << getName() << ": Skinning shader file not set.";
+ return false;
+ }
+
+ getRuntime()->getApplicationData()->tryFindFile(m_skinningShaderFileName, &shaderFilename);
+ if (shaderFilename.empty())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << getName() << ": Skinning shader file (" << m_skinningShaderFileName
+ << ") not found.";
+ return false;
+ }
+
+ m_skinningShader = new osg::Shader(osg::Shader::VERTEX);
+ if (!m_skinningShader->loadShaderSourceFromFile(shaderFilename))
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << getName() << ": Error loading shader (" << shaderFilename << ").";
+ return false;
+ }
+
+ if (m_model == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << getName() << ": Model is not set.";
+ return false;
+ }
+
+ m_skeleton = dynamic_cast<osg::Node*>(m_model->getOsgNode().get());
+ if (m_skeleton == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << getName() << ": Model does not have a valid osgNode.";
+ return false;
+ }
+
+ if (!setupBones())
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << getName() << ": Could not find any bones in model.";
+ return false;
+ }
+
+ // Setup the transform updater for the skeleton.
+ m_updateVisitor = new osgUtil::UpdateVisitor();
+ m_frameCount = 0;
+ m_updateVisitor->setTraversalNumber(m_frameCount);
+ m_base->accept(*m_updateVisitor);
+
+ // Add the bone skeleton as a child to m_transform
+ m_transform->addChild(m_base.get());
+
+ return true;
+}
+
+bool OsgSkeletonRepresentation::setupBones()
+{
+ boost::unique_lock<boost::shared_mutex> lock(m_mutex);
+
+ BoneBuilder builder(m_skinningShader, m_bones);
+ builder.traverse(*m_skeleton.get());
+ m_base = builder.getRootTransform();
+
+ for (auto bone = m_bones->begin(); bone != m_bones->end();)
+ {
+ if (bone->second.osgBone == nullptr)
+ {
+ SURGSIM_FAILURE() << "Bone with name " << bone->first << " is not present in mesh.";
+ bone = m_bones->erase(bone);
+ }
+ else
+ {
+ ++bone;
+ }
+ }
+
+ return (m_bones->size() != 0);
+}
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/OsgSkeletonRepresentation.h b/SurgSim/Graphics/OsgSkeletonRepresentation.h
new file mode 100644
index 0000000..79b628e
--- /dev/null
+++ b/SurgSim/Graphics/OsgSkeletonRepresentation.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_GRAPHICS_OSGSKELETONREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGSKELETONREPRESENTATION_H
+
+#include <boost/thread.hpp>
+#include <map>
+#include <osg/MatrixTransform>
+#include <osg/ref_ptr>
+#include <osgUtil/UpdateVisitor>
+#include <string>
+
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Graphics/SkeletonRepresentation.h"
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace osg
+{
+class Node;
+class Shader;
+};
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+struct BoneData;
+class OsgModel;
+
+/// Skeleton representation is used to move a mesh based on the movements of
+/// pre-selected control points (bones).
+class OsgSkeletonRepresentation : public OsgRepresentation, public SkeletonRepresentation
+{
+public:
+ /// Constructor.
+ /// \param name The name of the representation.
+ explicit OsgSkeletonRepresentation(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgSkeletonRepresentation);
+
+ void loadModel(const std::string& fileName) override;
+
+ void setModel(std::shared_ptr<SurgSim::Framework::Asset> model) override;
+
+ std::shared_ptr<Model> getModel() const override;
+
+ /// Set the file containing the skinning shader.
+ /// \param fileName The file containing the skinning shader.
+ void setSkinningShaderFileName(const std::string& fileName);
+
+ /// \return The file containing the skinning shader.
+ std::string getSkinningShaderFileName() const;
+
+ void setBonePose(const std::string& name, const SurgSim::Math::RigidTransform3d& pose) override;
+
+ SurgSim::Math::RigidTransform3d getBonePose(const std::string& name) const override;
+
+ void setNeutralBonePose(const std::string& name, const SurgSim::Math::RigidTransform3d& pose) override;
+
+ SurgSim::Math::RigidTransform3d getNeutralBonePose(const std::string& name) const override;
+
+protected:
+ void setNeutralBonePoses(const std::map<std::string, SurgSim::Math::RigidTransform3d>& poses) override;
+
+ std::map<std::string, SurgSim::Math::RigidTransform3d> getNeutralBonePoses() const override;
+
+ void doUpdate(double dt) override;
+
+ bool doInitialize() override;
+
+private:
+ /// Setup the bones with the model and skinning shader
+ bool setupBones();
+
+ /// The logger for this class.
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+
+ /// The model containing the bone and mesh information.
+ std::shared_ptr<OsgModel> m_model;
+
+ /// The named map of the bones in this skeleton.
+ std::shared_ptr<std::map<std::string, BoneData>> m_bones;
+
+ /// Mutex to access m_bones safely.
+ mutable boost::shared_mutex m_mutex;
+
+ /// The skeleton which is read from the mesh file.
+ osg::ref_ptr<osg::Node> m_skeleton;
+
+ /// The file containing the skinning shader.
+ std::string m_skinningShaderFileName;
+
+ /// The hardware skinning shader.
+ osg::ref_ptr<osg::Shader> m_skinningShader;
+
+ /// Tree updater which updates the position of the bones.
+ osg::ref_ptr<osgUtil::UpdateVisitor> m_updateVisitor;
+
+ /// Parameter to keep track of the skeleton's frame count. Set to the UpdateVisitor.
+ size_t m_frameCount;
+
+ /// The root node of the skeleton tree.
+ osg::ref_ptr<osg::Node> m_root;
+
+ /// The first MatrixTransform node
+ osg::ref_ptr<osg::MatrixTransform> m_base;
+};
+
+}; // namespace Graphics
+}; // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_GRAPHICS_OSGSKELETONREPRESENTATION_H
diff --git a/SurgSim/Graphics/OsgTextRepresentation.cpp b/SurgSim/Graphics/OsgTextRepresentation.cpp
new file mode 100644
index 0000000..efd81c5
--- /dev/null
+++ b/SurgSim/Graphics/OsgTextRepresentation.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.
+
+#include "SurgSim/Graphics/OsgTextRepresentation.h"
+
+#include <osg/Geode>
+#include <osg/Drawable>
+#include <osg/PositionAttitudeTransform>
+#include <osgText/Text>
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Graphics/OsgConversions.h"
+#include "SurgSim/Graphics/OsgFont.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::OsgTextRepresentation, OsgTextRepresentation);
+
+OsgTextRepresentation::OsgTextRepresentation(const std::string& name) :
+ Representation(name),
+ OsgRepresentation(name),
+ TextRepresentation(name),
+ m_geode(new osg::Geode),
+ m_textNode(new osgText::Text),
+ m_needUpdate(true)
+{
+ m_textNode->setDataVariance(osg::Object::DYNAMIC);
+ m_textNode->setUseDisplayList(false);
+ m_characterSize = m_textNode->getCharacterHeight();
+ m_geode->addDrawable(m_textNode);
+
+ m_transform->addChild(m_geode);
+ m_transform->setCullingActive(false);
+
+ // Try to find default font, if this fails OSG will fall back to its own default, only a warning warranted
+ auto font = std::make_shared<OsgFont>();
+ try
+ {
+ font->load("Fonts/Vera.ttf");
+ }
+ catch (std::exception e)
+ {
+ font = nullptr;
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getLogger("Graphics"))
+ << "Could not set default font Fonts/Vera.ttf";
+ }
+
+ m_font = font;
+ setUseScreenSpace(true);
+}
+
+OsgTextRepresentation::~OsgTextRepresentation()
+{
+}
+
+void OsgTextRepresentation::setLocation(double x, double y)
+{
+ setLocalPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(x, y, 0.0)));
+}
+
+void OsgTextRepresentation::getLocation(double* x, double* y) const
+{
+ SURGSIM_ASSERT(x != nullptr && y != nullptr) << "Cannot use a nullptr as an output parameter.";
+ Vector3d position = getLocalPose().translation();
+
+ *x = position.x();
+ *y = position.y();
+}
+
+void OsgTextRepresentation::setText(const std::string& text)
+{
+ boost::mutex::scoped_lock lock(m_parameterMutex);
+ m_text = text;
+ m_needUpdate = true;
+}
+
+std::string OsgTextRepresentation::getText() const
+{
+ return m_text;
+}
+
+void OsgTextRepresentation::doUpdate(double dt)
+{
+ {
+ boost::mutex::scoped_lock lock(m_parameterMutex);
+ if (m_needUpdate)
+ {
+ // Osg will catch which ones have been updated, no need for more fine grained control.
+ // Each of these may trigger an operation that rebuilds the internal structure of the text drawable
+ // which may cause problems with the render
+ m_textNode->setFont(m_font->getOsgFont());
+ m_textNode->setText(m_text);
+ m_textNode->setCharacterSize(m_characterSize);
+ m_textNode->setMaximumWidth(m_optionalWidth.hasValue() ? m_optionalWidth.getValue() : 0.0);
+ m_needUpdate = false;
+ }
+ }
+ if (isUsingScreenSpace())
+ {
+ m_transform->setAttitude(osg::Quat(0.0, 0.0, 0.0, 1.0));
+ }
+}
+
+bool OsgTextRepresentation::doInitialize()
+{
+ bool result = true;
+
+ // if the material was preassigned, don't create a default one
+ if (getMaterial() == nullptr)
+ {
+ result = false;
+ auto material = buildMaterial("Shaders/unlit_texture.vert", "Shaders/unlit_text.frag");
+ if (material != nullptr)
+ {
+ m_textNode->getOrCreateStateSet()->addUniform(new osg::Uniform("texture", 0));
+ setMaterial(material);
+ result = true;
+ }
+ }
+
+ return result;
+
+}
+
+void OsgTextRepresentation::loadFont(const std::string& fileName)
+{
+ auto newFont = std::make_shared<OsgFont>();
+ newFont->load(fileName);
+ setFont(newFont);
+}
+
+void OsgTextRepresentation::setFont(std::shared_ptr<SurgSim::Framework::Asset> font)
+{
+ SURGSIM_ASSERT(! isInitialized()) << "Can't set font after text has been initialized.";
+ SURGSIM_ASSERT(font != nullptr) << "Can't use nullptr font.";
+
+ auto osgFont = std::dynamic_pointer_cast<OsgFont>(font);
+
+ SURGSIM_ASSERT(osgFont != nullptr)
+ << "Font has to be OsgFont, instead it was " << font->getClassName();
+ {
+ boost::mutex::scoped_lock lock(m_parameterMutex);
+ m_font = osgFont;
+ m_needUpdate = true;
+ }
+}
+
+std::shared_ptr<Font> OsgTextRepresentation::getFont() const
+{
+ return m_font;
+}
+
+void OsgTextRepresentation::setOptionalMaximumWidth(SurgSim::DataStructures::OptionalValue<double> maximum)
+{
+ boost::mutex::scoped_lock lock(m_parameterMutex);
+ m_optionalWidth = maximum;
+ m_needUpdate = true;
+}
+
+SurgSim::DataStructures::OptionalValue<double> OsgTextRepresentation::getOptionalMaximumWidth()
+{
+ return m_optionalWidth;
+}
+
+void OsgTextRepresentation::setDrawBackground(bool value)
+{
+ int drawMode = osgText::TextBase::TEXT;
+ if (value)
+ {
+ drawMode |= osgText::TextBase::FILLEDBOUNDINGBOX;
+ }
+
+ m_textNode->setDrawMode(drawMode);
+}
+
+bool OsgTextRepresentation::isDrawingBackground() const
+{
+ return (m_textNode->getDrawMode() & osgText::TextBase::FILLEDBOUNDINGBOX) != 0;
+}
+
+void OsgTextRepresentation::setBackgroundColor(Math::Vector4d color)
+{
+ m_textNode->setBoundingBoxColor(toOsg(color));
+}
+
+Math::Vector4d OsgTextRepresentation::getBackgroundColor()
+{
+ Math::Vector4d result = fromOsg(m_textNode->getBoundingBoxColor()).cast<double>();
+ return result;
+}
+
+void OsgTextRepresentation::setBackgroundMargin(double margin)
+{
+ m_textNode->setBoundingBoxMargin(static_cast<float>(margin));
+}
+
+double OsgTextRepresentation::getBackgroundMargin() const
+{
+ return m_textNode->getBoundingBoxMargin();
+}
+
+void OsgTextRepresentation::setUseScreenSpace(bool value)
+{
+ if (value == true)
+ {
+ removeGroupReference(Representation::DefaultGroupName);
+ addGroupReference(Representation::DefaultHudGroupName);
+ m_transform->setReferenceFrame(osg::Transform::ABSOLUTE_RF);
+ m_textNode->setCharacterSizeMode(osgText::TextBase::SCREEN_COORDS);
+ }
+ else
+ {
+ removeGroupReference(Representation::DefaultHudGroupName);
+ addGroupReference(Representation::DefaultGroupName);
+ m_transform->setReferenceFrame(osg::Transform::RELATIVE_RF);
+ m_textNode->setCharacterSizeMode(osgText::TextBase::OBJECT_COORDS);
+ }
+}
+
+bool OsgTextRepresentation::isUsingScreenSpace() const
+{
+ auto references = getGroupReferences();
+ auto found = std::find(references.begin(), references.end(), Representation::DefaultHudGroupName);
+ return (found != references.end());
+}
+
+void OsgTextRepresentation::setMaximumWidth(double width)
+{
+ boost::mutex::scoped_lock lock(m_parameterMutex);
+ if (width > 0.0)
+ {
+ m_optionalWidth.setValue(width);
+ }
+ else
+ {
+ m_optionalWidth.invalidate();
+ }
+ m_needUpdate = true;
+}
+
+double OsgTextRepresentation::getMaximumWidth()
+{
+ return m_optionalWidth.hasValue() ? m_optionalWidth.getValue() : 0.0;
+}
+
+void OsgTextRepresentation::setFontSize(double size)
+{
+ boost::mutex::scoped_lock lock(m_parameterMutex);
+ m_characterSize = size;
+ m_needUpdate = true;
+}
+
+double OsgTextRepresentation::getFontSize() const
+{
+ return m_characterSize;
+}
+
+void OsgTextRepresentation::setColor(SurgSim::Math::Vector4d color)
+{
+ m_textNode->setColor(toOsg(color));
+}
+
+SurgSim::Math::Vector4d OsgTextRepresentation::getColor() const
+{
+ SurgSim::Math::Vector4d result = fromOsg(m_textNode->getColor()).cast<double>();
+ return result;
+}
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/OsgTextRepresentation.h b/SurgSim/Graphics/OsgTextRepresentation.h
new file mode 100644
index 0000000..d89421b
--- /dev/null
+++ b/SurgSim/Graphics/OsgTextRepresentation.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_OSGTEXTREPRESENTATION_H
+#define SURGSIM_GRAPHICS_OSGTEXTREPRESENTATION_H
+
+#include <string>
+
+#include "SurgSim/Graphics/OsgRepresentation.h"
+#include "SurgSim/Graphics/TextRepresentation.h"
+#include "SurgSim/DataStructures/OptionalValue.h"
+
+#include <osg/Vec3>
+#include <boost/thread/mutex.hpp>
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4250)
+#endif
+
+namespace osg
+{
+class Geode;
+}
+
+namespace osgText
+{
+class Text;
+}
+
+namespace SurgSim
+{
+namespace Framework
+{
+class Asset;
+}
+namespace Graphics
+{
+class OsgFont;
+
+SURGSIM_STATIC_REGISTRATION(OsgTextRepresentation);
+
+/// Osg implementation of the TextRepresentation, to be used with OsgFont assets
+class OsgTextRepresentation : public OsgRepresentation, public TextRepresentation
+{
+public:
+ /// Constructor
+ /// \param name Name of this OsgInfo
+ explicit OsgTextRepresentation(const std::string& name);
+
+ /// Destructor
+ ~OsgTextRepresentation();
+
+ friend class OsgTextRepresentationTests_MaximumWidth_Test;
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgTextRepresentation);
+
+ void setLocation(double x, double y) override;
+ void getLocation(double* x, double* y) const override;
+
+ void setMaximumWidth(double width) override;
+ double getMaximumWidth() override;
+
+ void setText(const std::string& text) override;
+ std::string getText() const override;
+
+ void loadFont(const std::string& fileName) override;
+ void setFont(std::shared_ptr<SurgSim::Framework::Asset> font) override;
+ std::shared_ptr<Font> getFont() const override;
+
+ void setColor(SurgSim::Math::Vector4d color) override;
+ SurgSim::Math::Vector4d getColor() const override;
+
+ void setFontSize(double size) override;
+ double getFontSize() const override;
+
+ void setUseScreenSpace(bool value) override;
+ bool isUsingScreenSpace() const override;
+
+ enum Anchor
+ {
+ ANCHOR_TOP_LEFT,
+ ANCHOR_CENTER
+ };
+
+ void setAnchor(int anchor);
+ int getAnchor() const;
+
+ void setDrawBackground(bool value) override;
+ bool isDrawingBackground() const override;
+
+ void setBackgroundColor(Math::Vector4d color) override;
+ Math::Vector4d getBackgroundColor() override;
+
+ void setBackgroundMargin(double margin) override;
+ double getBackgroundMargin() const override;
+
+protected:
+ void doUpdate(double dt) override;
+ bool doInitialize() override;
+
+ void setOptionalMaximumWidth(SurgSim::DataStructures::OptionalValue<double> maximum) override;
+ SurgSim::DataStructures::OptionalValue<double> getOptionalMaximumWidth() override;
+
+private:
+ osg::ref_ptr<osg::Geode> m_geode; ///< node used to render text
+ osg::ref_ptr<osgText::Text> m_textNode; ///< node for text display
+
+ std::string m_text; ///< Text set by the user
+ std::shared_ptr<OsgFont> m_font; ///< font used for rendering
+ SurgSim::DataStructures::OptionalValue<double> m_optionalWidth; ///< information about the maximum width
+
+ double m_characterSize; ///< the font height
+
+ boost::mutex m_parameterMutex; ///< protect changes of parameters
+ bool m_needUpdate; ///< indicate whether parameters need to be updated
+ int m_anchor;
+
+ SurgSim::Math::Vector3d m_offset;
+};
+
+}; // Graphics
+}; // SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif
\ No newline at end of file
diff --git a/SurgSim/Graphics/OsgTextureCubeMap.cpp b/SurgSim/Graphics/OsgTextureCubeMap.cpp
index 102caeb..f930972 100644
--- a/SurgSim/Graphics/OsgTextureCubeMap.cpp
+++ b/SurgSim/Graphics/OsgTextureCubeMap.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -19,8 +19,10 @@
using SurgSim::Graphics::OsgTextureCubeMap;
+
OsgTextureCubeMap::OsgTextureCubeMap() : OsgTexture(new osg::TextureCubeMap())
{
+
}
void OsgTextureCubeMap::setSize(int width, int height)
@@ -60,16 +62,16 @@ bool OsgTextureCubeMap::loadImage(const std::string& filePath)
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);
+ 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);
+ 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);
+ 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);
+ osg::ref_ptr<osg::Image> positiveY = copyImageBlock(*imageNode, width, height * 3, width, height);
positiveY->flipVertical();
getOsgTexture()->setImage(osg::TextureCubeMap::POSITIVE_X, positiveX);
@@ -83,8 +85,8 @@ bool OsgTextureCubeMap::loadImage(const std::string& filePath)
}
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)
+ 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);
@@ -110,22 +112,24 @@ bool OsgTextureCubeMap::loadImageFaces(const std::string& negativeX, const std::
}
}
-osg::ref_ptr<osg::Image> OsgTextureCubeMap::copyImageBlock(const osg::Image& source, int column, int row,
- int width, int height)
+osg::ref_ptr<osg::Image> OsgTextureCubeMap::copyImageBlock(
+ const osg::Image& source,
+ size_t startColumn, size_t startRow,
+ size_t width, size_t height)
{
- int pixelSize = source.getPixelSizeInBits() / 8;
+ size_t pixelSize = source.getPixelSizeInBits() / 8;
- unsigned char* buffer = new unsigned char[(width*height) * pixelSize];
+ unsigned char* buffer = new unsigned char[(width * height) * pixelSize];
- int index = 0;
+ size_t index = 0;
- for(int i = row; i < row + height; ++i)
+ for (size_t row = startRow; row < startRow + height; ++row)
{
- for(int j = column; j < column + width; ++j)
+ for (size_t column = startColumn; column < startColumn + width; ++column)
{
const unsigned char* pixel = source.data(column, row, 0);
- for (int p = 0; p < pixelSize; ++p)
+ for (size_t p = 0; p < pixelSize; ++p)
{
buffer[index] = pixel[p];
index++;
@@ -134,10 +138,13 @@ osg::ref_ptr<osg::Image> OsgTextureCubeMap::copyImageBlock(const osg::Image& sou
}
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);
+ GL_UNSIGNED_BYTE, buffer, osg::Image::USE_NEW_DELETE, 1);
return subImage;
-}
\ No newline at end of file
+}
+
+osg::ref_ptr<osg::TextureCubeMap> SurgSim::Graphics::OsgTextureCubeMap::getOsgTextureCubeMap() const
+{
+ return static_cast<osg::TextureCubeMap*>(getOsgTexture().get());
+}
diff --git a/SurgSim/Graphics/OsgTextureCubeMap.h b/SurgSim/Graphics/OsgTextureCubeMap.h
index 8847a95..259f9d7 100644
--- a/SurgSim/Graphics/OsgTextureCubeMap.h
+++ b/SurgSim/Graphics/OsgTextureCubeMap.h
@@ -17,12 +17,13 @@
#define SURGSIM_GRAPHICS_OSGTEXTURECUBEMAP_H
#include "SurgSim/Graphics/OsgTexture.h"
+#include "SurgSim/Graphics/TextureCubeMap.h"
#include <osg/TextureCubeMap>
#if defined(_MSC_VER)
-#pragma warning(push)
-#pragma warning(disable:4250)
+ #pragma warning(push)
+ #pragma warning(disable:4250)
#endif
namespace SurgSim
@@ -34,23 +35,16 @@ namespace Graphics
/// OSG implementation of a Cube Map Texture
///
/// Wraps an osg::TextureCubeMap
-class OsgTextureCubeMap : public OsgTexture
+class OsgTextureCubeMap : public OsgTexture, public TextureCubeMap
{
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);
+ void setSize(int width, int height) override;
- /// 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;
+ void getSize(int* width, int* height) const override;
/// Loads an image into the texture from a file
/// \param filePath Path to the image file
@@ -65,35 +59,26 @@ public:
/// (+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);
+ bool loadImage(const std::string& filePath) override;
+
+ 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) override;
/// Returns the osg::TextureCubeMap
- osg::ref_ptr<osg::TextureCubeMap> getOsgTextureCubeMap() const
- {
- return static_cast<osg::TextureCubeMap*>(getOsgTexture().get());
- }
+ osg::ref_ptr<osg::TextureCubeMap> getOsgTextureCubeMap() const;
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 startColumn First column of block in the source image
+ /// \param startRow 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);
+ osg::ref_ptr<osg::Image> copyImageBlock(const osg::Image& source,
+ size_t startColumn, size_t startRow,
+ size_t width, size_t height);
};
}; // namespace Graphics
@@ -101,7 +86,7 @@ protected:
}; // namespace SurgSim
#if defined(_MSC_VER)
-#pragma warning(pop)
+ #pragma warning(pop)
#endif
#endif // SURGSIM_GRAPHICS_OSGTEXTURECUBEMAP_H
diff --git a/SurgSim/Graphics/OsgTextureUniform-inl.h b/SurgSim/Graphics/OsgTextureUniform-inl.h
index 6db3571..de80e81 100644
--- a/SurgSim/Graphics/OsgTextureUniform-inl.h
+++ b/SurgSim/Graphics/OsgTextureUniform-inl.h
@@ -16,6 +16,8 @@
#ifndef SURGSIM_GRAPHICS_OSGTEXTUREUNIFORM_INL_H
#define SURGSIM_GRAPHICS_OSGTEXTUREUNIFORM_INL_H
+#include <osg/PointSprite>
+
#include "SurgSim/Framework/Assert.h"
#include "SurgSim/Graphics/OsgTexture.h"
#include "SurgSim/Graphics/OsgTexture1d.h"
@@ -55,6 +57,12 @@ void OsgTextureUniform<T>::set(const std::shared_ptr<T>& value)
}
template <class T>
+void OsgTextureUniform<T>::set(const YAML::Node& value)
+{
+ m_unit = value.as<int>();
+}
+
+template <class T>
const std::shared_ptr<T>& OsgTextureUniform<T>::get() const
{
return m_texture;
@@ -63,7 +71,7 @@ const std::shared_ptr<T>& OsgTextureUniform<T>::get() const
template <class T>
void OsgTextureUniform<T>::addToStateSet(osg::StateSet* stateSet)
{
- SURGSIM_ASSERT(m_stateset == nullptr) << "Unexpected addToStateSet for OsgTextureUniform.";
+ SURGSIM_ASSERT(m_stateset == nullptr) << "Unexpected addToStateSet for OsgTextureUniform " << getName() << ".";
const osg::StateSet::TextureAttributeList& textures = stateSet->getTextureAttributeList();
@@ -84,7 +92,13 @@ void OsgTextureUniform<T>::addToStateSet(osg::StateSet* stateSet)
m_unit = availableUnit;
- SURGSIM_ASSERT(m_texture != nullptr) << "Tried to add this uniform without a valid Texture";
+ SURGSIM_ASSERT(m_texture != nullptr) << "Tried to add uniform " << getName() << " without a valid Texture";
+ if(m_texture->isPointSprite())
+ {
+ osg::PointSprite* sprite = new osg::PointSprite();
+ stateSet->setTextureAttributeAndModes(m_unit, sprite, osg::StateAttribute::ON);
+ stateSet->setMode(GL_VERTEX_PROGRAM_POINT_SIZE, osg::StateAttribute::ON);
+ }
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!" <<
diff --git a/SurgSim/Graphics/OsgTextureUniform.h b/SurgSim/Graphics/OsgTextureUniform.h
index df632e3..9c1fedc 100644
--- a/SurgSim/Graphics/OsgTextureUniform.h
+++ b/SurgSim/Graphics/OsgTextureUniform.h
@@ -46,6 +46,8 @@ public:
/// the texture that was used to create this uniform
virtual void set(const std::shared_ptr<T>& value);
+ virtual void set(const YAML::Node& node);
+
/// Returns the value of the uniform
virtual const std::shared_ptr<T>& get() const;
diff --git a/SurgSim/Graphics/OsgTrackballZoomManipulator.cpp b/SurgSim/Graphics/OsgTrackballZoomManipulator.cpp
index 8c76b21..5f88d2e 100644
--- a/SurgSim/Graphics/OsgTrackballZoomManipulator.cpp
+++ b/SurgSim/Graphics/OsgTrackballZoomManipulator.cpp
@@ -210,5 +210,10 @@ bool OsgTrackballZoomManipulator::handleMouseWheel(
}
}
+void OsgTrackballZoomManipulator::updateCamera(osg::Camera& camera)
+{
+ return;
+}
+
}; // namespace Graphics
}; // namespace SurgSim
diff --git a/SurgSim/Graphics/OsgTrackballZoomManipulator.h b/SurgSim/Graphics/OsgTrackballZoomManipulator.h
index c7bb993..8152668 100644
--- a/SurgSim/Graphics/OsgTrackballZoomManipulator.h
+++ b/SurgSim/Graphics/OsgTrackballZoomManipulator.h
@@ -115,17 +115,21 @@ protected:
/// \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);
+ virtual bool handle(const osgGA::GUIEventAdapter& eventAdapter, osgGA::GUIActionAdapter& actionAdapter); //NOLINT
/// 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);
+ virtual bool handleMouseWheel(const osgGA::GUIEventAdapter& eventAdapter,
+ osgGA::GUIActionAdapter& actionAdapter); //NOLINT
+
+ void updateCamera(osg::Camera& camera) override;
+
};
}; // namespace Graphics
}; // namespace SurgSim
-#endif
\ No newline at end of file
+#endif
diff --git a/SurgSim/Graphics/OsgUniform-inl.h b/SurgSim/Graphics/OsgUniform-inl.h
index 2b5408b..cc41612 100644
--- a/SurgSim/Graphics/OsgUniform-inl.h
+++ b/SurgSim/Graphics/OsgUniform-inl.h
@@ -17,9 +17,11 @@
#define SURGSIM_GRAPHICS_OSGUNIFORM_INL_H
#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Math/MathConvert.h"
#include "SurgSim/Graphics/OsgConversions.h"
#include "SurgSim/Graphics/OsgUniformTypes.h"
+
namespace SurgSim
{
@@ -56,6 +58,12 @@ void OsgUniform<T>::set(const T& value)
}
template <class T>
+void OsgUniform<T>::set(const YAML::Node& node)
+{
+ set(node.as<T>());
+}
+
+template <class T>
const T& OsgUniform<T>::get() const
{
return m_value;
@@ -99,6 +107,13 @@ void OsgUniform<std::vector<T>>::set(const std::vector<T>& value)
}
template <class T>
+void OsgUniform<std::vector<T>>::set(const YAML::Node& node)
+{
+ SURGSIM_ASSERT(node.IsSequence()) << "Yaml setter called on vector uniform with non-sequence yaml node.";
+ set(node.as<std::vector<T>>());
+}
+
+template <class T>
typename std::vector<T>::const_reference OsgUniform<std::vector<T>>::getElement(size_t index) const
{
return m_value[index];
diff --git a/SurgSim/Graphics/OsgUniform.h b/SurgSim/Graphics/OsgUniform.h
index fad307e..871cd99 100644
--- a/SurgSim/Graphics/OsgUniform.h
+++ b/SurgSim/Graphics/OsgUniform.h
@@ -19,6 +19,11 @@
#include "SurgSim/Graphics/OsgUniformBase.h"
#include "SurgSim/Graphics/Uniform.h"
+namespace YAML
+{
+class Node;
+}
+
namespace SurgSim
{
@@ -26,7 +31,7 @@ namespace Graphics
{
/// OSG implementation of graphics uniform with a value of type T.
-/// \tparam Value type
+/// \tparam Value the value type of the uniform
template <class T>
class OsgUniform : public Uniform<T>, public OsgUniformBase
{
@@ -36,9 +41,14 @@ public:
explicit OsgUniform(const std::string& name);
/// Sets the value of the uniform
+ /// \param value the value for this uniform
virtual void set(const T& value);
- /// Returns the value of the uniform
+ /// Sets the value of the uniform from a YAML Node doing the correct conversion
+ /// \param node the node that contains the value for this uniform
+ virtual void set(const YAML::Node& node);
+
+ /// \return the value of the uniform
virtual const T& get() const;
private:
@@ -57,7 +67,7 @@ public:
/// \param numElements Number of elements
OsgUniform(const std::string& name, size_t numElements);
- /// Returns the number of elements
+ /// \return the number of elements in the uniform
virtual size_t getNumElements() const;
/// Sets the value of one of the uniform's elements
@@ -69,6 +79,11 @@ public:
/// \param value Array of values
virtual void set(const std::vector<T>& value);
+ /// Sets the value of the uniform from a YAML Node doing the correct conversion
+ /// \param node that contains the values for this uniform (needs to be sequence type)
+ /// \throws if the node is not of sequence type
+ virtual void set(const YAML::Node& node);
+
/// Gets the value of one of the uniform's elements
/// \param index Index of the element
/// \return Value of the element
diff --git a/SurgSim/Graphics/OsgUniformBase.cpp b/SurgSim/Graphics/OsgUniformBase.cpp
index de01127..8dca1cb 100644
--- a/SurgSim/Graphics/OsgUniformBase.cpp
+++ b/SurgSim/Graphics/OsgUniformBase.cpp
@@ -26,7 +26,7 @@ OsgUniformBase::OsgUniformBase(const std::string& name) : UniformBase(),
void OsgUniformBase::addToStateSet(osg::StateSet* stateSet)
{
- SURGSIM_LOG_DEBUG(SurgSim::Framework::Logger::getDefaultLogger()) << "Base Add To Texture StateSet called";
+ SURGSIM_LOG_DEBUG(Framework::Logger::getLogger("Graphics/OsgUniformBase")) << "Base Add To Texture StateSet called";
stateSet->addUniform(m_uniform);
}
diff --git a/SurgSim/Graphics/OsgUniformBase.h b/SurgSim/Graphics/OsgUniformBase.h
index fb9fa93..e91a3f6 100644
--- a/SurgSim/Graphics/OsgUniformBase.h
+++ b/SurgSim/Graphics/OsgUniformBase.h
@@ -21,6 +21,11 @@
#include <osg/StateSet>
#include <osg/Uniform>
+namespace YAML
+{
+class Node;
+}
+
namespace SurgSim
{
@@ -42,6 +47,11 @@ public:
return m_uniform->getName();
}
+ /// Sets the value of the uniform from a YAML Node, the uniform is responsible for converting the node to its own
+ /// value
+ /// \param node YAML node for setting the uniform value
+ virtual void set(const YAML::Node& node) = 0;
+
/// Adds this uniform to the OSG state set
/// \param stateSet OSG state set
virtual void addToStateSet(osg::StateSet* stateSet);
diff --git a/SurgSim/Graphics/OsgUniformFactory.cpp b/SurgSim/Graphics/OsgUniformFactory.cpp
index 65bc2c5..149d854 100644
--- a/SurgSim/Graphics/OsgUniformFactory.cpp
+++ b/SurgSim/Graphics/OsgUniformFactory.cpp
@@ -20,6 +20,7 @@
#include "SurgSim/Graphics/OsgTexture1d.h"
#include "SurgSim/Graphics/OsgTexture2d.h"
#include "SurgSim/Graphics/OsgTexture3d.h"
+#include "SurgSim/Graphics/OsgTextureCubeMap.h"
#include "SurgSim/Graphics/OsgUniform.h"
#include "SurgSim/Math/Vector.h"
@@ -60,6 +61,7 @@ OsgUniformFactory::OsgUniformFactory()
registerClass<OsgTextureUniform<OsgTexture1d>>("sampler1D");
registerClass<OsgTextureUniform<OsgTexture2d>>("sampler2D");
registerClass<OsgTextureUniform<OsgTexture3d>>("sampler3D");
+ registerClass<OsgTextureUniform<OsgTextureCubeMap>>("samplerCube");
}
OsgUniformFactory::~OsgUniformFactory()
diff --git a/SurgSim/Graphics/OsgVectorFieldRepresentation.cpp b/SurgSim/Graphics/OsgVectorFieldRepresentation.cpp
index c9cff07..7db5cee 100644
--- a/SurgSim/Graphics/OsgVectorFieldRepresentation.cpp
+++ b/SurgSim/Graphics/OsgVectorFieldRepresentation.cpp
@@ -27,6 +27,10 @@ namespace SurgSim
namespace Graphics
{
+SURGSIM_REGISTER(SurgSim::Framework::Component,
+ SurgSim::Graphics::OsgVectorFieldRepresentation,
+ OsgVectorFieldRepresentation);
+
using SurgSim::DataStructures::Vertex;
OsgVectorFieldRepresentation::OsgVectorFieldRepresentation(const std::string& name) :
@@ -35,7 +39,12 @@ OsgVectorFieldRepresentation::OsgVectorFieldRepresentation(const std::string& na
OsgRepresentation(name),
m_scale(0.1)
{
- m_vectorField = std::make_shared<SurgSim::Graphics::VectorField>();
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgVectorFieldRepresentation, double, LineWidth, getLineWidth, setLineWidth);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgVectorFieldRepresentation, double, Scale, getScale, setScale);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgVectorFieldRepresentation, double, PointSize, getPointSize, setPointSize);
+
+
+ m_vectorField = std::make_shared<VectorField>();
m_vertexData = new osg::Vec3Array;
m_lineGeometry = new osg::Geometry;
m_pointGeometry = new osg::Geometry;
@@ -72,7 +81,20 @@ OsgVectorFieldRepresentation::~OsgVectorFieldRepresentation()
void OsgVectorFieldRepresentation::doUpdate(double dt)
{
- std::vector<Vertex<VectorFieldData>> vertices = m_vectorField->getVertices();
+ VectorField vectorField;
+ if (m_writeBuffer.tryTakeChanged(&vectorField))
+ {
+ privateUpdate(vectorField.getVertices());
+ }
+ else
+ {
+ // #threadsafety This update is through the shared datastructure, not threadsafe
+ privateUpdate(m_vectorField->getVertices());
+ }
+}
+
+void OsgVectorFieldRepresentation::privateUpdate(const std::vector<DataStructures::Vertex<VectorFieldData>>& vertices)
+{
size_t count = vertices.size();
if (0 != count)
@@ -102,10 +124,10 @@ void OsgVectorFieldRepresentation::doUpdate(double dt)
for (size_t i = 0; i < count; ++i)
{
// Starting location of vector
- (*m_vertexData)[2 * i] = SurgSim::Graphics::toOsg(vertices[i].position);
+ (*m_vertexData)[2 * i] = 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_vertexData)[2 * i + 1] = toOsg(vertices[i].position) +
+ toOsg(vertices[i].data.direction) * m_scale;
m_drawPoints->at(i) = (2 * i);
}
@@ -115,7 +137,7 @@ void OsgVectorFieldRepresentation::doUpdate(double dt)
{
for (size_t i = 0; i < count; ++i)
{
- osg::Vec4d color = SurgSim::Graphics::toOsg(vertices[i].data.color.getValue());
+ osg::Vec4d color = toOsg(vertices[i].data.color.getValue());
(*m_colors)[2 * i] = color;
(*m_colors)[2 * i + 1] = color;
}
@@ -124,7 +146,7 @@ void OsgVectorFieldRepresentation::doUpdate(double dt)
}
else
{
- (*m_colors)[0]= osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f);
+ (*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);
}
@@ -134,7 +156,12 @@ void OsgVectorFieldRepresentation::doUpdate(double dt)
}
}
-std::shared_ptr< SurgSim::Graphics::VectorField > OsgVectorFieldRepresentation::getVectorField() const
+void OsgVectorFieldRepresentation::updateVectorField(const VectorField& vectorfield)
+{
+ m_writeBuffer.set(vectorfield);
+}
+
+std::shared_ptr<VectorField> OsgVectorFieldRepresentation::getVectorField() const
{
return m_vectorField;
}
diff --git a/SurgSim/Graphics/OsgVectorFieldRepresentation.h b/SurgSim/Graphics/OsgVectorFieldRepresentation.h
index f8e599b..8b1a10b 100644
--- a/SurgSim/Graphics/OsgVectorFieldRepresentation.h
+++ b/SurgSim/Graphics/OsgVectorFieldRepresentation.h
@@ -16,8 +16,10 @@
#ifndef SURGSIM_GRAPHICS_OSGVECTORFIELDREPRESENTATION_H
#define SURGSIM_GRAPHICS_OSGVECTORFIELDREPRESENTATION_H
+#include "SurgSim/Framework/LockedContainer.h"
#include "SurgSim/Graphics/OsgRepresentation.h"
#include "SurgSim/Graphics/VectorFieldRepresentation.h"
+#include "SurgSim/Graphics/VectorField.h"
#include <osg/Array>
#include <osg/Geometry>
@@ -29,6 +31,8 @@ namespace SurgSim
namespace Graphics
{
+SURGSIM_STATIC_REGISTRATION(OsgVectorFieldRepresentation);
+
#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable:4250)
@@ -44,40 +48,37 @@ public:
/// Destructor
~OsgVectorFieldRepresentation();
- /// Gets the vector field
- /// \return The vector field
- virtual std::shared_ptr< SurgSim::Graphics::VectorField > getVectorField() const override;
+ SURGSIM_CLASSNAME(SurgSim::Graphics::OsgVectorFieldRepresentation);
+
+ std::shared_ptr<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;
+ void setLineWidth(double width) override;
+ 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;
+ void setScale(double scale) override;
+ 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;
+ void doUpdate(double dt) override;
+
+ void updateVectorField(const VectorField& vectorfield) override;
private:
+ void privateUpdate(const std::vector<DataStructures::Vertex<VectorFieldData>>& vertices);
+
/// 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;
+ SurgSim::Framework::LockedContainer<VectorField> m_writeBuffer;
+
/// OSG vertex data structure
osg::ref_ptr<osg::Vec3Array> m_vertexData;
@@ -110,4 +111,4 @@ private:
}; // Graphics
}; // SurgSim
-#endif // SURGSIM_GRAPHICS_OSGVECTORFIELDREPRESENTATION_H
\ No newline at end of file
+#endif // SURGSIM_GRAPHICS_OSGVECTORFIELDREPRESENTATION_H
diff --git a/SurgSim/Graphics/OsgView.cpp b/SurgSim/Graphics/OsgView.cpp
index cc2f802..50c3e94 100644
--- a/SurgSim/Graphics/OsgView.cpp
+++ b/SurgSim/Graphics/OsgView.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -15,6 +15,8 @@
#include "SurgSim/Graphics/OsgView.h"
+#include <osg/DisplaySettings>
+#include <osgViewer/ViewerEventHandlers>
#include "SurgSim/Devices/Keyboard/KeyboardDevice.h"
#include "SurgSim/Devices/Keyboard/OsgKeyboardHandler.h"
@@ -28,13 +30,11 @@
#include "SurgSim/Framework/FrameworkConvert.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Framework/PoseComponent.h"
#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/RigidTransform.h"
-#include <osgViewer/ViewerEventHandlers>
-#include <osg/DisplaySettings>
-
-using SurgSim::Graphics::OsgCamera;
-using SurgSim::Graphics::OsgView;
namespace
{
@@ -76,8 +76,8 @@ OsgView::OsgView(const std::string& name) : View(name),
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_manipulatorPosition(Math::Vector3d(0.0, 0.0, -3.0)),
+ m_manipulatorLookat(Math::Vector3d::Zero()),
m_keyboardEnabled(false),
m_mouseEnabled(false)
{
@@ -91,15 +91,17 @@ OsgView::OsgView(const std::string& name) : View(name),
SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgView, bool, CameraManipulatorEnabled,
isManipulatorEnabled, enableManipulator);
- SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgView, SurgSim::Math::Vector3d, CameraPosition,
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgView, Math::Vector3d, CameraPosition,
getManipulatorPosition, setManipulatorPosition);
- SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgView, SurgSim::Math::Vector3d, CameraLookAt,
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OsgView, 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);
+
+ doSetTargetScreen(getTargetScreen());
}
OsgView::~OsgView()
@@ -134,7 +136,32 @@ void OsgView::setDimensions(const std::array<int, 2>& dimensions)
std::array<int, 2> OsgView::getDimensions() const
{
- return m_dimensions;
+ if (isFullScreen())
+ {
+ return m_screenDimensions;
+ }
+ else
+ {
+ return m_dimensions;
+ }
+}
+
+void OsgView::setDimensionsDouble(const std::array<double, 2>& dimensions)
+{
+ if (m_dimensions[0] != static_cast<int>(dimensions[0]) && m_dimensions[1] != static_cast<int>(dimensions[1]))
+ {
+ m_areWindowSettingsDirty = true;
+ m_dimensions[0] = dimensions[0];
+ m_dimensions[1] = dimensions[1];
+ }
+}
+
+std::array<double, 2> OsgView::getDimensionsDouble() const
+{
+ std::array<int, 2> m_d = getDimensions();
+
+ std::array<double, 2> dimensions = {static_cast<double>(m_d[0]), static_cast<double>(m_d[1])};
+ return dimensions;
}
void OsgView::setWindowBorderEnabled(bool enabled)
@@ -150,28 +177,52 @@ bool OsgView::isWindowBorderEnabled() const
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.";
+ auto osgCamera = Framework::checkAndConvert<OsgCamera>(camera, "SurgSim::Graphics::OsgCamera");
View::setCamera(camera);
m_view->setCamera(osgCamera->getOsgCamera());
+ osgCamera->setViewport(0, 0, m_dimensions[0], m_dimensions[1]);
}
void OsgView::update(double dt)
{
- if (m_areWindowSettingsDirty)
+ if (isActive() && m_areWindowSettingsDirty)
{
osg::Camera* viewCamera = m_view->getCamera();
if (viewCamera)
{
osgViewer::GraphicsWindow* window =
dynamic_cast<osgViewer::GraphicsWindow*>(viewCamera->getGraphicsContext());
- if (window)
+ if (window && !isFullScreen())
{
window->setWindowDecoration(m_isWindowBorderEnabled);
window->setWindowRectangle(m_position[0], m_position[1], m_dimensions[0], m_dimensions[1]);
m_areWindowSettingsDirty = false;
}
+
+ }
+ }
+ else if (isActive())
+ {
+ if (!isStereo())
+ {
+ m_dimensions[0] = m_view->getCamera()->getGraphicsContext()->getTraits()->width;
+ m_dimensions[1] = m_view->getCamera()->getGraphicsContext()->getTraits()->height;
+ }
+ }
+ if (isManipulatorEnabled())
+ {
+ auto matrix = m_view->getCameraManipulator()->getMatrix();
+ auto pose = Math::RigidTransform3d(fromOsg(matrix));
+ auto element = getSceneElement();
+ if (element != nullptr)
+ {
+ element->setPose(pose);
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(Framework::Logger::getDefaultLogger()) << getFullName()
+ << " is not in a SceneElement, unable to set the SceneElement pose with the camera manipulator.";
}
}
}
@@ -183,22 +234,7 @@ bool OsgView::doInitialize()
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);
+ m_view->setDisplaySettings(createDisplaySettings());
osg::ref_ptr<osgViewer::ViewConfig> viewConfig;
@@ -218,6 +254,7 @@ bool OsgView::doWakeUp()
if (isFullScreen())
{
m_view->setUpViewOnSingleScreen(getTargetScreen());
+ m_isWindowBorderEnabled = false;
}
else
{
@@ -236,10 +273,28 @@ bool OsgView::doWakeUp()
m_view->getCamera()->getGraphicsContext()->getState()->setUseVertexAttributeAliasing(true);
}
-
return true;
}
+osg::ref_ptr<osg::DisplaySettings> OsgView::createDisplaySettings() const
+{
+ 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()));
+ }
+
+ return displaySettings;
+}
+
void OsgView::fixupStatsHandler(osgViewer::StatsHandler* statsHandler)
{
// use ref_ptr in case loading fails we don't have to clean up
@@ -254,7 +309,7 @@ void OsgView::fixupStatsHandler(osgViewer::StatsHandler* statsHandler)
}
else
{
- SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ SURGSIM_LOG_WARNING(Framework::Logger::getDefaultLogger())
<< "Could not find Shaders/osg_statshandler.vert, the osg stats "
<< "display will probably not work correctly.";
success = false;
@@ -266,7 +321,7 @@ void OsgView::fixupStatsHandler(osgViewer::StatsHandler* statsHandler)
}
else
{
- SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ SURGSIM_LOG_WARNING(Framework::Logger::getDefaultLogger())
<< "Could not find Shaders/osg_statshandler.frag, the osg stats "
<< "display will probably not work correctly.";
success = false;
@@ -289,6 +344,38 @@ void OsgView::fixupStatsHandler(osgViewer::StatsHandler* statsHandler)
}
}
+int OsgView::doSetTargetScreen(int val)
+{
+ osg::GraphicsContext::WindowingSystemInterface* wsi =
+ osg::GraphicsContext::getWindowingSystemInterface();
+
+ if (!wsi)
+ {
+ SURGSIM_FAILURE() << "Error, no WindowSystemInterface available, cannot create windows. ";
+ }
+
+ int numScreens = wsi->getNumScreens();
+
+ if (val >= numScreens)
+ {
+ val = numScreens - 1;
+ }
+
+ if (val < 0)
+ {
+ val = 0;
+ }
+
+ unsigned int width, height;
+ wsi->getScreenResolution(osg::GraphicsContext::ScreenIdentifier(val), width, height);
+
+ m_screenDimensions[0] = static_cast<int>(width);
+ m_screenDimensions[1] = static_cast<int>(height);
+
+ return val;
+
+}
+
void SurgSim::Graphics::OsgView::setOsgMapsUniforms(bool val)
{
SURGSIM_ASSERT(!isAwake()) << "Can't change mapping mode after waking up.";
@@ -309,17 +396,15 @@ void SurgSim::Graphics::OsgView::enableManipulator(bool val)
{
if (m_manipulator == nullptr)
{
- m_manipulator = new SurgSim::Graphics::OsgTrackballZoomManipulator();
+ m_manipulator = new 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));
+ m_manipulator->setTransformation(toOsg(m_manipulatorPosition), toOsg(m_manipulatorLookat),
+ osg::Vec3d(0.0f, 1.0f, 0.0f));
}
if (val)
{
- getOsgView()->setCameraManipulator(m_manipulator);
+ getOsgView()->setCameraManipulator(m_manipulator, false);
}
else
{
@@ -342,7 +427,7 @@ void SurgSim::Graphics::OsgView::enableKeyboardDevice(bool val)
std::shared_ptr<SurgSim::Input::CommonDevice> keyboardDevice = getKeyboardDevice();
osg::ref_ptr<osgGA::GUIEventHandler> keyboardHandle =
- std::static_pointer_cast<SurgSim::Device::KeyboardDevice>(keyboardDevice)->getKeyboardHandler();
+ std::static_pointer_cast<Devices::KeyboardDevice>(keyboardDevice)->getKeyboardHandler();
if (val)
{
getOsgView()->addEventHandler(keyboardHandle);
@@ -364,7 +449,7 @@ std::shared_ptr<SurgSim::Input::CommonDevice> SurgSim::Graphics::OsgView::getKey
{
if (m_keyboardDevice == nullptr)
{
- m_keyboardDevice = std::make_shared<SurgSim::Device::KeyboardDevice>("Keyboard");
+ m_keyboardDevice = std::make_shared<Devices::KeyboardDevice>("Keyboard");
if (!m_keyboardDevice->isInitialized())
{
m_keyboardDevice->initialize();
@@ -381,9 +466,9 @@ void SurgSim::Graphics::OsgView::enableMouseDevice(bool val)
return;
}
- std::shared_ptr<SurgSim::Input::CommonDevice> mouseDevice = getMouseDevice();
+ std::shared_ptr<Input::CommonDevice> mouseDevice = getMouseDevice();
osg::ref_ptr<osgGA::GUIEventHandler> mouseHandler =
- std::static_pointer_cast<SurgSim::Device::MouseDevice>(mouseDevice)->getMouseHandler();
+ std::static_pointer_cast<Devices::MouseDevice>(mouseDevice)->getMouseHandler();
if (val)
{
getOsgView()->addEventHandler(mouseHandler);
@@ -405,7 +490,7 @@ std::shared_ptr<SurgSim::Input::CommonDevice> SurgSim::Graphics::OsgView::getMou
{
if (m_mouseDevice == nullptr)
{
- m_mouseDevice = std::make_shared<SurgSim::Device::MouseDevice>("Mouse");
+ m_mouseDevice = std::make_shared<Devices::MouseDevice>("Mouse");
if (!m_mouseDevice->isInitialized())
{
m_mouseDevice->initialize();
@@ -423,10 +508,8 @@ void SurgSim::Graphics::OsgView::setManipulatorParameters(const SurgSim::Math::V
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));
+ m_manipulator->setTransformation(toOsg(m_manipulatorPosition), toOsg(m_manipulatorLookat),
+ osg::Vec3d(0.0f, 1.0f, 0.0f));
}
}
@@ -451,4 +534,4 @@ SurgSim::Math::Vector3d SurgSim::Graphics::OsgView::getManipulatorLookAt()
}
}
-}
\ No newline at end of file
+}
diff --git a/SurgSim/Graphics/OsgView.h b/SurgSim/Graphics/OsgView.h
index f53448e..ea62f95 100644
--- a/SurgSim/Graphics/OsgView.h
+++ b/SurgSim/Graphics/OsgView.h
@@ -34,7 +34,7 @@ namespace Input
class CommonDevice;
}
-namespace Device
+namespace Devices
{
class KeyboardDevice;
class MouseDevice;
@@ -69,23 +69,27 @@ public:
SURGSIM_CLASSNAME(SurgSim::Graphics::OsgView);
- virtual void setPosition(const std::array<int, 2>& position) override;
+ void setPosition(const std::array<int, 2>& position) override;
- virtual std::array<int, 2> getPosition() const override;
+ std::array<int, 2> getPosition() const override;
- virtual void setDimensions(const std::array<int, 2>& dimensions) override;
+ void setDimensions(const std::array<int, 2>& dimensions) override;
- virtual std::array<int, 2> getDimensions() const override;
+ std::array<int, 2> getDimensions() const override;
- virtual void setWindowBorderEnabled(bool enabled) override;
+ void setDimensionsDouble(const std::array<double, 2>& dimensions) override;
- virtual bool isWindowBorderEnabled() const override;
+ std::array<double, 2> getDimensionsDouble() const override;
+
+ void setWindowBorderEnabled(bool enabled) override;
+
+ 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;
+ 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.
@@ -145,7 +149,7 @@ public:
/// \return Whether the mouse device is enabled.
bool isMouseDeviceEnabled();
- virtual void update(double dt) override;
+ 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;
@@ -153,10 +157,17 @@ public:
protected:
/// Initialize the view
/// \post The view's window is setup.
- virtual bool doInitialize() override;
+ bool doInitialize() override;
/// Wake up the view
- virtual bool doWakeUp() override;
+ bool doWakeUp() override;
+
+ /// Determine Screen Size
+ int doSetTargetScreen(int val) override;
+
+
+ /// \return The DisplaySettings to be used in this view
+ virtual osg::ref_ptr<osg::DisplaySettings> createDisplaySettings() const;
private:
/// Patch the StatsHandler rendering
@@ -167,6 +178,7 @@ private:
std::array<int, 2> m_position;
/// Dimensions of the view on the screen (in pixels)
std::array<int, 2> m_dimensions;
+ std::array<int, 2> m_screenDimensions;
/// Whether the view window has a border
bool m_isWindowBorderEnabled;
@@ -188,11 +200,11 @@ private:
/// Indicate if a keyboard device is enabled
bool m_keyboardEnabled;
- std::shared_ptr<SurgSim::Device::KeyboardDevice> m_keyboardDevice;
+ std::shared_ptr<SurgSim::Devices::KeyboardDevice> m_keyboardDevice;
/// Indicate if a mouse device is enabled
bool m_mouseEnabled;
- std::shared_ptr<SurgSim::Device::MouseDevice> m_mouseDevice;
+ std::shared_ptr<SurgSim::Devices::MouseDevice> m_mouseDevice;
};
}; // namespace Graphics
diff --git a/SurgSim/Graphics/OsgViewElement.cpp b/SurgSim/Graphics/OsgViewElement.cpp
index ee806ee..82fe76b 100644
--- a/SurgSim/Graphics/OsgViewElement.cpp
+++ b/SurgSim/Graphics/OsgViewElement.cpp
@@ -33,8 +33,8 @@ OsgViewElement::OsgViewElement(const std::string& name) :
m_keyboardEnabled(false),
m_mouseEnabled(false)
{
- setView(std::make_shared<OsgView>(name + " View"));
- setCamera(std::make_shared<OsgCamera>(name + " Camera"));
+ setView(std::make_shared<OsgView>("View"));
+ setCamera(std::make_shared<OsgCamera>("Camera"));
getCamera()->setRenderGroupReference(Representation::DefaultGroupName);
}
@@ -97,7 +97,7 @@ std::shared_ptr<SurgSim::Input::CommonDevice> SurgSim::Graphics::OsgViewElement:
void SurgSim::Graphics::OsgViewElement::setManipulatorParameters(const SurgSim::Math::Vector3d& position,
- const SurgSim::Math::Vector3d& lookat)
+ 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
index b7a6c99..bede635 100644
--- a/SurgSim/Graphics/OsgViewElement.h
+++ b/SurgSim/Graphics/OsgViewElement.h
@@ -52,7 +52,7 @@ public:
/// 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;
+ 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.
@@ -67,17 +67,17 @@ public:
/// Return the keyboard to be used with this view.
/// \return A keyboard device
- virtual std::shared_ptr<SurgSim::Input::CommonDevice> getKeyboardDevice() override;
+ 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;
+ 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;
+ 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;
+ void enableMouseDevice(bool val) override;
private:
/// Indicate if a keyboard device is enabled
diff --git a/SurgSim/Graphics/PaintBehavior.cpp b/SurgSim/Graphics/PaintBehavior.cpp
new file mode 100644
index 0000000..c34dfbe
--- /dev/null
+++ b/SurgSim/Graphics/PaintBehavior.cpp
@@ -0,0 +1,314 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+
+#include "SurgSim/DataStructures/Image.h"
+#include "SurgSim/DataStructures/ImageMap.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/PaintBehavior.h"
+#include "SurgSim/Math/Scalar.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Graphics::PaintBehavior, PaintBehavior);
+
+PaintBehavior::PaintBehavior(const std::string& name) :
+ Framework::Behavior(name),
+ m_width(0),
+ m_height(0),
+ m_radius(0),
+ m_antialias(false)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(PaintBehavior, std::shared_ptr<Framework::Component>, Representation,
+ getRepresentation, setRepresentation);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(PaintBehavior, Math::Vector4d, Color, getColor, setColor);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(PaintBehavior, bool, AntiAlias, getAntiAlias, setAntiAlias);
+}
+
+void PaintBehavior::setRepresentation(std::shared_ptr<Framework::Component> representation)
+{
+ m_representation = Framework::checkAndConvert<Graphics::OsgMeshRepresentation>(representation,
+ "SurgSim::Graphics::Representation");
+}
+
+std::shared_ptr<Graphics::OsgMeshRepresentation> PaintBehavior::getRepresentation() const
+{
+ return m_representation;
+}
+
+void PaintBehavior::setColor(const Math::Vector4d& color)
+{
+ m_color = color;
+}
+
+Math::Vector4d PaintBehavior::getColor() const
+{
+ return m_color;
+}
+
+void PaintBehavior::setAntiAlias(bool antialias)
+{
+ if (isInitialized())
+ {
+ SURGSIM_LOG_WARNING(Framework::Logger::getDefaultLogger()) << "You cannot set anti-aliasing of " << getName() <<
+ " after it has already been initialized.";
+ }
+ else
+ {
+ m_antialias = antialias;
+ }
+}
+
+bool PaintBehavior::getAntiAlias() const
+{
+ return m_antialias;
+}
+
+void PaintBehavior::setCoordinates(const std::vector<DataStructures::IndexedLocalCoordinate>& coordinates)
+{
+ boost::unique_lock<boost::mutex> lock(m_mutex);
+ m_coordinates = coordinates;
+}
+
+void PaintBehavior::setTextureSize(int width, int height)
+{
+ if (isInitialized())
+ {
+ SURGSIM_LOG_WARNING(Framework::Logger::getDefaultLogger()) << "You cannot set texture size of " << getName() <<
+ " after it has already been initialized.";
+ }
+ else
+ {
+ m_width = width;
+ m_height = height;
+ }
+}
+
+bool PaintBehavior::doInitialize()
+{
+ auto textureUniform = std::make_shared<Graphics::OsgTextureUniform<Graphics::OsgTexture2d>>("paintMap");
+
+ m_texture = std::make_shared<OsgTexture2d>();
+ m_texture->setSize(m_width, m_height);
+ osg::Texture* osgTexture = m_texture->getOsgTexture();
+ osgTexture->setDataVariance(osg::Object::DYNAMIC);
+ 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);
+
+ int textureSize = m_width * m_height * 4;
+ auto data = new unsigned char[textureSize]();
+ for (int i = 0; i < textureSize; i++)
+ {
+ data[i] = 0;
+ }
+
+ osg::ref_ptr<osg::Image> image = new osg::Image();
+ image->setImage(m_width, m_height, 1, GL_RGBA32F_ARB, GL_RGBA, GL_UNSIGNED_BYTE, data, osg::Image::NO_DELETE);
+
+ m_texture->getOsgTexture2d()->setImage(image);
+ textureUniform->set(m_texture);
+ m_representation->getMaterial()->addUniform(textureUniform);
+ return true;
+}
+
+bool PaintBehavior::doWakeUp()
+{
+ if (m_representation == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(Framework::Logger::getDefaultLogger()) << getClassName() << " named " << getName() <<
+ " does not have a graphics representation to paint into";
+ return false;
+ }
+
+ if (m_texture == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(Framework::Logger::getDefaultLogger()) << getClassName() << " named " << getName() <<
+ " does not have a texture to paint onto";
+ return false;
+ }
+
+ if (m_radius <= 0.0 || m_radius > 1.0)
+ {
+ SURGSIM_LOG_SEVERE(Framework::Logger::getDefaultLogger()) << getClassName() << " named " << getName() <<
+ " does not have a radius set";
+ return false;
+ }
+
+ if (m_antialias)
+ {
+ buildAntiAliasedBrush(getRadius());
+ }
+ else
+ {
+ buildBrush(getRadius());
+ }
+
+ return true;
+}
+
+void PaintBehavior::update(double dt)
+{
+ std::vector<Math::Vector2d> uvCoordinates;
+
+ {
+ boost::unique_lock<boost::mutex> lock(m_mutex);
+ auto mesh = m_representation->getMesh();
+ for (auto coordinate : m_coordinates)
+ {
+ if (coordinate.index < mesh->getNumTriangles())
+ {
+ auto vertex1 = mesh->getVertex(mesh->getTriangle(coordinate.index).verticesId[0]);
+ auto vertex2 = mesh->getVertex(mesh->getTriangle(coordinate.index).verticesId[1]);
+ auto vertex3 = mesh->getVertex(mesh->getTriangle(coordinate.index).verticesId[2]);
+ Math::Vector2d uv = vertex1.data.texture.getValue() * coordinate.coordinate[0] +
+ vertex2.data.texture.getValue() * coordinate.coordinate[1] +
+ vertex3.data.texture.getValue() * coordinate.coordinate[2];
+ uvCoordinates.push_back(uv);
+ }
+ }
+ }
+
+ for (auto& uv : uvCoordinates)
+ {
+ Math::Vector2d xy = toPixel(uv);
+
+ paint(xy);
+
+ m_texture->getOsgTexture2d()->dirtyTextureObject();
+ }
+}
+
+void PaintBehavior::setRadius(double radius)
+{
+ m_radius = radius;
+}
+
+double PaintBehavior::getRadius() const
+{
+ return m_radius;
+}
+
+void PaintBehavior::buildBrush(double radius)
+{
+ int brushWidth = static_cast<int>(ceil(2 * radius * m_width));
+ int brushHeight = static_cast<int>(ceil(2 * radius * m_height));
+
+ int centerX = brushWidth / 2;
+ int centerY = brushHeight / 2;
+
+ m_brushOffsetX = -1 * centerX;
+ m_brushOffsetY = -1 * centerY;
+
+ m_brush.resize(brushWidth, brushHeight);
+
+ for (int i = 0; i < m_brush.cols(); i++)
+ {
+ for (int j = 0; j < m_brush.rows(); j++)
+ {
+ double mag = ((i - centerX) * (i - centerX)) + ((j - centerY) * (j - centerY));
+ if (sqrt(mag) < m_brush.cols() / 2.0)
+ {
+ m_brush(i, j) = 1.0;
+ }
+ else
+ {
+ m_brush(i, j) = 0.0;
+ }
+ }
+ }
+}
+
+void PaintBehavior::buildAntiAliasedBrush(double radius)
+{
+ static const double sqrt2Half = 0.7071067811865475;
+ double dist;
+ int brushWidth = static_cast<int>(ceil(2 * radius * m_width));
+ int brushHeight = static_cast<int>(ceil(2 * radius * m_height));
+
+ int centerX = brushWidth / 2;
+ int centerY = brushHeight / 2;
+
+ m_brushOffsetX = -1 * centerX;
+ m_brushOffsetY = -1 * centerY;
+
+ m_brush.resize(brushWidth, brushHeight);
+
+ for (int i = 0; i < m_brush.cols(); i++)
+ {
+ for (int j = 0; j < m_brush.rows(); j++)
+ {
+ double mag = (i - centerX) * (i - centerX) + (j - centerY) * (j - centerY);
+ dist = (m_brush.cols() / 2) - sqrt(mag);
+ dist /= m_brush.cols() / 2;
+ if (dist > sqrt2Half)
+ {
+ m_brush(i, j) = 1.0;
+ }
+ else if (dist < 0.0)
+ {
+ m_brush(i, j) = 0.0;
+ }
+ else
+ {
+ m_brush(i, j) = sqrt(2) * dist;
+ }
+ }
+ }
+}
+
+Math::Vector2d PaintBehavior::toPixel(const Math::Vector2d& uv)
+{
+ double s = uv[0];
+ double t = uv[1];
+
+ s = Math::clamp(s, 0.0, 1.0, 0.0);
+ t = Math::clamp(t, 0.0, 1.0, 0.0);
+
+ Math::Vector2d xy;
+ xy << floor(s * m_width + 0.5), floor(t * m_height + 0.5);
+
+ return xy;
+}
+
+void PaintBehavior::paint(const Math::Vector2d& coordinates)
+{
+ size_t numChannels = 4;
+
+ DataStructures::ImageMap<unsigned char> image(m_width,
+ m_height,
+ numChannels,
+ m_texture->getOsgTexture2d()->getImage()->data());
+
+ size_t topLeftX = static_cast<size_t>(std::max(0.0, coordinates[0] + m_brushOffsetX));
+ size_t topLeftY = static_cast<size_t>(std::max(0.0, coordinates[1] + m_brushOffsetY));
+ size_t bottomRightX = std::min(m_width - topLeftX, static_cast<size_t>(m_brush.cols()));
+ size_t bottomRightY = std::min(m_height - topLeftY, static_cast<size_t>(m_brush.rows()));
+ for (size_t channel = 0; channel < numChannels; channel++)
+ {
+ auto imageBlock = image.getChannel(channel).block(topLeftX, topLeftY, bottomRightX, bottomRightY);
+ auto brushBlock = m_brush.topLeftCorner(bottomRightX, bottomRightY);
+ imageBlock = (brushBlock.array() > 0).select((brushBlock * m_color(channel)*255).template cast<unsigned char>(),
+ imageBlock);
+ }
+}
+
+} // Graphics
+} // SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/PaintBehavior.h b/SurgSim/Graphics/PaintBehavior.h
new file mode 100644
index 0000000..834db31
--- /dev/null
+++ b/SurgSim/Graphics/PaintBehavior.h
@@ -0,0 +1,139 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_PAINTBEHAVIOR_H
+#define SURGSIM_GRAPHICS_PAINTBEHAVIOR_H
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/Logger.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Graphics/OsgTexture2d.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+SURGSIM_STATIC_REGISTRATION(PaintBehavior);
+
+/// Behavior class to allow a specified scene element to receive painting effects
+class PaintBehavior : public Framework::Behavior
+{
+public:
+ explicit PaintBehavior(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Graphics::PaintBehavior);
+
+ /// Sets graphics representation being painted on
+ /// \param representation Graphics representation pointer
+ void setRepresentation(std::shared_ptr<Framework::Component> representation);
+
+ /// Gets graphics representation being painted on
+ /// \return Shared pointer to a graphics representation
+ std::shared_ptr<Graphics::OsgMeshRepresentation> getRepresentation() const;
+
+ /// Sets the size of the texture layer to paint onto
+ /// \param width texture width
+ /// \param height texture height
+ void setTextureSize(int width, int height);
+
+ /// Sets color of the paint
+ /// \param color RGBA color in [0-1] range
+ void setColor(const Math::Vector4d& color);
+
+ /// Gets color of the paint
+ /// \return Vector4d representation of RGBA color in [0-1] range
+ Math::Vector4d getColor() const;
+
+ /// Sets radius of paint splat
+ /// \param radius Radius in texture coordinate range [0-1]
+ void setRadius(double radius);
+
+ /// Gets radius of paint splat
+ /// \return Radius in texture coordinate range [0-1]
+ double getRadius() const;
+
+ /// Sets whether to anti-alias the brush
+ /// \param antialias True enables AA, False disables
+ void setAntiAlias(bool antialias);
+
+ /// Gets status of antialiased brush
+ /// \return Boolean indicating if antialiasing is on
+ bool getAntiAlias() const;
+
+ /// Sets collection of local triangle coordinates to paint on during next update
+ /// \param coordinate Standard vector of IndexedLocalCoordinates
+ void setCoordinates(const std::vector<DataStructures::IndexedLocalCoordinate>& coordinate);
+
+ bool doInitialize() override;
+ bool doWakeUp() override;
+
+ void update(double dt) override;
+
+private:
+
+ /// Builds paint brush at the set radius size
+ void buildBrush(double radius);
+
+ /// Builds an antialiased brush at the set radius size
+ void buildAntiAliasedBrush(double radius);
+
+ /// Convert texture uv coordinates to pixel coordinates
+ Math::Vector2d toPixel(const Math::Vector2d& uv);
+
+ /// Apply paint brush to texture at specified texture coordinates
+ void paint(const Math::Vector2d& coordinates);
+
+ /// Graphics representation of the mesh to apply behavior to
+ std::shared_ptr<Graphics::OsgMeshRepresentation> m_representation;
+
+ /// Image data of the texture to be used as the decal layer
+ std::shared_ptr<Graphics::OsgTexture2d> m_texture;
+
+ /// Color to use for decal painting
+ Math::Vector4d m_color;
+
+ /// Width of assigned texture
+ int m_width;
+
+ /// Height of assigned texture
+ int m_height;
+
+ /// Radius of brush
+ double m_radius;
+
+ /// Flag for antialiasing the brush
+ bool m_antialias;
+
+ /// Collection of UV texture coordinates to paint to on next update
+ std::vector<DataStructures::IndexedLocalCoordinate> m_coordinates;
+
+ int m_brushOffsetX;
+ int m_brushOffsetY;
+
+ Math::Matrix m_brush;
+
+ boost::mutex m_mutex;
+};
+
+} // Graphics
+} // SurgSim
+
+#endif // SURGSIM_GRAPHICS_PAINTBEHAVIOR_H
\ No newline at end of file
diff --git a/SurgSim/Graphics/PointCloudRepresentation.cpp b/SurgSim/Graphics/PointCloudRepresentation.cpp
index 40f1cd2..35e8ae5 100644
--- a/SurgSim/Graphics/PointCloudRepresentation.cpp
+++ b/SurgSim/Graphics/PointCloudRepresentation.cpp
@@ -26,11 +26,28 @@ PointCloudRepresentation::PointCloudRepresentation(const std::string& name) : Re
{
SURGSIM_ADD_SERIALIZABLE_PROPERTY(PointCloudRepresentation, double, PointSize, getPointSize, setPointSize);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(PointCloudRepresentation, SurgSim::Math::Vector4d, Color, getColor, setColor);
+
+ // Provides a common entry point for representations taking DataStructure::VerticesPlain
+ auto converter = std::bind(SurgSim::Framework::convert<DataStructures::VerticesPlain>, std::placeholders::_1);
+ auto functor = std::bind((void(PointCloudRepresentation::*)(const DataStructures::VerticesPlain&))
+ &PointCloudRepresentation::updateVertices, this, converter);
+
+ setSetter("Vertices", functor);
}
PointCloudRepresentation::~PointCloudRepresentation()
{
}
+void PointCloudRepresentation::updateVertices(const DataStructures::VerticesPlain& vertices)
+{
+ m_locker.set(vertices);
+}
+
+void PointCloudRepresentation::updateVertices(DataStructures::VerticesPlain&& vertices)
+{
+ m_locker.set(std::move(vertices));
+}
+
}; // Graphics
}; // SurgSim
diff --git a/SurgSim/Graphics/PointCloudRepresentation.h b/SurgSim/Graphics/PointCloudRepresentation.h
index d930276..45d4750 100644
--- a/SurgSim/Graphics/PointCloudRepresentation.h
+++ b/SurgSim/Graphics/PointCloudRepresentation.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -20,6 +20,7 @@
#include "SurgSim/DataStructures/EmptyData.h"
#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Framework/LockedContainer.h"
#include "SurgSim/Graphics/Representation.h"
#include "SurgSim/Math/MathConvert.h"
#include "SurgSim/Math/Vector.h"
@@ -46,10 +47,12 @@ public:
virtual std::shared_ptr<PointCloud> getVertices() const = 0;
/// Sets point size for the point elements.
+ /// The point size defines the the width and height of the drawn square, in window pixels.
/// \param val The value.
virtual void setPointSize(double val) = 0;
/// Gets point size.
+ /// The point size defines the the width and height of the drawn square, in window pixels.
/// \return The point size.
virtual double getPointSize() const = 0;
@@ -60,6 +63,14 @@ public:
/// Gets the color.
/// \return The current color.
virtual SurgSim::Math::Vector4d getColor() const = 0;
+
+ void updateVertices(const DataStructures::VerticesPlain& vertices);
+
+ void updateVertices(DataStructures::VerticesPlain&& vertices);
+
+protected:
+
+ Framework::LockedContainer<DataStructures::VerticesPlain> m_locker;
};
}; // Graphics
diff --git a/SurgSim/Graphics/Program.h b/SurgSim/Graphics/Program.h
new file mode 100644
index 0000000..bf3152e
--- /dev/null
+++ b/SurgSim/Graphics/Program.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_GRAPHICS_PROGRAM_H
+#define SURGSIM_GRAPHICS_PROGRAM_H
+
+#include <string>
+#include "SurgSim/Framework/Accessible.h"
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+class Asset;
+}
+
+namespace Graphics
+{
+
+/// Base class that defines the interface for graphics programs.
+///
+/// A program in OSS is a collection of 'shaders' that are usually executed in sequence (Geometry,
+/// Vertex and Fragment). Each shader itself is software that is executed on the GPU, shaders are usually introduced
+/// as source. The concept is similar to the use of program in
+/// OpenGL see https://www.opengl.org/sdk/docs/man4/html/glCreateProgram.xhtml
+class Program
+{
+public:
+
+ /// Destructor
+ virtual ~Program() = 0;
+
+ /// \return 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 loadVertexShader(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;
+
+ /// \return 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 loadGeometryShader(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;
+
+
+ /// \return true if the fragment shader has been set, otherwise false.
+ virtual bool hasFragmentShader() const = 0;
+
+ /// \return 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 loadFragmentShader(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 Program::~Program()
+{
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_PROGRAM_H
diff --git a/SurgSim/Graphics/RenderPass.cpp b/SurgSim/Graphics/RenderPass.cpp
index bfa14b2..ed57100 100644
--- a/SurgSim/Graphics/RenderPass.cpp
+++ b/SurgSim/Graphics/RenderPass.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -70,12 +70,12 @@ std::shared_ptr<RenderTarget> RenderPass::getRenderTarget()
return m_renderTarget;
}
-bool RenderPass::setMaterial(std::shared_ptr<Material> material)
+bool RenderPass::setMaterial(std::shared_ptr<SurgSim::Framework::Component> material)
{
bool result = m_camera->setMaterial(material);
if (result)
{
- m_material = material;
+ m_material = std::dynamic_pointer_cast<Material>(material);
}
return result;
}
@@ -101,7 +101,7 @@ void RenderPass::showColorTarget(int x, int y, int width, int height)
{
m_debugColor->setLocation(x, y);
m_debugColor->setSize(width, height);
- m_debugColor->setVisible(true);
+ m_debugColor->setLocalActive(true);
}
}
@@ -109,7 +109,7 @@ void RenderPass::hideColorTarget()
{
if (m_debugColor != nullptr)
{
- m_debugColor->setVisible(false);
+ m_debugColor->setLocalActive(false);
}
}
@@ -124,7 +124,7 @@ void RenderPass::showDepthTarget(int x, int y, int width, int height)
{
m_debugDepth->setLocation(x, y);
m_debugDepth->setSize(width, height);
- m_debugDepth->setVisible(true);
+ m_debugDepth->setLocalActive(true);
}
}
@@ -132,12 +132,12 @@ void RenderPass::hideDepthTarget()
{
if (m_debugDepth != nullptr)
{
- m_debugDepth->setVisible(false);
+ m_debugDepth->setLocalActive(false);
}
}
std::shared_ptr<ScreenSpaceQuadRepresentation> RenderPass::buildDebugQuad(const std::string& name,
- std::shared_ptr<Texture> texture)
+ std::shared_ptr<Texture> texture)
{
auto result = std::make_shared<OsgScreenSpaceQuadRepresentation>(name);
result->setTexture(texture);
diff --git a/SurgSim/Graphics/RenderPass.h b/SurgSim/Graphics/RenderPass.h
index 46646f8..39e8b30 100644
--- a/SurgSim/Graphics/RenderPass.h
+++ b/SurgSim/Graphics/RenderPass.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -58,7 +58,7 @@ public:
/// Executes the initialize operation.
/// \return true if it succeeds, false if it fails.
- virtual bool doInitialize() override;
+ 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.
@@ -81,7 +81,7 @@ public:
/// 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);
+ bool setMaterial(std::shared_ptr<SurgSim::Framework::Component> material);
/// Gets the current material.
/// \return The material.
diff --git a/SurgSim/Graphics/RenderTests/CMakeLists.txt b/SurgSim/Graphics/RenderTests/CMakeLists.txt
index c7d081f..21b03a2 100644
--- a/SurgSim/Graphics/RenderTests/CMakeLists.txt
+++ b/SurgSim/Graphics/RenderTests/CMakeLists.txt
@@ -18,22 +18,27 @@ include_directories (
)
set(UNIT_TEST_SOURCES
+ ImplicitSurfaceRenderTests.cpp
OsgBoxRepresentationRenderTests.cpp
OsgCameraRenderTests.cpp
OsgCapsuleRepresentationRenderTests.cpp
+ OsgCurveRepresentationRenderTests.cpp
OsgCylinderRepresentationRenderTests.cpp
OsgManagerRenderTests.cpp
OsgMeshRepresentationRenderTests.cpp
OsgOctreeRepresentationRenderTests.cpp
OsgPlaneRepresentationRenderTests.cpp
OsgPointCloudRepresentationRenderTests.cpp
+ OsgProgramRenderTests.cpp
OsgRepresentationRenderTests.cpp
OsgSceneryRepresentationRenderTests.cpp
OsgScreenSpaceQuadRenderTests.cpp
- OsgShaderRenderTests.cpp
+ OsgSkeletonRepresentationRenderTests.cpp
OsgSphereRepresentationRenderTests.cpp
+ OsgTextRepresentationRenderTests.cpp
OsgVectorFieldRepresentationRenderTests.cpp
OsgViewElementRenderTests.cpp
+ PaintBehaviorRenderTests.cpp
RenderTest.cpp
)
@@ -42,7 +47,6 @@ set(UNIT_TEST_HEADERS
)
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(
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/wound_deformable.ply b/SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/wound_deformable.ply
deleted file mode 100644
index 765f44f..0000000
--- a/SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/wound_deformable.ply
+++ /dev/null
@@ -1,2391 +0,0 @@
-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
deleted file mode 100644
index aafbea2..0000000
Binary files a/SurgSim/Graphics/RenderTests/Data/OsgScreenSpaceQuadRenderTests/CheckerBoard.png and /dev/null differ
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/L_forcep.obj b/SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/L_forcep.obj
new file mode 100644
index 0000000..f865d7b
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/L_forcep.obj
@@ -0,0 +1,132 @@
+# Blender3D v249 OBJ File: forceps_collision._blocky_tip.blend
+# www.blender3d.org
+v 0.024467 0.001716 0.000824
+v -0.020851 -0.000170 0.004465
+v 0.015221 -0.004827 -0.000001
+v -0.019522 0.003106 0.003026
+v -0.082222 -0.002056 0.008415
+v 0.015886 0.004555 0.000816
+v -0.081055 -0.004124 0.007234
+v -0.036645 0.004459 0.002150
+v -0.036274 -0.004451 0.002135
+v -0.080115 0.005198 0.005436
+v 0.015357 -0.004223 0.000838
+v -0.104150 0.003159 0.006363
+v 0.024155 0.002336 0.000013
+v 0.000061 -0.003838 0.001019
+v -0.104285 -0.003012 0.006608
+v 0.001048 0.004519 0.000157
+v 0.024474 -0.000834 0.000810
+v -0.082213 0.001829 0.008614
+v -0.080297 0.004058 0.007428
+v -0.019028 -0.003582 0.001435
+v -0.019202 -0.003043 0.003002
+v -0.018394 0.003543 0.001425
+v 0.000090 -0.004398 0.000012
+v 0.015714 0.004829 0.000008
+v 0.024297 -0.001433 0.000006
+v -0.081263 -0.005133 0.005527
+v -0.057169 -0.002681 0.006440
+v -0.055761 -0.005151 0.005118
+v -0.037785 -0.001029 0.005453
+v -0.036256 -0.003701 0.004060
+v -0.057356 0.002278 0.006540
+v -0.055734 0.005031 0.005227
+v -0.037930 0.000571 0.005502
+v -0.036454 0.003585 0.004126
+v -0.130109 0.002182 0.007166
+v -0.134452 0.001378 0.009129
+v -0.130427 -0.002319 0.007300
+v -0.134419 -0.001339 0.009097
+v -0.130501 -0.000018 0.007319
+v -0.134581 0.000047 0.009374
+v -0.134454 -0.000680 0.005675
+v -0.134470 0.000678 0.005691
+v -0.132460 -0.000679 0.005686
+v -0.132476 0.000680 0.005702
+usemtl (null)
+s 1
+f 26 15 12
+f 21 20 23
+f 20 22 16
+f 26 7 15
+f 4 21 14
+f 40 38 5
+f 23 20 16
+f 14 16 4
+f 5 15 7
+f 12 35 36
+f 5 38 15
+f 3 25 11
+f 26 12 10
+f 6 1 13
+f 14 23 11
+f 14 21 23
+f 38 37 15
+f 1 17 13
+f 4 2 21
+f 4 16 22
+f 23 3 11
+f 24 6 13
+f 24 16 6
+f 11 25 17
+f 13 17 25
+f 2 33 29
+f 7 26 28
+f 9 28 26
+f 9 30 28
+f 8 10 32
+f 8 32 34
+f 10 19 32
+f 10 12 19
+f 3 24 25
+f 24 13 25
+f 3 23 16
+f 3 16 24
+f 11 6 16
+f 11 16 14
+f 1 6 17
+f 6 11 17
+f 20 21 9
+f 21 30 9
+f 8 22 20
+f 8 20 9
+f 4 22 8
+f 4 8 34
+f 8 9 10
+f 9 26 10
+f 5 27 31
+f 5 31 18
+f 27 29 33
+f 27 33 31
+f 34 32 33
+f 32 31 33
+f 4 34 2
+f 34 33 2
+f 32 19 31
+f 19 18 31
+f 29 27 28
+f 29 28 30
+f 2 29 30
+f 2 30 21
+f 27 5 7
+f 27 7 28
+f 12 36 18
+f 12 18 19
+f 5 18 40
+f 36 40 18
+f 35 12 39
+f 39 12 15
+f 39 15 37
+f 41 38 40
+f 36 42 40
+f 38 43 37
+f 43 39 37
+f 39 44 35
+f 44 36 35
+f 43 38 41
+f 36 44 42
+f 40 42 41
+f 39 43 44
+f 41 42 44
+f 41 44 43
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/shader.geom b/SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/shader_axis_mirrored.geom
similarity index 100%
rename from SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/shader.geom
rename to SurgSim/Graphics/RenderTests/Data/OsgShaderRenderTests/shader_axis_mirrored.geom
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgSkeletonRepresentationRenderTests/rigged_cylinder.osgt b/SurgSim/Graphics/RenderTests/Data/OsgSkeletonRepresentationRenderTests/rigged_cylinder.osgt
new file mode 100644
index 0000000..ce5e96b
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/Data/OsgSkeletonRepresentationRenderTests/rigged_cylinder.osgt
@@ -0,0 +1,11743 @@
+#Ascii Scene
+#Version 100
+#Generator OpenSceneGraph 3.2.0
+
+osg::Group {
+ UniqueID 1
+ Name "Collada visual scene group"
+ StateSet TRUE {
+ osg::StateSet {
+ UniqueID 2
+ DataVariance STATIC
+ }
+ }
+ Children 1 {
+ osg::MatrixTransform {
+ UniqueID 3
+ Name "Armature"
+ StateSet TRUE {
+ osg::StateSet {
+ UniqueID 4
+ DataVariance STATIC
+ ModeList 1 {
+ GL_RESCALE_NORMAL ON|OVERRIDE
+ }
+ }
+ }
+ Children 1 {
+ osgAnimation::Skeleton {
+ UniqueID 5
+ DataVariance DYNAMIC
+ UpdateCallback TRUE {
+ osgAnimation::UpdateSkeleton {
+ UniqueID 6
+ }
+ }
+ Children 2 {
+ osgAnimation::Bone {
+ UniqueID 7
+ Name "Bone"
+ DataVariance DYNAMIC
+ UpdateCallback TRUE {
+ osgAnimation::UpdateBone {
+ UniqueID 8
+ Name "Bone"
+ StackedTransforms 1 {
+ osgAnimation::StackedMatrixElement {
+ UniqueID 9
+ Name "transform"
+ Matrix {
+ 1 0 0 0
+ 0 0 1 0
+ 0 -1 0 0
+ 0 0 0 1
+ }
+
+ }
+ }
+ }
+ }
+ Children 1 {
+ osgAnimation::Bone {
+ UniqueID 10
+ Name "Bone_002"
+ DataVariance DYNAMIC
+ UpdateCallback TRUE {
+ osgAnimation::UpdateBone {
+ UniqueID 11
+ Name "Bone_002"
+ StackedTransforms 1 {
+ osgAnimation::StackedMatrixElement {
+ UniqueID 12
+ Name "transform"
+ Matrix {
+ 1 0 0 0
+ 0 1 0 0
+ 0 0 1 0
+ 0 0.25 0 1
+ }
+
+ }
+ }
+ }
+ }
+ Children 1 {
+ osgAnimation::Bone {
+ UniqueID 13
+ Name "Bone_001"
+ DataVariance DYNAMIC
+ UpdateCallback TRUE {
+ osgAnimation::UpdateBone {
+ UniqueID 14
+ Name "Bone_001"
+ StackedTransforms 1 {
+ osgAnimation::StackedMatrixElement {
+ UniqueID 15
+ Name "transform"
+ Matrix {
+ 1 0 0 0
+ 0 1 0 0
+ 0 0 1 0
+ 0 0.25 0 1
+ }
+
+ }
+ }
+ }
+ }
+ Children 1 {
+ osgAnimation::Bone {
+ UniqueID 16
+ Name "Bone_003"
+ DataVariance DYNAMIC
+ UpdateCallback TRUE {
+ osgAnimation::UpdateBone {
+ UniqueID 17
+ Name "Bone_003"
+ StackedTransforms 1 {
+ osgAnimation::StackedMatrixElement {
+ UniqueID 18
+ Name "transform"
+ Matrix {
+ 1 0 0 0
+ 0 1 0 0
+ 0 0 1 0
+ 0 0.25 0 1
+ }
+
+ }
+ }
+ }
+ }
+ Matrix {
+ 1 0 0 0
+ 0 1 0 0
+ 0 0 1 0
+ 0 0.25 0 1
+ }
+
+ InvBindMatrixInSkeletonSpace {
+ 10 0 0 0
+ 0 0 -10 0
+ 0 10 0 0
+ 0 -0.25 0 1
+ }
+
+ }
+ }
+ Matrix {
+ 1 0 0 0
+ 0 1 0 0
+ 0 0 1 0
+ 0 0.25 0 1
+ }
+
+ InvBindMatrixInSkeletonSpace {
+ 10 0 0 0
+ 0 0 -10 0
+ 0 10 0 0
+ 0 0 0 1
+ }
+
+ }
+ }
+ Matrix {
+ 1 0 0 0
+ 0 1 0 0
+ 0 0 1 0
+ 0 0.25 0 1
+ }
+
+ InvBindMatrixInSkeletonSpace {
+ 10 0 0 0
+ 0 0 -10 0
+ 0 10 0 0
+ 0 0.25 0 1
+ }
+
+ }
+ }
+ Matrix {
+ 1 0 0 0
+ 0 0 1 0
+ 0 -1 0 0
+ 0 0 0 1
+ }
+
+ InvBindMatrixInSkeletonSpace {
+ 10 0 0 0
+ 0 0 -10 0
+ 0 10 0 0
+ 0 0.5 0 1
+ }
+
+ }
+ osg::Geode {
+ UniqueID 19
+ DataVariance DYNAMIC
+ Drawables 1 {
+ osgAnimation::RigGeometry {
+ UniqueID 20
+ DataVariance DYNAMIC
+ ComputeBoundingBoxCallback TRUE {
+ osg::ComputeBoundingBoxCallback {
+ UniqueID 21
+ }
+ }
+ SupportsDisplayList FALSE
+ UseDisplayList FALSE
+ UseVertexBufferObjects TRUE
+ UpdateCallback TRUE {
+ osg::UpdateCallback {
+ UniqueID 22
+ }
+ }
+ PrimitiveSetList 1 {
+ DrawElementsUInt GL_TRIANGLES 0 1428 {
+ 1141 653 12 1147
+ 676 20 1153 693
+ 28 1159 710 36
+ 1165 727 44 1171
+ 744 52 1177 761
+ 60 1183 779 68
+ 1189 796 76 1195
+ 813 84 762 246
+ 268 1201 847 4
+ 1207 830 92 303
+ 168 259 1213 852
+ 116 1219 858 371
+ 1225 448 103 1231
+ 876 141 1237 882
+ 377 1243 471 132
+ 1249 900 162 1255
+ 906 383 1261 488
+ 154 1267 924 186
+ 1273 930 389 1279
+ 506 178 1285 948
+ 208 1291 954 395
+ 1297 523 200 1303
+ 972 232 1309 978
+ 401 1315 540 222
+ 1321 996 253 1327
+ 1002 407 1333 557
+ 247 1339 1020 277
+ 1345 1026 413 1351
+ 574 269 1357 1044
+ 297 1363 1050 419
+ 1369 591 290 1375
+ 1068 322 1381 1074
+ 425 608 314 1086
+ 1392 1093 110 1398
+ 1099 431 1404 641
+ 363 1410 1117 348
+ 1416 1123 437 1422
+ 625 336 853 1142
+ 664 372 870 1143
+ 871 104 654 877
+ 1148 681 378 894
+ 1149 895 133 677
+ 901 1154 698 384
+ 918 1155 919 155
+ 694 925 1160 716
+ 390 942 1161 943
+ 179 711 949 1166
+ 732 396 966 1167
+ 967 201 728 973
+ 1172 750 402 990
+ 1173 991 223 745
+ 997 1178 767 408
+ 1014 1179 1015 248
+ 763 1021 1184 785
+ 414 1038 1185 1039
+ 270 780 1045 1190
+ 802 420 1062 1191
+ 1063 291 797 1069
+ 1196 819 426 1087
+ 1197 1088 315 814
+ 1094 1202 454 432
+ 1111 1203 1112 364
+ 848 1118 1208 835
+ 438 1135 1209 1136
+ 337 831 443 1214
+ 460 96 864 1215
+ 865 373 854 647
+ 1220 866 0 658
+ 1221 659 111 859
+ 860 1226 872 112
+ 455 1227 456 5
+ 449 466 1232 477
+ 122 888 1233 889
+ 379 878 670 1238
+ 890 8 461 1239
+ 462 117 883 884
+ 1244 896 118 665
+ 1245 666 13 472
+ 483 1250 494 147
+ 912 1251 913 385
+ 902 687 1256 914
+ 16 478 1257 479
+ 142 907 908 1262
+ 920 143 682 1263
+ 683 21 489 500
+ 1268 512 169 936
+ 1269 937 391 926
+ 704 1274 938 24
+ 495 1275 496 163
+ 931 932 1280 944
+ 164 699 1281 700
+ 29 507 518 1286
+ 528 192 960 1287
+ 961 397 950 722
+ 1292 962 32 513
+ 1293 514 187 955
+ 956 1298 968 188
+ 717 1299 718 37
+ 524 534 1304 546
+ 214 984 1305 985
+ 403 974 738 1310
+ 986 40 529 1311
+ 530 209 979 980
+ 1316 992 210 733
+ 1317 734 45 541
+ 552 1322 562 238
+ 1008 1323 1009 409
+ 998 756 1328 1010
+ 48 547 1329 548
+ 233 1003 1004 1334
+ 1016 234 751 1335
+ 752 53 558 568
+ 1340 579 260 1032
+ 1341 1033 415 1022
+ 773 1346 1034 56
+ 563 1347 564 254
+ 1027 1028 1352 1040
+ 255 768 1353 769
+ 61 575 585 1358
+ 596 283 1056 1359
+ 1057 421 1046 791
+ 1364 1058 64 580
+ 1365 581 278 1051
+ 1052 1370 1064 279
+ 786 1371 787 69
+ 592 602 1376 614
+ 304 1080 1377 1081
+ 427 1070 808 1382
+ 1082 72 597 1383
+ 598 298 1075 1076
+ 1387 1089 299 803
+ 1388 804 77 609
+ 636 1393 660 354
+ 1105 1394 1106 433
+ 1095 841 1399 1107
+ 88 630 1400 631
+ 349 1100 1101 1405
+ 1113 350 836 1406
+ 837 93 642 620
+ 1411 632 328 1129
+ 1412 1130 439 1119
+ 825 1417 1131 80
+ 615 1418 616 323
+ 1124 1125 1423 1137
+ 324 820 1424 821
+ 85 626 667 1144
+ 14 684 1150 22
+ 701 1156 30 719
+ 1162 38 735 1168
+ 46 753 1174 54
+ 770 1180 62 788
+ 1186 70 805 1192
+ 78 822 1198 86
+ 473 15 655 105
+ 450 365 451 6
+ 849 798 316 610
+ 338 643 832 87
+ 815 627 317 339
+ 816 79 799 611
+ 593 781 292 594
+ 71 782 576 764
+ 271 577 63 765
+ 559 746 249 560
+ 55 747 729 202
+ 542 543 47 730
+ 525 712 203 526
+ 39 713 695 156
+ 508 509 31 696
+ 678 134 490 491
+ 23 679 474 106
+ 135 714 180 204
+ 452 850 366 644
+ 94 833 817 340
+ 628 748 224 250
+ 157 136 341 137
+ 158 492 475 656
+ 107 457 1204 7
+ 159 181 510 783
+ 272 293 182 160
+ 342 318 273 225
+ 343 138 367 800
+ 294 319 295 274
+ 320 251 226 275
+ 205 183 227 321
+ 228 344 229 184
+ 345 206 230 544
+ 139 108 368 346
+ 369 645 838 1210
+ 95 637 1 648
+ 444 671 97 445
+ 9 672 467 688
+ 123 468 17 689
+ 484 705 148 485
+ 25 706 723 193
+ 501 502 33 724
+ 519 739 194 520
+ 41 740 757 239
+ 535 536 49 758
+ 553 774 240 554
+ 57 775 792 284
+ 569 570 65 793
+ 809 305 586 587
+ 73 810 826 329
+ 603 604 81 827
+ 621 842 330 622
+ 89 843 355 638
+ 649 331 306 605
+ 98 124 356 776
+ 261 241 463 1216
+ 119 125 307 357
+ 332 358 308 741
+ 215 195 309 285
+ 588 690 149 126
+ 196 170 503 286
+ 262 571 650 99
+ 359 242 216 537
+ 150 171 127 197
+ 217 172 243 263
+ 218 287 310 264
+ 707 173 151 333
+ 844 360 128 174
+ 311 175 219 265
+ 100 673 129 867
+ 1222 374 873 1228
+ 109 480 1234 144
+ 891 1240 380 897
+ 1246 140 497 1252
+ 165 915 1258 386
+ 921 1264 161 515
+ 1270 189 939 1276
+ 392 945 1282 185
+ 531 1288 211 963
+ 1294 398 969 1300
+ 207 549 1306 235
+ 987 1312 404 993
+ 1318 231 565 1324
+ 256 1011 1330 410
+ 1017 1336 252 582
+ 1342 280 1035 1348
+ 416 1041 1354 276
+ 599 1360 300 1059
+ 1366 422 1065 1372
+ 296 617 1378 325
+ 1083 1384 428 1389
+ 612 1090 661 1395
+ 113 1108 1401 434
+ 1114 1407 370 633
+ 1413 351 1132 1419
+ 440 1138 1425 347
+ 120 855 668 856
+ 375 1145 1146 874
+ 657 145 879 685
+ 880 381 1151 1152
+ 898 680 166 903
+ 702 904 387 1157
+ 1158 922 697 190
+ 927 720 928 393
+ 1163 1164 946 715
+ 212 951 736 952
+ 399 1169 1170 970
+ 731 236 975 754
+ 976 405 1175 1176
+ 994 749 257 999
+ 771 1000 411 1181
+ 1182 1018 766 281
+ 1023 789 1024 417
+ 1187 1188 1042 784
+ 301 1047 806 1048
+ 423 1193 1194 1066
+ 801 326 1071 823
+ 1072 429 1199 1200
+ 1091 818 114 1096
+ 458 1097 435 1205
+ 1206 1115 851 352
+ 1120 839 1121 441
+ 1211 1212 1139 834
+ 10 446 464 447
+ 101 1217 1218 868
+ 857 102 651 869
+ 652 2 1223 1224
+ 662 861 376 862
+ 875 863 115 1229
+ 1230 459 453 18
+ 469 481 470 130
+ 1235 1236 892 881
+ 131 674 893 675
+ 11 1241 1242 465
+ 885 382 886 899
+ 887 121 1247 1248
+ 669 476 26 486
+ 498 487 152 1253
+ 1254 916 905 153
+ 691 917 692 19
+ 1259 1260 482 909
+ 388 910 923 911
+ 146 1265 1266 686
+ 493 34 504 516
+ 505 176 1271 1272
+ 940 929 177 708
+ 941 709 27 1277
+ 1278 499 933 394
+ 934 947 935 167
+ 1283 1284 703 511
+ 42 521 532 522
+ 198 1289 1290 964
+ 953 199 725 965
+ 726 35 1295 1296
+ 517 957 400 958
+ 971 959 191 1301
+ 1302 721 527 50
+ 538 550 539 220
+ 1307 1308 988 977
+ 221 742 989 743
+ 43 1313 1314 533
+ 981 406 982 995
+ 983 213 1319 1320
+ 737 545 58 555
+ 566 556 244 1325
+ 1326 1012 1001 245
+ 759 1013 760 51
+ 1331 1332 551 1005
+ 412 1006 1019 1007
+ 237 1337 1338 755
+ 561 66 572 583
+ 573 266 1343 1344
+ 1036 1025 267 777
+ 1037 778 59 1349
+ 1350 567 1029 418
+ 1030 1043 1031 258
+ 1355 1356 772 578
+ 74 589 600 590
+ 288 1361 1362 1060
+ 1049 289 794 1061
+ 795 67 1367 1368
+ 584 1053 424 1054
+ 1067 1055 282 1373
+ 1374 790 595 82
+ 606 618 607 312
+ 1379 1380 1084 1073
+ 313 811 1085 812
+ 75 1385 1386 601
+ 1077 430 1078 1092
+ 1079 302 1390 1391
+ 807 613 3 639
+ 663 640 361 1396
+ 1397 1109 1098 362
+ 845 1110 846 90
+ 1402 1403 634 1102
+ 436 1103 1116 1104
+ 353 1408 1409 840
+ 646 91 623 635
+ 624 334 1414 1415
+ 1133 1122 335 828
+ 1134 829 83 1420
+ 1421 619 1126 442
+ 1127 1140 1128 327
+ 1426 1427 824 629
+ }
+
+ }
+ VertexData {
+ Array TRUE ArrayID 1 Vec3fArray 1428 {
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 0.0499999
+ 0 0.0499999 0.0499999
+ 0 0.0499999 0.0499999
+ 0 0.0499999 0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 0.0499999
+ 0.0499999 0 0.0499999
+ 0.0499999 0 0.0499999
+ 0.0499999 0 0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 0.0499999
+ 0 -0.0499999 0.0499999
+ 0 -0.0499999 0.0499999
+ 0 -0.0499999 0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 0.0499999
+ -0.0499999 0 0.0499999
+ -0.0499999 0 0.0499999
+ -0.0499999 0 0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 0.0499999
+ -0.025 0.0433012 0.0499999
+ -0.025 0.0433012 0.0499999
+ -0.025 0.0433012 0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ }
+ Indices FALSE
+ Binding BIND_PER_VERTEX
+ Normalize 0
+ }
+ NormalData {
+ Array TRUE ArrayID 2 Vec3fArray 1428 {
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ -0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ -0.707107 -0.707107 0
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ -0.965926 -0.25882 0
+ 0 0 -1
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ 0 0 1
+ -0.965926 0.258819 0
+ 0 0 -1
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ 0 0 1
+ -0.707107 0.707106 0
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ 0 0 1
+ -0.258819 0.965926 0
+ 0 0 -1
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ 0 0 1
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ 0 0 -1
+ -0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ 0 0 1
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ 0 0 -1
+ -0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ 0 0 1
+ 0 0 1
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 -1
+ 0 0 -1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 1
+ 0 0 1
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0 0 1
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ -0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 -1
+ 0 0 -1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 1
+ 0 0 1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ 0 0 1
+ 0 0 1
+ -0.25882 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ }
+ Indices FALSE
+ Binding BIND_PER_VERTEX
+ Normalize 0
+ }
+ InfluenceMap 4 {
+ VertexInfluence "Bone" 1146 {
+ 0 0.34816
+ 1 0.34816
+ 2 0.34816
+ 3 0.34816
+ 8 0.367419
+ 9 0.367419
+ 10 0.367419
+ 11 0.367419
+ 16 0.364055
+ 17 0.364055
+ 18 0.364055
+ 19 0.364055
+ 24 0.379784
+ 25 0.379784
+ 26 0.379784
+ 27 0.379784
+ 32 0.342064
+ 33 0.342064
+ 34 0.342064
+ 35 0.342064
+ 40 0.355754
+ 41 0.355754
+ 42 0.355754
+ 43 0.355754
+ 48 0.340748
+ 49 0.340748
+ 50 0.340748
+ 51 0.340748
+ 56 0.337092
+ 57 0.337092
+ 58 0.337092
+ 59 0.337092
+ 64 0.305945
+ 65 0.305945
+ 66 0.305945
+ 67 0.305945
+ 72 0.344481
+ 73 0.344481
+ 74 0.344481
+ 75 0.344481
+ 80 0.372747
+ 81 0.372747
+ 82 0.372747
+ 83 0.372747
+ 88 0.386408
+ 89 0.386408
+ 90 0.386408
+ 91 0.386408
+ 96 0.358124
+ 97 0.358124
+ 98 0.358124
+ 99 0.358124
+ 100 0.358124
+ 101 0.358124
+ 102 0.358124
+ 110 0.096263
+ 111 0.096263
+ 112 0.096263
+ 113 0.096263
+ 114 0.096263
+ 115 0.096263
+ 116 0.0964708
+ 117 0.0964708
+ 118 0.0964708
+ 119 0.0964708
+ 120 0.0964708
+ 121 0.0964708
+ 122 0.378926
+ 123 0.378926
+ 124 0.378926
+ 125 0.378926
+ 126 0.378926
+ 127 0.378926
+ 128 0.378926
+ 129 0.378926
+ 130 0.378926
+ 131 0.378926
+ 141 0.0968001
+ 142 0.0968001
+ 143 0.0968001
+ 144 0.0968001
+ 145 0.0968001
+ 146 0.0968001
+ 147 0.38288
+ 148 0.38288
+ 149 0.38288
+ 150 0.38288
+ 151 0.38288
+ 152 0.38288
+ 153 0.38288
+ 162 0.0968163
+ 163 0.0968163
+ 164 0.0968163
+ 165 0.0968163
+ 166 0.0968163
+ 167 0.0968163
+ 168 0.380374
+ 169 0.380374
+ 170 0.380374
+ 171 0.380374
+ 172 0.380374
+ 173 0.380374
+ 174 0.380374
+ 175 0.380374
+ 176 0.380374
+ 177 0.380374
+ 186 0.0958862
+ 187 0.0958862
+ 188 0.0958862
+ 189 0.0958862
+ 190 0.0958862
+ 191 0.0958862
+ 192 0.349886
+ 193 0.349886
+ 194 0.349886
+ 195 0.349886
+ 196 0.349886
+ 197 0.349886
+ 198 0.349886
+ 199 0.349886
+ 208 0.0954361
+ 209 0.0954361
+ 210 0.0954361
+ 211 0.0954361
+ 212 0.0954361
+ 213 0.0954361
+ 214 0.363129
+ 215 0.363129
+ 216 0.363129
+ 217 0.363129
+ 218 0.363129
+ 219 0.363129
+ 220 0.363129
+ 221 0.363129
+ 232 0.0949529
+ 233 0.0949529
+ 234 0.0949529
+ 235 0.0949529
+ 236 0.0949529
+ 237 0.0949529
+ 238 0.347336
+ 239 0.347336
+ 240 0.347336
+ 241 0.347336
+ 242 0.347336
+ 243 0.347336
+ 244 0.347336
+ 245 0.347336
+ 253 0.0943446
+ 254 0.0943446
+ 255 0.0943446
+ 256 0.0943446
+ 257 0.0943446
+ 258 0.0943446
+ 259 0.339065
+ 260 0.339065
+ 261 0.339065
+ 262 0.339065
+ 263 0.339065
+ 264 0.339065
+ 265 0.339065
+ 266 0.339065
+ 267 0.339065
+ 277 0.0938559
+ 278 0.0938559
+ 279 0.0938559
+ 280 0.0938559
+ 281 0.0938559
+ 282 0.0938559
+ 283 0.324145
+ 284 0.324145
+ 285 0.324145
+ 286 0.324145
+ 287 0.324145
+ 288 0.324145
+ 289 0.324145
+ 297 0.09501
+ 298 0.09501
+ 299 0.09501
+ 300 0.09501
+ 301 0.09501
+ 302 0.09501
+ 303 0.378378
+ 304 0.378378
+ 305 0.378378
+ 306 0.378378
+ 307 0.378378
+ 308 0.378378
+ 309 0.378378
+ 310 0.378378
+ 311 0.378378
+ 312 0.378378
+ 313 0.378378
+ 322 0.0965146
+ 323 0.0965146
+ 324 0.0965146
+ 325 0.0965146
+ 326 0.0965146
+ 327 0.0965146
+ 328 0.388315
+ 329 0.388315
+ 330 0.388315
+ 331 0.388315
+ 332 0.388315
+ 333 0.388315
+ 334 0.388315
+ 335 0.388315
+ 348 0.0969059
+ 349 0.0969059
+ 350 0.0969059
+ 351 0.0969059
+ 352 0.0969059
+ 353 0.0969059
+ 354 0.365767
+ 355 0.365767
+ 356 0.365767
+ 357 0.365767
+ 358 0.365767
+ 359 0.365767
+ 360 0.365767
+ 361 0.365767
+ 362 0.365767
+ 371 0.0958394
+ 372 0.0958394
+ 373 0.0958394
+ 374 0.0958394
+ 375 0.0958394
+ 376 0.0958394
+ 377 0.0962926
+ 378 0.0962926
+ 379 0.0962926
+ 380 0.0962926
+ 381 0.0962926
+ 382 0.0962926
+ 383 0.0965003
+ 384 0.0965003
+ 385 0.0965003
+ 386 0.0965003
+ 387 0.0965003
+ 388 0.0965003
+ 389 0.0960521
+ 390 0.0960521
+ 391 0.0960521
+ 392 0.0960521
+ 393 0.0960521
+ 394 0.0960521
+ 395 0.0951623
+ 396 0.0951623
+ 397 0.0951623
+ 398 0.0951623
+ 399 0.0951623
+ 400 0.0951623
+ 401 0.0948754
+ 402 0.0948754
+ 403 0.0948754
+ 404 0.0948754
+ 405 0.0948754
+ 406 0.0948754
+ 407 0.0942636
+ 408 0.0942636
+ 409 0.0942636
+ 410 0.0942636
+ 411 0.0942636
+ 412 0.0942636
+ 413 0.0936536
+ 414 0.0936536
+ 415 0.0936536
+ 416 0.0936536
+ 417 0.0936536
+ 418 0.0936536
+ 419 0.0938172
+ 420 0.0938172
+ 421 0.0938172
+ 422 0.0938172
+ 423 0.0938172
+ 424 0.0938172
+ 425 0.0955049
+ 426 0.0955049
+ 427 0.0955049
+ 428 0.0955049
+ 429 0.0955049
+ 430 0.0955049
+ 431 0.096215
+ 432 0.096215
+ 433 0.096215
+ 434 0.096215
+ 435 0.096215
+ 436 0.096215
+ 437 0.0964742
+ 438 0.0964742
+ 439 0.0964742
+ 440 0.0964742
+ 441 0.0964742
+ 442 0.0964742
+ 443 0.354635
+ 444 0.354635
+ 445 0.354635
+ 446 0.354635
+ 447 0.354635
+ 454 0.00430191
+ 455 0.00430191
+ 456 0.00430191
+ 457 0.00430191
+ 458 0.00430191
+ 459 0.00430191
+ 460 0.374686
+ 461 0.374686
+ 462 0.374686
+ 463 0.374686
+ 464 0.374686
+ 465 0.374686
+ 466 0.357486
+ 467 0.357486
+ 468 0.357486
+ 469 0.357486
+ 470 0.357486
+ 477 0.375587
+ 478 0.375587
+ 479 0.375587
+ 480 0.375587
+ 481 0.375587
+ 482 0.375587
+ 483 0.373781
+ 484 0.373781
+ 485 0.373781
+ 486 0.373781
+ 487 0.373781
+ 494 0.376868
+ 495 0.376868
+ 496 0.376868
+ 497 0.376868
+ 498 0.376868
+ 499 0.376868
+ 500 0.354076
+ 501 0.354076
+ 502 0.354076
+ 503 0.354076
+ 504 0.354076
+ 505 0.354076
+ 512 0.371253
+ 513 0.371253
+ 514 0.371253
+ 515 0.371253
+ 516 0.371253
+ 517 0.371253
+ 518 0.344402
+ 519 0.344402
+ 520 0.344402
+ 521 0.344402
+ 522 0.344402
+ 528 0.371254
+ 529 0.371254
+ 530 0.371254
+ 531 0.371254
+ 532 0.371254
+ 533 0.371254
+ 534 0.337368
+ 535 0.337368
+ 536 0.337368
+ 537 0.337368
+ 538 0.337368
+ 539 0.337368
+ 546 0.368766
+ 547 0.368766
+ 548 0.368766
+ 549 0.368766
+ 550 0.368766
+ 551 0.368766
+ 552 0.32735
+ 553 0.32735
+ 554 0.32735
+ 555 0.32735
+ 556 0.32735
+ 562 0.366421
+ 563 0.366421
+ 564 0.366421
+ 565 0.366421
+ 566 0.366421
+ 567 0.366421
+ 568 0.307046
+ 569 0.307046
+ 570 0.307046
+ 571 0.307046
+ 572 0.307046
+ 573 0.307046
+ 579 0.362455
+ 580 0.362455
+ 581 0.362455
+ 582 0.362455
+ 583 0.362455
+ 584 0.362455
+ 585 0.319194
+ 586 0.319194
+ 587 0.319194
+ 588 0.319194
+ 589 0.319194
+ 590 0.319194
+ 596 0.368885
+ 597 0.368885
+ 598 0.368885
+ 599 0.368885
+ 600 0.368885
+ 601 0.368885
+ 602 0.368849
+ 603 0.368849
+ 604 0.368849
+ 605 0.368849
+ 606 0.368849
+ 607 0.368849
+ 614 0.375813
+ 615 0.375813
+ 616 0.375813
+ 617 0.375813
+ 618 0.375813
+ 619 0.375813
+ 620 0.38096
+ 621 0.38096
+ 622 0.38096
+ 623 0.38096
+ 624 0.38096
+ 630 0.377718
+ 631 0.377718
+ 632 0.377718
+ 633 0.377718
+ 634 0.377718
+ 635 0.377718
+ 636 0.359173
+ 637 0.359173
+ 638 0.359173
+ 639 0.359173
+ 640 0.359173
+ 647 0.313242
+ 648 0.313242
+ 649 0.313242
+ 650 0.313242
+ 651 0.313242
+ 652 0.313242
+ 658 0.372815
+ 659 0.372815
+ 660 0.372815
+ 661 0.372815
+ 662 0.372815
+ 663 0.372815
+ 664 0.00514126
+ 665 0.00514126
+ 666 0.00514126
+ 667 0.00514126
+ 668 0.00514126
+ 669 0.00514126
+ 670 0.357776
+ 671 0.357776
+ 672 0.357776
+ 673 0.357776
+ 674 0.357776
+ 675 0.357776
+ 681 0.00567305
+ 682 0.00567305
+ 683 0.00567305
+ 684 0.00567305
+ 685 0.00567305
+ 686 0.00567305
+ 687 0.347794
+ 688 0.347794
+ 689 0.347794
+ 690 0.347794
+ 691 0.347794
+ 692 0.347794
+ 698 0.00545162
+ 699 0.00545162
+ 700 0.00545162
+ 701 0.00545162
+ 702 0.00545162
+ 703 0.00545162
+ 704 0.364472
+ 705 0.364472
+ 706 0.364472
+ 707 0.364472
+ 708 0.364472
+ 709 0.364472
+ 716 0.00471312
+ 717 0.00471312
+ 718 0.00471312
+ 719 0.00471312
+ 720 0.00471312
+ 721 0.00471312
+ 722 0.305746
+ 723 0.305746
+ 724 0.305746
+ 725 0.305746
+ 726 0.305746
+ 732 0.00370216
+ 733 0.00370216
+ 734 0.00370216
+ 735 0.00370216
+ 736 0.00370216
+ 737 0.00370216
+ 738 0.343717
+ 739 0.343717
+ 740 0.343717
+ 741 0.343717
+ 742 0.343717
+ 743 0.343717
+ 750 0.00352383
+ 751 0.00352383
+ 752 0.00352383
+ 753 0.00352383
+ 754 0.00352383
+ 755 0.00352383
+ 756 0.319341
+ 757 0.319341
+ 758 0.319341
+ 759 0.319341
+ 760 0.319341
+ 767 0.00311261
+ 768 0.00311261
+ 769 0.00311261
+ 770 0.00311261
+ 771 0.00311261
+ 772 0.00311261
+ 773 0.321807
+ 774 0.321807
+ 775 0.321807
+ 776 0.321807
+ 777 0.321807
+ 778 0.321807
+ 785 0.00276154
+ 786 0.00276154
+ 787 0.00276154
+ 788 0.00276154
+ 789 0.00276154
+ 790 0.00276154
+ 791 0.277267
+ 792 0.277267
+ 793 0.277267
+ 794 0.277267
+ 795 0.277267
+ 802 0.00300527
+ 803 0.00300527
+ 804 0.00300527
+ 805 0.00300527
+ 806 0.00300527
+ 807 0.00300527
+ 808 0.345333
+ 809 0.345333
+ 810 0.345333
+ 811 0.345333
+ 812 0.345333
+ 819 0.00410753
+ 820 0.00410753
+ 821 0.00410753
+ 822 0.00410753
+ 823 0.00410753
+ 824 0.00410753
+ 825 0.354611
+ 826 0.354611
+ 827 0.354611
+ 828 0.354611
+ 829 0.354611
+ 835 0.00450843
+ 836 0.00450843
+ 837 0.00450843
+ 838 0.00450843
+ 839 0.00450843
+ 840 0.00450843
+ 841 0.371096
+ 842 0.371096
+ 843 0.371096
+ 844 0.371096
+ 845 0.371096
+ 846 0.371096
+ 852 0.0961208
+ 853 0.0961208
+ 854 0.0961208
+ 855 0.0961208
+ 856 0.0961208
+ 857 0.0961208
+ 858 0.0959629
+ 859 0.0959629
+ 860 0.0959629
+ 861 0.0959629
+ 862 0.0959629
+ 863 0.0959629
+ 864 0.373473
+ 865 0.373473
+ 866 0.373473
+ 867 0.373473
+ 868 0.373473
+ 869 0.373473
+ 870 0.00435418
+ 871 0.00435418
+ 872 0.00435418
+ 873 0.00435418
+ 874 0.00435418
+ 875 0.00435418
+ 876 0.0965277
+ 877 0.0965277
+ 878 0.0965277
+ 879 0.0965277
+ 880 0.0965277
+ 881 0.0965277
+ 882 0.0963668
+ 883 0.0963668
+ 884 0.0963668
+ 885 0.0963668
+ 886 0.0963668
+ 887 0.0963668
+ 888 0.376317
+ 889 0.376317
+ 890 0.376317
+ 891 0.376317
+ 892 0.376317
+ 893 0.376317
+ 894 0.00527549
+ 895 0.00527549
+ 896 0.00527549
+ 897 0.00527549
+ 898 0.00527549
+ 899 0.00527549
+ 900 0.0966689
+ 901 0.0966689
+ 902 0.0966689
+ 903 0.0966689
+ 904 0.0966689
+ 905 0.0966689
+ 906 0.0966253
+ 907 0.0966253
+ 908 0.0966253
+ 909 0.0966253
+ 910 0.0966253
+ 911 0.0966253
+ 912 0.377324
+ 913 0.377324
+ 914 0.377324
+ 915 0.377324
+ 916 0.377324
+ 917 0.377324
+ 918 0.00535691
+ 919 0.00535691
+ 920 0.00535691
+ 921 0.00535691
+ 922 0.00535691
+ 923 0.00535691
+ 924 0.0959472
+ 925 0.0959472
+ 926 0.0959472
+ 927 0.0959472
+ 928 0.0959472
+ 929 0.0959472
+ 930 0.0964418
+ 931 0.0964418
+ 932 0.0964418
+ 933 0.0964418
+ 934 0.0964418
+ 935 0.0964418
+ 936 0.375737
+ 937 0.375737
+ 938 0.375737
+ 939 0.375737
+ 940 0.375737
+ 941 0.375737
+ 942 0.00496679
+ 943 0.00496679
+ 944 0.00496679
+ 945 0.00496679
+ 946 0.00496679
+ 947 0.00496679
+ 948 0.0952691
+ 949 0.0952691
+ 950 0.0952691
+ 951 0.0952691
+ 952 0.0952691
+ 953 0.0952691
+ 954 0.0954447
+ 955 0.0954447
+ 956 0.0954447
+ 957 0.0954447
+ 958 0.0954447
+ 959 0.0954447
+ 960 0.370982
+ 961 0.370982
+ 962 0.370982
+ 963 0.370982
+ 964 0.370982
+ 965 0.370982
+ 966 0.00385237
+ 967 0.00385237
+ 968 0.00385237
+ 969 0.00385237
+ 970 0.00385237
+ 971 0.00385237
+ 972 0.0948983
+ 973 0.0948983
+ 974 0.0948983
+ 975 0.0948983
+ 976 0.0948983
+ 977 0.0948983
+ 978 0.095146
+ 979 0.095146
+ 980 0.095146
+ 981 0.095146
+ 982 0.095146
+ 983 0.095146
+ 984 0.371512
+ 985 0.371512
+ 986 0.371512
+ 987 0.371512
+ 988 0.371512
+ 989 0.371512
+ 990 0.00338936
+ 991 0.00338936
+ 992 0.00338936
+ 993 0.00338936
+ 994 0.00338936
+ 995 0.00338936
+ 996 0.0942812
+ 997 0.0942812
+ 998 0.0942812
+ 999 0.0942812
+ 1000 0.0942812
+ 1001 0.0942812
+ 1002 0.0945738
+ 1003 0.0945738
+ 1004 0.0945738
+ 1005 0.0945738
+ 1006 0.0945738
+ 1007 0.0945738
+ 1008 0.368375
+ 1009 0.368375
+ 1010 0.368375
+ 1011 0.368375
+ 1012 0.368375
+ 1013 0.368375
+ 1014 0.00306702
+ 1015 0.00306702
+ 1016 0.00306702
+ 1017 0.00306702
+ 1018 0.00306702
+ 1019 0.00306702
+ 1020 0.0936896
+ 1021 0.0936896
+ 1022 0.0936896
+ 1023 0.0936896
+ 1024 0.0936896
+ 1025 0.0936896
+ 1026 0.0939704
+ 1027 0.0939704
+ 1028 0.0939704
+ 1029 0.0939704
+ 1030 0.0939704
+ 1031 0.0939704
+ 1032 0.365541
+ 1033 0.365541
+ 1034 0.365541
+ 1035 0.365541
+ 1036 0.365541
+ 1037 0.365541
+ 1038 0.00270522
+ 1039 0.00270522
+ 1040 0.00270522
+ 1041 0.00270522
+ 1042 0.00270522
+ 1043 0.00270522
+ 1044 0.0943534
+ 1045 0.0943534
+ 1046 0.0943534
+ 1047 0.0943534
+ 1048 0.0943534
+ 1049 0.0943534
+ 1050 0.0937264
+ 1051 0.0937264
+ 1052 0.0937264
+ 1053 0.0937264
+ 1054 0.0937264
+ 1055 0.0937264
+ 1056 0.364966
+ 1057 0.364966
+ 1058 0.364966
+ 1059 0.364966
+ 1060 0.364966
+ 1061 0.364966
+ 1062 0.00244218
+ 1063 0.00244218
+ 1064 0.00244218
+ 1065 0.00244218
+ 1066 0.00244218
+ 1067 0.00244218
+ 1068 0.0960259
+ 1069 0.0960259
+ 1070 0.0960259
+ 1071 0.0960259
+ 1072 0.0960259
+ 1073 0.0960259
+ 1074 0.0952469
+ 1075 0.0952469
+ 1076 0.0952469
+ 1077 0.0952469
+ 1078 0.0952469
+ 1079 0.0952469
+ 1080 0.374353
+ 1081 0.374353
+ 1082 0.374353
+ 1083 0.374353
+ 1084 0.374353
+ 1085 0.374353
+ 1086 0.00335479
+ 1087 0.00335479
+ 1088 0.00335479
+ 1089 0.00335479
+ 1090 0.00335479
+ 1091 0.00335479
+ 1092 0.00335479
+ 1093 0.0962022
+ 1094 0.0962022
+ 1095 0.0962022
+ 1096 0.0962022
+ 1097 0.0962022
+ 1098 0.0962022
+ 1099 0.096563
+ 1100 0.096563
+ 1101 0.096563
+ 1102 0.096563
+ 1103 0.096563
+ 1104 0.096563
+ 1105 0.37611
+ 1106 0.37611
+ 1107 0.37611
+ 1108 0.37611
+ 1109 0.37611
+ 1110 0.37611
+ 1111 0.0041514
+ 1112 0.0041514
+ 1113 0.0041514
+ 1114 0.0041514
+ 1115 0.0041514
+ 1116 0.0041514
+ 1117 0.0967142
+ 1118 0.0967142
+ 1119 0.0967142
+ 1120 0.0967142
+ 1121 0.0967142
+ 1122 0.0967142
+ 1123 0.0964939
+ 1124 0.0964939
+ 1125 0.0964939
+ 1126 0.0964939
+ 1127 0.0964939
+ 1128 0.0964939
+ 1129 0.378176
+ 1130 0.378176
+ 1131 0.378176
+ 1132 0.378176
+ 1133 0.378176
+ 1134 0.378176
+ 1135 0.0041101
+ 1136 0.0041101
+ 1137 0.0041101
+ 1138 0.0041101
+ 1139 0.0041101
+ 1140 0.0041101
+ 1141 0.00472903
+ 1142 0.00472903
+ 1143 0.00472903
+ 1144 0.00472903
+ 1145 0.00472903
+ 1146 0.00472903
+ 1147 0.00546944
+ 1148 0.00546944
+ 1149 0.00546944
+ 1150 0.00546944
+ 1151 0.00546944
+ 1152 0.00546944
+ 1153 0.00535601
+ 1154 0.00535601
+ 1155 0.00535601
+ 1156 0.00535601
+ 1157 0.00535601
+ 1158 0.00535601
+ 1159 0.00483459
+ 1160 0.00483459
+ 1161 0.00483459
+ 1162 0.00483459
+ 1163 0.00483459
+ 1164 0.00483459
+ 1165 0.00366831
+ 1166 0.00366831
+ 1167 0.00366831
+ 1168 0.00366831
+ 1169 0.00366831
+ 1170 0.00366831
+ 1171 0.00344825
+ 1172 0.00344825
+ 1173 0.00344825
+ 1174 0.00344825
+ 1175 0.00344825
+ 1176 0.00344825
+ 1177 0.00304955
+ 1178 0.00304955
+ 1179 0.00304955
+ 1180 0.00304955
+ 1181 0.00304955
+ 1182 0.00304955
+ 1183 0.00270885
+ 1184 0.00270885
+ 1185 0.00270885
+ 1186 0.00270885
+ 1187 0.00270885
+ 1188 0.00270885
+ 1189 0.00261372
+ 1190 0.00261372
+ 1191 0.00261372
+ 1192 0.00261372
+ 1193 0.00261372
+ 1194 0.00261372
+ 1195 0.00375396
+ 1196 0.00375396
+ 1197 0.00375396
+ 1198 0.00375396
+ 1199 0.00375396
+ 1200 0.00375396
+ 1201 0.00419354
+ 1202 0.00419354
+ 1203 0.00419354
+ 1204 0.00419354
+ 1205 0.00419354
+ 1206 0.00419354
+ 1207 0.00432968
+ 1208 0.00432968
+ 1209 0.00432968
+ 1210 0.00432968
+ 1211 0.00432968
+ 1212 0.00432968
+ 1213 0.374055
+ 1214 0.374055
+ 1215 0.374055
+ 1216 0.374055
+ 1217 0.374055
+ 1218 0.374055
+ 1219 0.372065
+ 1220 0.372065
+ 1221 0.372065
+ 1222 0.372065
+ 1223 0.372065
+ 1224 0.372065
+ 1225 0.00420994
+ 1226 0.00420994
+ 1227 0.00420994
+ 1228 0.00420994
+ 1229 0.00420994
+ 1230 0.00420994
+ 1231 0.375788
+ 1232 0.375788
+ 1233 0.375788
+ 1234 0.375788
+ 1235 0.375788
+ 1236 0.375788
+ 1237 0.375401
+ 1238 0.375401
+ 1239 0.375401
+ 1240 0.375401
+ 1241 0.375401
+ 1242 0.375401
+ 1243 0.0052079
+ 1244 0.0052079
+ 1245 0.0052079
+ 1246 0.0052079
+ 1247 0.0052079
+ 1248 0.0052079
+ 1249 0.377278
+ 1250 0.377278
+ 1251 0.377278
+ 1252 0.377278
+ 1253 0.377278
+ 1254 0.377278
+ 1255 0.375941
+ 1256 0.375941
+ 1257 0.375941
+ 1258 0.375941
+ 1259 0.375941
+ 1260 0.375941
+ 1261 0.00551689
+ 1262 0.00551689
+ 1263 0.00551689
+ 1264 0.00551689
+ 1265 0.00551689
+ 1266 0.00551689
+ 1267 0.373505
+ 1268 0.373505
+ 1269 0.373505
+ 1270 0.373505
+ 1271 0.373505
+ 1272 0.373505
+ 1273 0.376296
+ 1274 0.376296
+ 1275 0.376296
+ 1276 0.376296
+ 1277 0.376296
+ 1278 0.376296
+ 1279 0.00521576
+ 1280 0.00521576
+ 1281 0.00521576
+ 1282 0.00521576
+ 1283 0.00521576
+ 1284 0.00521576
+ 1285 0.371128
+ 1286 0.371128
+ 1287 0.371128
+ 1288 0.371128
+ 1289 0.371128
+ 1290 0.371128
+ 1291 0.369947
+ 1292 0.369947
+ 1293 0.369947
+ 1294 0.369947
+ 1295 0.369947
+ 1296 0.369947
+ 1297 0.00425982
+ 1298 0.00425982
+ 1299 0.00425982
+ 1300 0.00425982
+ 1301 0.00425982
+ 1302 0.00425982
+ 1303 0.370019
+ 1304 0.370019
+ 1305 0.370019
+ 1306 0.370019
+ 1307 0.370019
+ 1308 0.370019
+ 1309 0.37133
+ 1310 0.37133
+ 1311 0.37133
+ 1312 0.37133
+ 1313 0.37133
+ 1314 0.37133
+ 1315 0.003519
+ 1316 0.003519
+ 1317 0.003519
+ 1318 0.003519
+ 1319 0.003519
+ 1320 0.003519
+ 1321 0.367272
+ 1322 0.367272
+ 1323 0.367272
+ 1324 0.367272
+ 1325 0.367272
+ 1326 0.367272
+ 1327 0.368091
+ 1328 0.368091
+ 1329 0.368091
+ 1330 0.368091
+ 1331 0.368091
+ 1332 0.368091
+ 1333 0.00328618
+ 1334 0.00328618
+ 1335 0.00328618
+ 1336 0.00328618
+ 1337 0.00328618
+ 1338 0.00328618
+ 1339 0.36367
+ 1340 0.36367
+ 1341 0.36367
+ 1342 0.36367
+ 1343 0.36367
+ 1344 0.36367
+ 1345 0.365851
+ 1346 0.365851
+ 1347 0.365851
+ 1348 0.365851
+ 1349 0.365851
+ 1350 0.365851
+ 1351 0.00289476
+ 1352 0.00289476
+ 1353 0.00289476
+ 1354 0.00289476
+ 1355 0.00289476
+ 1356 0.00289476
+ 1357 0.366624
+ 1358 0.366624
+ 1359 0.366624
+ 1360 0.366624
+ 1361 0.366624
+ 1362 0.366624
+ 1363 0.362548
+ 1364 0.362548
+ 1365 0.362548
+ 1366 0.362548
+ 1367 0.362548
+ 1368 0.362548
+ 1369 0.00255603
+ 1370 0.00255603
+ 1371 0.00255603
+ 1372 0.00255603
+ 1373 0.00255603
+ 1374 0.00255603
+ 1375 0.375304
+ 1376 0.375304
+ 1377 0.375304
+ 1378 0.375304
+ 1379 0.375304
+ 1380 0.375304
+ 1381 0.37154
+ 1382 0.37154
+ 1383 0.37154
+ 1384 0.37154
+ 1385 0.37154
+ 1386 0.37154
+ 1387 0.00318003
+ 1388 0.00318003
+ 1389 0.00318003
+ 1390 0.00318003
+ 1391 0.00318003
+ 1392 0.374529
+ 1393 0.374529
+ 1394 0.374529
+ 1395 0.374529
+ 1396 0.374529
+ 1397 0.374529
+ 1398 0.377021
+ 1399 0.377021
+ 1400 0.377021
+ 1401 0.377021
+ 1402 0.377021
+ 1403 0.377021
+ 1404 0.00434786
+ 1405 0.00434786
+ 1406 0.00434786
+ 1407 0.00434786
+ 1408 0.00434786
+ 1409 0.00434786
+ 1410 0.378212
+ 1411 0.378212
+ 1412 0.378212
+ 1413 0.378212
+ 1414 0.378212
+ 1415 0.378212
+ 1416 0.376587
+ 1417 0.376587
+ 1418 0.376587
+ 1419 0.376587
+ 1420 0.376587
+ 1421 0.376587
+ 1422 0.00412774
+ 1423 0.00412774
+ 1424 0.00412774
+ 1425 0.00412774
+ 1426 0.00412774
+ 1427 0.00412774
+ }
+ VertexInfluence "Bone_001" 1428 {
+ 0 0.0536852
+ 1 0.0536852
+ 2 0.0536852
+ 3 0.0536852
+ 4 0.663579
+ 5 0.663579
+ 6 0.663579
+ 7 0.663579
+ 8 0.0582281
+ 9 0.0582281
+ 10 0.0582281
+ 11 0.0582281
+ 12 0.68577
+ 13 0.68577
+ 14 0.68577
+ 15 0.68577
+ 16 0.059167
+ 17 0.059167
+ 18 0.059167
+ 19 0.059167
+ 20 0.692283
+ 21 0.692283
+ 22 0.692283
+ 23 0.692283
+ 24 0.0595554
+ 25 0.0595554
+ 26 0.0595554
+ 27 0.0595554
+ 28 0.705973
+ 29 0.705973
+ 30 0.705973
+ 31 0.705973
+ 32 0.0534075
+ 33 0.0534075
+ 34 0.0534075
+ 35 0.0534075
+ 36 0.698447
+ 37 0.698447
+ 38 0.698447
+ 39 0.698447
+ 40 0.0581471
+ 41 0.0581471
+ 42 0.0581471
+ 43 0.0581471
+ 44 0.683882
+ 45 0.683882
+ 46 0.683882
+ 47 0.683882
+ 48 0.0594622
+ 49 0.0594622
+ 50 0.0594622
+ 51 0.0594622
+ 52 0.628315
+ 53 0.628315
+ 54 0.628315
+ 55 0.628315
+ 56 0.0600619
+ 57 0.0600619
+ 58 0.0600619
+ 59 0.0600619
+ 60 0.622147
+ 61 0.622147
+ 62 0.622147
+ 63 0.622147
+ 64 0.0537814
+ 65 0.0537814
+ 66 0.0537814
+ 67 0.0537814
+ 68 0.621214
+ 69 0.621214
+ 70 0.621214
+ 71 0.621214
+ 72 0.0575935
+ 73 0.0575935
+ 74 0.0575935
+ 75 0.0575935
+ 76 0.634884
+ 77 0.634884
+ 78 0.634884
+ 79 0.634884
+ 80 0.058103
+ 81 0.058103
+ 82 0.058103
+ 83 0.058103
+ 84 0.582854
+ 85 0.582854
+ 86 0.582854
+ 87 0.582854
+ 88 0.0591488
+ 89 0.0591488
+ 90 0.0591488
+ 91 0.0591488
+ 92 0.616904
+ 93 0.616904
+ 94 0.616904
+ 95 0.616904
+ 96 0.0547738
+ 97 0.0547738
+ 98 0.0547738
+ 99 0.0547738
+ 100 0.0547738
+ 101 0.0547738
+ 102 0.0547738
+ 103 0.690176
+ 104 0.690176
+ 105 0.690176
+ 106 0.690176
+ 107 0.690176
+ 108 0.690176
+ 109 0.690176
+ 110 0.408744
+ 111 0.408744
+ 112 0.408744
+ 113 0.408744
+ 114 0.408744
+ 115 0.408744
+ 116 0.410372
+ 117 0.410372
+ 118 0.410372
+ 119 0.410372
+ 120 0.410372
+ 121 0.410372
+ 122 0.0606667
+ 123 0.0606667
+ 124 0.0606667
+ 125 0.0606667
+ 126 0.0606667
+ 127 0.0606667
+ 128 0.0606667
+ 129 0.0606667
+ 130 0.0606667
+ 131 0.0606667
+ 132 0.683242
+ 133 0.683242
+ 134 0.683242
+ 135 0.683242
+ 136 0.683242
+ 137 0.683242
+ 138 0.683242
+ 139 0.683242
+ 140 0.683242
+ 141 0.411126
+ 142 0.411126
+ 143 0.411126
+ 144 0.411126
+ 145 0.411126
+ 146 0.411126
+ 147 0.059725
+ 148 0.059725
+ 149 0.059725
+ 150 0.059725
+ 151 0.059725
+ 152 0.059725
+ 153 0.059725
+ 154 0.699151
+ 155 0.699151
+ 156 0.699151
+ 157 0.699151
+ 158 0.699151
+ 159 0.699151
+ 160 0.699151
+ 161 0.699151
+ 162 0.411491
+ 163 0.411491
+ 164 0.411491
+ 165 0.411491
+ 166 0.411491
+ 167 0.411491
+ 168 0.0585465
+ 169 0.0585465
+ 170 0.0585465
+ 171 0.0585465
+ 172 0.0585465
+ 173 0.0585465
+ 174 0.0585465
+ 175 0.0585465
+ 176 0.0585465
+ 177 0.0585465
+ 178 0.692537
+ 179 0.692537
+ 180 0.692537
+ 181 0.692537
+ 182 0.692537
+ 183 0.692537
+ 184 0.692537
+ 185 0.692537
+ 186 0.411145
+ 187 0.411145
+ 188 0.411145
+ 189 0.411145
+ 190 0.411145
+ 191 0.411145
+ 192 0.0545445
+ 193 0.0545445
+ 194 0.0545445
+ 195 0.0545445
+ 196 0.0545445
+ 197 0.0545445
+ 198 0.0545445
+ 199 0.0545445
+ 200 0.704511
+ 201 0.704511
+ 202 0.704511
+ 203 0.704511
+ 204 0.704511
+ 205 0.704511
+ 206 0.704511
+ 207 0.704511
+ 208 0.410016
+ 209 0.410016
+ 210 0.410016
+ 211 0.410016
+ 212 0.410016
+ 213 0.410016
+ 214 0.0607347
+ 215 0.0607347
+ 216 0.0607347
+ 217 0.0607347
+ 218 0.0607347
+ 219 0.0607347
+ 220 0.0607347
+ 221 0.0607347
+ 222 0.633219
+ 223 0.633219
+ 224 0.633219
+ 225 0.633219
+ 226 0.633219
+ 227 0.633219
+ 228 0.633219
+ 229 0.633219
+ 230 0.633219
+ 231 0.633219
+ 232 0.407998
+ 233 0.407998
+ 234 0.407998
+ 235 0.407998
+ 236 0.407998
+ 237 0.407998
+ 238 0.0602369
+ 239 0.0602369
+ 240 0.0602369
+ 241 0.0602369
+ 242 0.0602369
+ 243 0.0602369
+ 244 0.0602369
+ 245 0.0602369
+ 246 0.61358
+ 247 0.61358
+ 248 0.61358
+ 249 0.61358
+ 250 0.61358
+ 251 0.61358
+ 252 0.61358
+ 253 0.406953
+ 254 0.406953
+ 255 0.406953
+ 256 0.406953
+ 257 0.406953
+ 258 0.406953
+ 259 0.0589982
+ 260 0.0589982
+ 261 0.0589982
+ 262 0.0589982
+ 263 0.0589982
+ 264 0.0589982
+ 265 0.0589982
+ 266 0.0589982
+ 267 0.0589982
+ 268 0.602094
+ 269 0.602094
+ 270 0.602094
+ 271 0.602094
+ 272 0.602094
+ 273 0.602094
+ 274 0.602094
+ 275 0.602094
+ 276 0.602094
+ 277 0.406649
+ 278 0.406649
+ 279 0.406649
+ 280 0.406649
+ 281 0.406649
+ 282 0.406649
+ 283 0.0546197
+ 284 0.0546197
+ 285 0.0546197
+ 286 0.0546197
+ 287 0.0546197
+ 288 0.0546197
+ 289 0.0546197
+ 290 0.629987
+ 291 0.629987
+ 292 0.629987
+ 293 0.629987
+ 294 0.629987
+ 295 0.629987
+ 296 0.629987
+ 297 0.406314
+ 298 0.406314
+ 299 0.406314
+ 300 0.406314
+ 301 0.406314
+ 302 0.406314
+ 303 0.0596827
+ 304 0.0596827
+ 305 0.0596827
+ 306 0.0596827
+ 307 0.0596827
+ 308 0.0596827
+ 309 0.0596827
+ 310 0.0596827
+ 311 0.0596827
+ 312 0.0596827
+ 313 0.0596827
+ 314 0.617563
+ 315 0.617563
+ 316 0.617563
+ 317 0.617563
+ 318 0.617563
+ 319 0.617563
+ 320 0.617563
+ 321 0.617563
+ 322 0.405375
+ 323 0.405375
+ 324 0.405375
+ 325 0.405375
+ 326 0.405375
+ 327 0.405375
+ 328 0.0589791
+ 329 0.0589791
+ 330 0.0589791
+ 331 0.0589791
+ 332 0.0589791
+ 333 0.0589791
+ 334 0.0589791
+ 335 0.0589791
+ 336 0.618805
+ 337 0.618805
+ 338 0.618805
+ 339 0.618805
+ 340 0.618805
+ 341 0.618805
+ 342 0.618805
+ 343 0.618805
+ 344 0.618805
+ 345 0.618805
+ 346 0.618805
+ 347 0.618805
+ 348 0.406375
+ 349 0.406375
+ 350 0.406375
+ 351 0.406375
+ 352 0.406375
+ 353 0.406375
+ 354 0.0567919
+ 355 0.0567919
+ 356 0.0567919
+ 357 0.0567919
+ 358 0.0567919
+ 359 0.0567919
+ 360 0.0567919
+ 361 0.0567919
+ 362 0.0567919
+ 363 0.641822
+ 364 0.641822
+ 365 0.641822
+ 366 0.641822
+ 367 0.641822
+ 368 0.641822
+ 369 0.641822
+ 370 0.641822
+ 371 0.410126
+ 372 0.410126
+ 373 0.410126
+ 374 0.410126
+ 375 0.410126
+ 376 0.410126
+ 377 0.411144
+ 378 0.411144
+ 379 0.411144
+ 380 0.411144
+ 381 0.411144
+ 382 0.411144
+ 383 0.411758
+ 384 0.411758
+ 385 0.411758
+ 386 0.411758
+ 387 0.411758
+ 388 0.411758
+ 389 0.411717
+ 390 0.411717
+ 391 0.411717
+ 392 0.411717
+ 393 0.411717
+ 394 0.411717
+ 395 0.411143
+ 396 0.411143
+ 397 0.411143
+ 398 0.411143
+ 399 0.411143
+ 400 0.411143
+ 401 0.409299
+ 402 0.409299
+ 403 0.409299
+ 404 0.409299
+ 405 0.409299
+ 406 0.409299
+ 407 0.407735
+ 408 0.407735
+ 409 0.407735
+ 410 0.407735
+ 411 0.407735
+ 412 0.407735
+ 413 0.407069
+ 414 0.407069
+ 415 0.407069
+ 416 0.407069
+ 417 0.407069
+ 418 0.407069
+ 419 0.407022
+ 420 0.407022
+ 421 0.407022
+ 422 0.407022
+ 423 0.407022
+ 424 0.407022
+ 425 0.4061
+ 426 0.4061
+ 427 0.4061
+ 428 0.4061
+ 429 0.4061
+ 430 0.4061
+ 431 0.407861
+ 432 0.407861
+ 433 0.407861
+ 434 0.407861
+ 435 0.407861
+ 436 0.407861
+ 437 0.406035
+ 438 0.406035
+ 439 0.406035
+ 440 0.406035
+ 441 0.406035
+ 442 0.406035
+ 443 0.0558424
+ 444 0.0558424
+ 445 0.0558424
+ 446 0.0558424
+ 447 0.0558424
+ 448 0.709458
+ 449 0.709458
+ 450 0.709458
+ 451 0.709458
+ 452 0.709458
+ 453 0.709458
+ 454 0.527779
+ 455 0.527779
+ 456 0.527779
+ 457 0.527779
+ 458 0.527779
+ 459 0.527779
+ 460 0.117577
+ 461 0.117577
+ 462 0.117577
+ 463 0.117577
+ 464 0.117577
+ 465 0.117577
+ 466 0.0593598
+ 467 0.0593598
+ 468 0.0593598
+ 469 0.0593598
+ 470 0.0593598
+ 471 0.690194
+ 472 0.690194
+ 473 0.690194
+ 474 0.690194
+ 475 0.690194
+ 476 0.690194
+ 477 0.118115
+ 478 0.118115
+ 479 0.118115
+ 480 0.118115
+ 481 0.118115
+ 482 0.118115
+ 483 0.0590717
+ 484 0.0590717
+ 485 0.0590717
+ 486 0.0590717
+ 487 0.0590717
+ 488 0.699241
+ 489 0.699241
+ 490 0.699241
+ 491 0.699241
+ 492 0.699241
+ 493 0.699241
+ 494 0.118085
+ 495 0.118085
+ 496 0.118085
+ 497 0.118085
+ 498 0.118085
+ 499 0.118085
+ 500 0.0552082
+ 501 0.0552082
+ 502 0.0552082
+ 503 0.0552082
+ 504 0.0552082
+ 505 0.0552082
+ 506 0.702913
+ 507 0.702913
+ 508 0.702913
+ 509 0.702913
+ 510 0.702913
+ 511 0.702913
+ 512 0.11715
+ 513 0.11715
+ 514 0.11715
+ 515 0.11715
+ 516 0.11715
+ 517 0.11715
+ 518 0.0556777
+ 519 0.0556777
+ 520 0.0556777
+ 521 0.0556777
+ 522 0.0556777
+ 523 0.705151
+ 524 0.705151
+ 525 0.705151
+ 526 0.705151
+ 527 0.705151
+ 528 0.117498
+ 529 0.117498
+ 530 0.117498
+ 531 0.117498
+ 532 0.117498
+ 533 0.117498
+ 534 0.0595562
+ 535 0.0595562
+ 536 0.0595562
+ 537 0.0595562
+ 538 0.0595562
+ 539 0.0595562
+ 540 0.675713
+ 541 0.675713
+ 542 0.675713
+ 543 0.675713
+ 544 0.675713
+ 545 0.675713
+ 546 0.117429
+ 547 0.117429
+ 548 0.117429
+ 549 0.117429
+ 550 0.117429
+ 551 0.117429
+ 552 0.0596137
+ 553 0.0596137
+ 554 0.0596137
+ 555 0.0596137
+ 556 0.0596137
+ 557 0.636998
+ 558 0.636998
+ 559 0.636998
+ 560 0.636998
+ 561 0.636998
+ 562 0.117068
+ 563 0.117068
+ 564 0.117068
+ 565 0.117068
+ 566 0.117068
+ 567 0.117068
+ 568 0.0557424
+ 569 0.0557424
+ 570 0.0557424
+ 571 0.0557424
+ 572 0.0557424
+ 573 0.0557424
+ 574 0.627912
+ 575 0.627912
+ 576 0.627912
+ 577 0.627912
+ 578 0.627912
+ 579 0.116074
+ 580 0.116074
+ 581 0.116074
+ 582 0.116074
+ 583 0.116074
+ 584 0.116074
+ 585 0.0554518
+ 586 0.0554518
+ 587 0.0554518
+ 588 0.0554518
+ 589 0.0554518
+ 590 0.0554518
+ 591 0.641835
+ 592 0.641835
+ 593 0.641835
+ 594 0.641835
+ 595 0.641835
+ 596 0.116331
+ 597 0.116331
+ 598 0.116331
+ 599 0.116331
+ 600 0.116331
+ 601 0.116331
+ 602 0.0582382
+ 603 0.0582382
+ 604 0.0582382
+ 605 0.0582382
+ 606 0.0582382
+ 607 0.0582382
+ 608 0.600511
+ 609 0.600511
+ 610 0.600511
+ 611 0.600511
+ 612 0.600511
+ 613 0.600511
+ 614 0.116458
+ 615 0.116458
+ 616 0.116458
+ 617 0.116458
+ 618 0.116458
+ 619 0.116458
+ 620 0.0584118
+ 621 0.0584118
+ 622 0.0584118
+ 623 0.0584118
+ 624 0.0584118
+ 625 0.577143
+ 626 0.577143
+ 627 0.577143
+ 628 0.577143
+ 629 0.577143
+ 630 0.116709
+ 631 0.116709
+ 632 0.116709
+ 633 0.116709
+ 634 0.116709
+ 635 0.116709
+ 636 0.055461
+ 637 0.055461
+ 638 0.055461
+ 639 0.055461
+ 640 0.055461
+ 641 0.603492
+ 642 0.603492
+ 643 0.603492
+ 644 0.603492
+ 645 0.603492
+ 646 0.603492
+ 647 0.0485551
+ 648 0.0485551
+ 649 0.0485551
+ 650 0.0485551
+ 651 0.0485551
+ 652 0.0485551
+ 653 0.691429
+ 654 0.691429
+ 655 0.691429
+ 656 0.691429
+ 657 0.691429
+ 658 0.116582
+ 659 0.116582
+ 660 0.116582
+ 661 0.116582
+ 662 0.116582
+ 663 0.116582
+ 664 0.533249
+ 665 0.533249
+ 666 0.533249
+ 667 0.533249
+ 668 0.533249
+ 669 0.533249
+ 670 0.0586392
+ 671 0.0586392
+ 672 0.0586392
+ 673 0.0586392
+ 674 0.0586392
+ 675 0.0586392
+ 676 0.695704
+ 677 0.695704
+ 678 0.695704
+ 679 0.695704
+ 680 0.695704
+ 681 0.535191
+ 682 0.535191
+ 683 0.535191
+ 684 0.535191
+ 685 0.535191
+ 686 0.535191
+ 687 0.0570583
+ 688 0.0570583
+ 689 0.0570583
+ 690 0.0570583
+ 691 0.0570583
+ 692 0.0570583
+ 693 0.720179
+ 694 0.720179
+ 695 0.720179
+ 696 0.720179
+ 697 0.720179
+ 698 0.537441
+ 699 0.537441
+ 700 0.537441
+ 701 0.537441
+ 702 0.537441
+ 703 0.537441
+ 704 0.0581637
+ 705 0.0581637
+ 706 0.0581637
+ 707 0.0581637
+ 708 0.0581637
+ 709 0.0581637
+ 710 0.702459
+ 711 0.702459
+ 712 0.702459
+ 713 0.702459
+ 714 0.702459
+ 715 0.702459
+ 716 0.536746
+ 717 0.536746
+ 718 0.536746
+ 719 0.536746
+ 720 0.536746
+ 721 0.536746
+ 722 0.0478414
+ 723 0.0478414
+ 724 0.0478414
+ 725 0.0478414
+ 726 0.0478414
+ 727 0.72579
+ 728 0.72579
+ 729 0.72579
+ 730 0.72579
+ 731 0.72579
+ 732 0.532719
+ 733 0.532719
+ 734 0.532719
+ 735 0.532719
+ 736 0.532719
+ 737 0.532719
+ 738 0.0586395
+ 739 0.0586395
+ 740 0.0586395
+ 741 0.0586395
+ 742 0.0586395
+ 743 0.0586395
+ 744 0.648489
+ 745 0.648489
+ 746 0.648489
+ 747 0.648489
+ 748 0.648489
+ 749 0.648489
+ 750 0.521925
+ 751 0.521925
+ 752 0.521925
+ 753 0.521925
+ 754 0.521925
+ 755 0.521925
+ 756 0.0575213
+ 757 0.0575213
+ 758 0.0575213
+ 759 0.0575213
+ 760 0.0575213
+ 761 0.64483
+ 762 0.64483
+ 763 0.64483
+ 764 0.64483
+ 765 0.64483
+ 766 0.64483
+ 767 0.518645
+ 768 0.518645
+ 769 0.518645
+ 770 0.518645
+ 771 0.518645
+ 772 0.518645
+ 773 0.0587439
+ 774 0.0587439
+ 775 0.0587439
+ 776 0.0587439
+ 777 0.0587439
+ 778 0.0587439
+ 779 0.628935
+ 780 0.628935
+ 781 0.628935
+ 782 0.628935
+ 783 0.628935
+ 784 0.628935
+ 785 0.518782
+ 786 0.518782
+ 787 0.518782
+ 788 0.518782
+ 789 0.518782
+ 790 0.518782
+ 791 0.0484385
+ 792 0.0484385
+ 793 0.0484385
+ 794 0.0484385
+ 795 0.0484385
+ 796 0.6766
+ 797 0.6766
+ 798 0.6766
+ 799 0.6766
+ 800 0.6766
+ 801 0.6766
+ 802 0.518391
+ 803 0.518391
+ 804 0.518391
+ 805 0.518391
+ 806 0.518391
+ 807 0.518391
+ 808 0.0577665
+ 809 0.0577665
+ 810 0.0577665
+ 811 0.0577665
+ 812 0.0577665
+ 813 0.576605
+ 814 0.576605
+ 815 0.576605
+ 816 0.576605
+ 817 0.576605
+ 818 0.576605
+ 819 0.510493
+ 820 0.510493
+ 821 0.510493
+ 822 0.510493
+ 823 0.510493
+ 824 0.510493
+ 825 0.0560403
+ 826 0.0560403
+ 827 0.0560403
+ 828 0.0560403
+ 829 0.0560403
+ 830 0.592783
+ 831 0.592783
+ 832 0.592783
+ 833 0.592783
+ 834 0.592783
+ 835 0.515418
+ 836 0.515418
+ 837 0.515418
+ 838 0.515418
+ 839 0.515418
+ 840 0.515418
+ 841 0.058051
+ 842 0.058051
+ 843 0.058051
+ 844 0.058051
+ 845 0.058051
+ 846 0.058051
+ 847 0.649668
+ 848 0.649668
+ 849 0.649668
+ 850 0.649668
+ 851 0.649668
+ 852 0.410309
+ 853 0.410309
+ 854 0.410309
+ 855 0.410309
+ 856 0.410309
+ 857 0.410309
+ 858 0.409519
+ 859 0.409519
+ 860 0.409519
+ 861 0.409519
+ 862 0.409519
+ 863 0.409519
+ 864 0.116426
+ 865 0.116426
+ 866 0.116426
+ 867 0.116426
+ 868 0.116426
+ 869 0.116426
+ 870 0.532346
+ 871 0.532346
+ 872 0.532346
+ 873 0.532346
+ 874 0.532346
+ 875 0.532346
+ 876 0.411165
+ 877 0.411165
+ 878 0.411165
+ 879 0.411165
+ 880 0.411165
+ 881 0.411165
+ 882 0.410794
+ 883 0.410794
+ 884 0.410794
+ 885 0.410794
+ 886 0.410794
+ 887 0.410794
+ 888 0.117535
+ 889 0.117535
+ 890 0.117535
+ 891 0.117535
+ 892 0.117535
+ 893 0.117535
+ 894 0.533844
+ 895 0.533844
+ 896 0.533844
+ 897 0.533844
+ 898 0.533844
+ 899 0.533844
+ 900 0.411683
+ 901 0.411683
+ 902 0.411683
+ 903 0.411683
+ 904 0.411683
+ 905 0.411683
+ 906 0.411475
+ 907 0.411475
+ 908 0.411475
+ 909 0.411475
+ 910 0.411475
+ 911 0.411475
+ 912 0.117656
+ 913 0.117656
+ 914 0.117656
+ 915 0.117656
+ 916 0.117656
+ 917 0.117656
+ 918 0.536717
+ 919 0.536717
+ 920 0.536717
+ 921 0.536717
+ 922 0.536717
+ 923 0.536717
+ 924 0.411463
+ 925 0.411463
+ 926 0.411463
+ 927 0.411463
+ 928 0.411463
+ 929 0.411463
+ 930 0.411641
+ 931 0.411641
+ 932 0.411641
+ 933 0.411641
+ 934 0.411641
+ 935 0.411641
+ 936 0.117234
+ 937 0.117234
+ 938 0.117234
+ 939 0.117234
+ 940 0.117234
+ 941 0.117234
+ 942 0.536611
+ 943 0.536611
+ 944 0.536611
+ 945 0.536611
+ 946 0.536611
+ 947 0.536611
+ 948 0.410672
+ 949 0.410672
+ 950 0.410672
+ 951 0.410672
+ 952 0.410672
+ 953 0.410672
+ 954 0.411195
+ 955 0.411195
+ 956 0.411195
+ 957 0.411195
+ 958 0.411195
+ 959 0.411195
+ 960 0.116627
+ 961 0.116627
+ 962 0.116627
+ 963 0.116627
+ 964 0.116627
+ 965 0.116627
+ 966 0.536487
+ 967 0.536487
+ 968 0.536487
+ 969 0.536487
+ 970 0.536487
+ 971 0.536487
+ 972 0.40864
+ 973 0.40864
+ 974 0.40864
+ 975 0.40864
+ 976 0.40864
+ 977 0.40864
+ 978 0.409689
+ 979 0.409689
+ 980 0.409689
+ 981 0.409689
+ 982 0.409689
+ 983 0.409689
+ 984 0.117148
+ 985 0.117148
+ 986 0.117148
+ 987 0.117148
+ 988 0.117148
+ 989 0.117148
+ 990 0.525977
+ 991 0.525977
+ 992 0.525977
+ 993 0.525977
+ 994 0.525977
+ 995 0.525977
+ 996 0.407355
+ 997 0.407355
+ 998 0.407355
+ 999 0.407355
+ 1000 0.407355
+ 1001 0.407355
+ 1002 0.407854
+ 1003 0.407854
+ 1004 0.407854
+ 1005 0.407854
+ 1006 0.407854
+ 1007 0.407854
+ 1008 0.116803
+ 1009 0.116803
+ 1010 0.116803
+ 1011 0.116803
+ 1012 0.116803
+ 1013 0.116803
+ 1014 0.519599
+ 1015 0.519599
+ 1016 0.519599
+ 1017 0.519599
+ 1018 0.519599
+ 1019 0.519599
+ 1020 0.406864
+ 1021 0.406864
+ 1022 0.406864
+ 1023 0.406864
+ 1024 0.406864
+ 1025 0.406864
+ 1026 0.407012
+ 1027 0.407012
+ 1028 0.407012
+ 1029 0.407012
+ 1030 0.407012
+ 1031 0.407012
+ 1032 0.116194
+ 1033 0.116194
+ 1034 0.116194
+ 1035 0.116194
+ 1036 0.116194
+ 1037 0.116194
+ 1038 0.517497
+ 1039 0.517497
+ 1040 0.517497
+ 1041 0.517497
+ 1042 0.517497
+ 1043 0.517497
+ 1044 0.406762
+ 1045 0.406762
+ 1046 0.406762
+ 1047 0.406762
+ 1048 0.406762
+ 1049 0.406762
+ 1050 0.406875
+ 1051 0.406875
+ 1052 0.406875
+ 1053 0.406875
+ 1054 0.406875
+ 1055 0.406875
+ 1056 0.115553
+ 1057 0.115553
+ 1058 0.115553
+ 1059 0.115553
+ 1060 0.115553
+ 1061 0.115553
+ 1062 0.520202
+ 1063 0.520202
+ 1064 0.520202
+ 1065 0.520202
+ 1066 0.520202
+ 1067 0.520202
+ 1068 0.405691
+ 1069 0.405691
+ 1070 0.405691
+ 1071 0.405691
+ 1072 0.405691
+ 1073 0.405691
+ 1074 0.40622
+ 1075 0.40622
+ 1076 0.40622
+ 1077 0.40622
+ 1078 0.40622
+ 1079 0.40622
+ 1080 0.116043
+ 1081 0.116043
+ 1082 0.116043
+ 1083 0.116043
+ 1084 0.116043
+ 1085 0.116043
+ 1086 0.513728
+ 1087 0.513728
+ 1088 0.513728
+ 1089 0.513728
+ 1090 0.513728
+ 1091 0.513728
+ 1092 0.513728
+ 1093 0.408328
+ 1094 0.408328
+ 1095 0.408328
+ 1096 0.408328
+ 1097 0.408328
+ 1098 0.408328
+ 1099 0.407093
+ 1100 0.407093
+ 1101 0.407093
+ 1102 0.407093
+ 1103 0.407093
+ 1104 0.407093
+ 1105 0.116205
+ 1106 0.116205
+ 1107 0.116205
+ 1108 0.116205
+ 1109 0.116205
+ 1110 0.116205
+ 1111 0.521077
+ 1112 0.521077
+ 1113 0.521077
+ 1114 0.521077
+ 1115 0.521077
+ 1116 0.521077
+ 1117 0.406172
+ 1118 0.406172
+ 1119 0.406172
+ 1120 0.406172
+ 1121 0.406172
+ 1122 0.406172
+ 1123 0.405649
+ 1124 0.405649
+ 1125 0.405649
+ 1126 0.405649
+ 1127 0.405649
+ 1128 0.405649
+ 1129 0.116095
+ 1130 0.116095
+ 1131 0.116095
+ 1132 0.116095
+ 1133 0.116095
+ 1134 0.116095
+ 1135 0.512592
+ 1136 0.512592
+ 1137 0.512592
+ 1138 0.512592
+ 1139 0.512592
+ 1140 0.512592
+ 1141 0.532998
+ 1142 0.532998
+ 1143 0.532998
+ 1144 0.532998
+ 1145 0.532998
+ 1146 0.532998
+ 1147 0.534609
+ 1148 0.534609
+ 1149 0.534609
+ 1150 0.534609
+ 1151 0.534609
+ 1152 0.534609
+ 1153 0.537637
+ 1154 0.537637
+ 1155 0.537637
+ 1156 0.537637
+ 1157 0.537637
+ 1158 0.537637
+ 1159 0.536753
+ 1160 0.536753
+ 1161 0.536753
+ 1162 0.536753
+ 1163 0.536753
+ 1164 0.536753
+ 1165 0.535728
+ 1166 0.535728
+ 1167 0.535728
+ 1168 0.535728
+ 1169 0.535728
+ 1170 0.535728
+ 1171 0.523952
+ 1172 0.523952
+ 1173 0.523952
+ 1174 0.523952
+ 1175 0.523952
+ 1176 0.523952
+ 1177 0.51959
+ 1178 0.51959
+ 1179 0.51959
+ 1180 0.51959
+ 1181 0.51959
+ 1182 0.51959
+ 1183 0.518243
+ 1184 0.518243
+ 1185 0.518243
+ 1186 0.518243
+ 1187 0.518243
+ 1188 0.518243
+ 1189 0.520723
+ 1190 0.520723
+ 1191 0.520723
+ 1192 0.520723
+ 1193 0.520723
+ 1194 0.520723
+ 1195 0.511297
+ 1196 0.511297
+ 1197 0.511297
+ 1198 0.511297
+ 1199 0.511297
+ 1200 0.511297
+ 1201 0.524385
+ 1202 0.524385
+ 1203 0.524385
+ 1204 0.524385
+ 1205 0.524385
+ 1206 0.524385
+ 1207 0.513435
+ 1208 0.513435
+ 1209 0.513435
+ 1210 0.513435
+ 1211 0.513435
+ 1212 0.513435
+ 1213 0.116967
+ 1214 0.116967
+ 1215 0.116967
+ 1216 0.116967
+ 1217 0.116967
+ 1218 0.116967
+ 1219 0.116342
+ 1220 0.116342
+ 1221 0.116342
+ 1222 0.116342
+ 1223 0.116342
+ 1224 0.116342
+ 1225 0.531194
+ 1226 0.531194
+ 1227 0.531194
+ 1228 0.531194
+ 1229 0.531194
+ 1230 0.531194
+ 1231 0.117819
+ 1232 0.117819
+ 1233 0.117819
+ 1234 0.117819
+ 1235 0.117819
+ 1236 0.117819
+ 1237 0.117551
+ 1238 0.117551
+ 1239 0.117551
+ 1240 0.117551
+ 1241 0.117551
+ 1242 0.117551
+ 1243 0.533594
+ 1244 0.533594
+ 1245 0.533594
+ 1246 0.533594
+ 1247 0.533594
+ 1248 0.533594
+ 1249 0.117864
+ 1250 0.117864
+ 1251 0.117864
+ 1252 0.117864
+ 1253 0.117864
+ 1254 0.117864
+ 1255 0.117811
+ 1256 0.117811
+ 1257 0.117811
+ 1258 0.117811
+ 1259 0.117811
+ 1260 0.117811
+ 1261 0.535981
+ 1262 0.535981
+ 1263 0.535981
+ 1264 0.535981
+ 1265 0.535981
+ 1266 0.535981
+ 1267 0.117146
+ 1268 0.117146
+ 1269 0.117146
+ 1270 0.117146
+ 1271 0.117146
+ 1272 0.117146
+ 1273 0.11765
+ 1274 0.11765
+ 1275 0.11765
+ 1276 0.11765
+ 1277 0.11765
+ 1278 0.11765
+ 1279 0.537053
+ 1280 0.537053
+ 1281 0.537053
+ 1282 0.537053
+ 1283 0.537053
+ 1284 0.537053
+ 1285 0.117029
+ 1286 0.117029
+ 1287 0.117029
+ 1288 0.117029
+ 1289 0.117029
+ 1290 0.117029
+ 1291 0.11669
+ 1292 0.11669
+ 1293 0.11669
+ 1294 0.11669
+ 1295 0.11669
+ 1296 0.11669
+ 1297 0.536812
+ 1298 0.536812
+ 1299 0.536812
+ 1300 0.536812
+ 1301 0.536812
+ 1302 0.536812
+ 1303 0.117275
+ 1304 0.117275
+ 1305 0.117275
+ 1306 0.117275
+ 1307 0.117275
+ 1308 0.117275
+ 1309 0.117316
+ 1310 0.117316
+ 1311 0.117316
+ 1312 0.117316
+ 1313 0.117316
+ 1314 0.117316
+ 1315 0.52955
+ 1316 0.52955
+ 1317 0.52955
+ 1318 0.52955
+ 1319 0.52955
+ 1320 0.52955
+ 1321 0.116922
+ 1322 0.116922
+ 1323 0.116922
+ 1324 0.116922
+ 1325 0.116922
+ 1326 0.116922
+ 1327 0.117038
+ 1328 0.117038
+ 1329 0.117038
+ 1330 0.117038
+ 1331 0.117038
+ 1332 0.117038
+ 1333 0.520788
+ 1334 0.520788
+ 1335 0.520788
+ 1336 0.520788
+ 1337 0.520788
+ 1338 0.520788
+ 1339 0.11609
+ 1340 0.11609
+ 1341 0.11609
+ 1342 0.11609
+ 1343 0.11609
+ 1344 0.11609
+ 1345 0.116618
+ 1346 0.116618
+ 1347 0.116618
+ 1348 0.116618
+ 1349 0.116618
+ 1350 0.116618
+ 1351 0.518144
+ 1352 0.518144
+ 1353 0.518144
+ 1354 0.518144
+ 1355 0.518144
+ 1356 0.518144
+ 1357 0.115908
+ 1358 0.115908
+ 1359 0.115908
+ 1360 0.115908
+ 1361 0.115908
+ 1362 0.115908
+ 1363 0.115628
+ 1364 0.115628
+ 1365 0.115628
+ 1366 0.115628
+ 1367 0.115628
+ 1368 0.115628
+ 1369 0.51984
+ 1370 0.51984
+ 1371 0.51984
+ 1372 0.51984
+ 1373 0.51984
+ 1374 0.51984
+ 1375 0.116223
+ 1376 0.116223
+ 1377 0.116223
+ 1378 0.116223
+ 1379 0.116223
+ 1380 0.116223
+ 1381 0.116171
+ 1382 0.116171
+ 1383 0.116171
+ 1384 0.116171
+ 1385 0.116171
+ 1386 0.116171
+ 1387 0.515533
+ 1388 0.515533
+ 1389 0.515533
+ 1390 0.515533
+ 1391 0.515533
+ 1392 0.116361
+ 1393 0.116361
+ 1394 0.116361
+ 1395 0.116361
+ 1396 0.116361
+ 1397 0.116361
+ 1398 0.116448
+ 1399 0.116448
+ 1400 0.116448
+ 1401 0.116448
+ 1402 0.116448
+ 1403 0.116448
+ 1404 0.517509
+ 1405 0.517509
+ 1406 0.517509
+ 1407 0.517509
+ 1408 0.517509
+ 1409 0.517509
+ 1410 0.116379
+ 1411 0.116379
+ 1412 0.116379
+ 1413 0.116379
+ 1414 0.116379
+ 1415 0.116379
+ 1416 0.116183
+ 1417 0.116183
+ 1418 0.116183
+ 1419 0.116183
+ 1420 0.116183
+ 1421 0.116183
+ 1422 0.510798
+ 1423 0.510798
+ 1424 0.510798
+ 1425 0.510798
+ 1426 0.510798
+ 1427 0.510798
+ }
+ VertexInfluence "Bone_002" 1428 {
+ 0 0.598155
+ 1 0.598155
+ 2 0.598155
+ 3 0.598155
+ 4 0.0582628
+ 5 0.0582628
+ 6 0.0582628
+ 7 0.0582628
+ 8 0.574353
+ 9 0.574353
+ 10 0.574353
+ 11 0.574353
+ 12 0.0690805
+ 13 0.0690805
+ 14 0.0690805
+ 15 0.0690805
+ 16 0.576778
+ 17 0.576778
+ 18 0.576778
+ 19 0.576778
+ 20 0.0726471
+ 21 0.0726471
+ 22 0.0726471
+ 23 0.0726471
+ 24 0.56066
+ 25 0.56066
+ 26 0.56066
+ 27 0.56066
+ 28 0.0699223
+ 29 0.0699223
+ 30 0.0699223
+ 31 0.0699223
+ 32 0.604528
+ 33 0.604528
+ 34 0.604528
+ 35 0.604528
+ 36 0.0679579
+ 37 0.0679579
+ 38 0.0679579
+ 39 0.0679579
+ 40 0.586099
+ 41 0.586099
+ 42 0.586099
+ 43 0.586099
+ 44 0.0579427
+ 45 0.0579427
+ 46 0.0579427
+ 47 0.0579427
+ 48 0.59979
+ 49 0.59979
+ 50 0.59979
+ 51 0.59979
+ 52 0.0618867
+ 53 0.0618867
+ 54 0.0618867
+ 55 0.0618867
+ 56 0.602846
+ 57 0.602846
+ 58 0.602846
+ 59 0.602846
+ 60 0.0601907
+ 61 0.0601907
+ 62 0.0601907
+ 63 0.0601907
+ 64 0.640274
+ 65 0.640274
+ 66 0.640274
+ 67 0.640274
+ 68 0.0585797
+ 69 0.0585797
+ 70 0.0585797
+ 71 0.0585797
+ 72 0.597926
+ 73 0.597926
+ 74 0.597926
+ 75 0.597926
+ 76 0.0539
+ 77 0.0539
+ 78 0.0539
+ 79 0.0539
+ 80 0.56915
+ 81 0.56915
+ 82 0.56915
+ 83 0.56915
+ 84 0.0600049
+ 85 0.0600049
+ 86 0.0600049
+ 87 0.0600049
+ 88 0.554443
+ 89 0.554443
+ 90 0.554443
+ 91 0.554443
+ 92 0.0616531
+ 93 0.0616531
+ 94 0.0616531
+ 95 0.0616531
+ 96 0.587102
+ 97 0.587102
+ 98 0.587102
+ 99 0.587102
+ 100 0.587102
+ 101 0.587102
+ 102 0.587102
+ 103 0.0637755
+ 104 0.0637755
+ 105 0.0637755
+ 106 0.0637755
+ 107 0.0637755
+ 108 0.0637755
+ 109 0.0637755
+ 110 0.404264
+ 111 0.404264
+ 112 0.404264
+ 113 0.404264
+ 114 0.404264
+ 115 0.404264
+ 116 0.40436
+ 117 0.40436
+ 118 0.40436
+ 119 0.40436
+ 120 0.40436
+ 121 0.40436
+ 122 0.560407
+ 123 0.560407
+ 124 0.560407
+ 125 0.560407
+ 126 0.560407
+ 127 0.560407
+ 128 0.560407
+ 129 0.560407
+ 130 0.560407
+ 131 0.560407
+ 132 0.0732167
+ 133 0.0732167
+ 134 0.0732167
+ 135 0.0732167
+ 136 0.0732167
+ 137 0.0732167
+ 138 0.0732167
+ 139 0.0732167
+ 140 0.0732167
+ 141 0.404198
+ 142 0.404198
+ 143 0.404198
+ 144 0.404198
+ 145 0.404198
+ 146 0.404198
+ 147 0.557395
+ 148 0.557395
+ 149 0.557395
+ 150 0.557395
+ 151 0.557395
+ 152 0.557395
+ 153 0.557395
+ 154 0.0720451
+ 155 0.0720451
+ 156 0.0720451
+ 157 0.0720451
+ 158 0.0720451
+ 159 0.0720451
+ 160 0.0720451
+ 161 0.0720451
+ 162 0.404162
+ 163 0.404162
+ 164 0.404162
+ 165 0.404162
+ 166 0.404162
+ 167 0.404162
+ 168 0.56108
+ 169 0.56108
+ 170 0.56108
+ 171 0.56108
+ 172 0.56108
+ 173 0.56108
+ 174 0.56108
+ 175 0.56108
+ 176 0.56108
+ 177 0.56108
+ 178 0.0721328
+ 179 0.0721328
+ 180 0.0721328
+ 181 0.0721328
+ 182 0.0721328
+ 183 0.0721328
+ 184 0.0721328
+ 185 0.0721328
+ 186 0.404965
+ 187 0.404965
+ 188 0.404965
+ 189 0.404965
+ 190 0.404965
+ 191 0.404965
+ 192 0.595569
+ 193 0.595569
+ 194 0.595569
+ 195 0.595569
+ 196 0.595569
+ 197 0.595569
+ 198 0.595569
+ 199 0.595569
+ 200 0.0630788
+ 201 0.0630788
+ 202 0.0630788
+ 203 0.0630788
+ 204 0.0630788
+ 205 0.0630788
+ 206 0.0630788
+ 207 0.0630788
+ 208 0.404967
+ 209 0.404967
+ 210 0.404967
+ 211 0.404967
+ 212 0.404967
+ 213 0.404967
+ 214 0.576136
+ 215 0.576136
+ 216 0.576136
+ 217 0.576136
+ 218 0.576136
+ 219 0.576136
+ 220 0.576136
+ 221 0.576136
+ 222 0.0616774
+ 223 0.0616774
+ 224 0.0616774
+ 225 0.0616774
+ 226 0.0616774
+ 227 0.0616774
+ 228 0.0616774
+ 229 0.0616774
+ 230 0.0616774
+ 231 0.0616774
+ 232 0.405261
+ 233 0.405261
+ 234 0.405261
+ 235 0.405261
+ 236 0.405261
+ 237 0.405261
+ 238 0.592427
+ 239 0.592427
+ 240 0.592427
+ 241 0.592427
+ 242 0.592427
+ 243 0.592427
+ 244 0.592427
+ 245 0.592427
+ 246 0.0613356
+ 247 0.0613356
+ 248 0.0613356
+ 249 0.0613356
+ 250 0.0613356
+ 251 0.0613356
+ 252 0.0613356
+ 253 0.405806
+ 254 0.405806
+ 255 0.405806
+ 256 0.405806
+ 257 0.405806
+ 258 0.405806
+ 259 0.601937
+ 260 0.601937
+ 261 0.601937
+ 262 0.601937
+ 263 0.601937
+ 264 0.601937
+ 265 0.601937
+ 266 0.601937
+ 267 0.601937
+ 268 0.0612082
+ 269 0.0612082
+ 270 0.0612082
+ 271 0.0612082
+ 272 0.0612082
+ 273 0.0612082
+ 274 0.0612082
+ 275 0.0612082
+ 276 0.0612082
+ 277 0.406295
+ 278 0.406295
+ 279 0.406295
+ 280 0.406295
+ 281 0.406295
+ 282 0.406295
+ 283 0.621235
+ 284 0.621235
+ 285 0.621235
+ 286 0.621235
+ 287 0.621235
+ 288 0.621235
+ 289 0.621235
+ 290 0.0550011
+ 291 0.0550011
+ 292 0.0550011
+ 293 0.0550011
+ 294 0.0550011
+ 295 0.0550011
+ 296 0.0550011
+ 297 0.405013
+ 298 0.405013
+ 299 0.405013
+ 300 0.405013
+ 301 0.405013
+ 302 0.405013
+ 303 0.561939
+ 304 0.561939
+ 305 0.561939
+ 306 0.561939
+ 307 0.561939
+ 308 0.561939
+ 309 0.561939
+ 310 0.561939
+ 311 0.561939
+ 312 0.561939
+ 313 0.561939
+ 314 0.057139
+ 315 0.057139
+ 316 0.057139
+ 317 0.057139
+ 318 0.057139
+ 319 0.057139
+ 320 0.057139
+ 321 0.057139
+ 322 0.403626
+ 323 0.403626
+ 324 0.403626
+ 325 0.403626
+ 326 0.403626
+ 327 0.403626
+ 328 0.552706
+ 329 0.552706
+ 330 0.552706
+ 331 0.552706
+ 332 0.552706
+ 333 0.552706
+ 334 0.552706
+ 335 0.552706
+ 336 0.0588137
+ 337 0.0588137
+ 338 0.0588137
+ 339 0.0588137
+ 340 0.0588137
+ 341 0.0588137
+ 342 0.0588137
+ 343 0.0588137
+ 344 0.0588137
+ 345 0.0588137
+ 346 0.0588137
+ 347 0.0588137
+ 348 0.403396
+ 349 0.403396
+ 350 0.403396
+ 351 0.403396
+ 352 0.403396
+ 353 0.403396
+ 354 0.577441
+ 355 0.577441
+ 356 0.577441
+ 357 0.577441
+ 358 0.577441
+ 359 0.577441
+ 360 0.577441
+ 361 0.577441
+ 362 0.577441
+ 363 0.0601323
+ 364 0.0601323
+ 365 0.0601323
+ 366 0.0601323
+ 367 0.0601323
+ 368 0.0601323
+ 369 0.0601323
+ 370 0.0601323
+ 371 0.404841
+ 372 0.404841
+ 373 0.404841
+ 374 0.404841
+ 375 0.404841
+ 376 0.404841
+ 377 0.404642
+ 378 0.404642
+ 379 0.404642
+ 380 0.404642
+ 381 0.404642
+ 382 0.404642
+ 383 0.404494
+ 384 0.404494
+ 385 0.404494
+ 386 0.404494
+ 387 0.404494
+ 388 0.404494
+ 389 0.404896
+ 390 0.404896
+ 391 0.404896
+ 392 0.404896
+ 393 0.404896
+ 394 0.404896
+ 395 0.405469
+ 396 0.405469
+ 397 0.405469
+ 398 0.405469
+ 399 0.405469
+ 400 0.405469
+ 401 0.405397
+ 402 0.405397
+ 403 0.405397
+ 404 0.405397
+ 405 0.405397
+ 406 0.405397
+ 407 0.405906
+ 408 0.405906
+ 409 0.405906
+ 410 0.405906
+ 411 0.405906
+ 412 0.405906
+ 413 0.406506
+ 414 0.406506
+ 415 0.406506
+ 416 0.406506
+ 417 0.406506
+ 418 0.406506
+ 419 0.40626
+ 420 0.40626
+ 421 0.40626
+ 422 0.40626
+ 423 0.40626
+ 424 0.40626
+ 425 0.404552
+ 426 0.404552
+ 427 0.404552
+ 428 0.404552
+ 429 0.404552
+ 430 0.404552
+ 431 0.404184
+ 432 0.404184
+ 433 0.404184
+ 434 0.404184
+ 435 0.404184
+ 436 0.404184
+ 437 0.403743
+ 438 0.403743
+ 439 0.403743
+ 440 0.403743
+ 441 0.403743
+ 442 0.403743
+ 443 0.589523
+ 444 0.589523
+ 445 0.589523
+ 446 0.589523
+ 447 0.589523
+ 448 0.0552031
+ 449 0.0552031
+ 450 0.0552031
+ 451 0.0552031
+ 452 0.0552031
+ 453 0.0552031
+ 454 0.116564
+ 455 0.116564
+ 456 0.116564
+ 457 0.116564
+ 458 0.116564
+ 459 0.116564
+ 460 0.507737
+ 461 0.507737
+ 462 0.507737
+ 463 0.507737
+ 464 0.507737
+ 465 0.507737
+ 466 0.583154
+ 467 0.583154
+ 468 0.583154
+ 469 0.583154
+ 470 0.583154
+ 471 0.0695297
+ 472 0.0695297
+ 473 0.0695297
+ 474 0.0695297
+ 475 0.0695297
+ 476 0.0695297
+ 477 0.506298
+ 478 0.506298
+ 479 0.506298
+ 480 0.506298
+ 481 0.506298
+ 482 0.506298
+ 483 0.567147
+ 484 0.567147
+ 485 0.567147
+ 486 0.567147
+ 487 0.567147
+ 488 0.0712864
+ 489 0.0712864
+ 490 0.0712864
+ 491 0.0712864
+ 492 0.0712864
+ 493 0.0712864
+ 494 0.505047
+ 495 0.505047
+ 496 0.505047
+ 497 0.505047
+ 498 0.505047
+ 499 0.505047
+ 500 0.590716
+ 501 0.590716
+ 502 0.590716
+ 503 0.590716
+ 504 0.590716
+ 505 0.590716
+ 506 0.0699366
+ 507 0.0699366
+ 508 0.0699366
+ 509 0.0699366
+ 510 0.0699366
+ 511 0.0699366
+ 512 0.511597
+ 513 0.511597
+ 514 0.511597
+ 515 0.511597
+ 516 0.511597
+ 517 0.511597
+ 518 0.599921
+ 519 0.599921
+ 520 0.599921
+ 521 0.599921
+ 522 0.599921
+ 523 0.0643527
+ 524 0.0643527
+ 525 0.0643527
+ 526 0.0643527
+ 527 0.0643527
+ 528 0.511186
+ 529 0.511186
+ 530 0.511186
+ 531 0.511186
+ 532 0.511186
+ 533 0.511186
+ 534 0.603076
+ 535 0.603076
+ 536 0.603076
+ 537 0.603076
+ 538 0.603076
+ 539 0.603076
+ 540 0.059269
+ 541 0.059269
+ 542 0.059269
+ 543 0.059269
+ 544 0.059269
+ 545 0.059269
+ 546 0.512515
+ 547 0.512515
+ 548 0.512515
+ 549 0.512515
+ 550 0.512515
+ 551 0.512515
+ 552 0.613036
+ 553 0.613036
+ 554 0.613036
+ 555 0.613036
+ 556 0.613036
+ 557 0.0611269
+ 558 0.0611269
+ 559 0.0611269
+ 560 0.0611269
+ 561 0.0611269
+ 562 0.514589
+ 563 0.514589
+ 564 0.514589
+ 565 0.514589
+ 566 0.514589
+ 567 0.514589
+ 568 0.637212
+ 569 0.637212
+ 570 0.637212
+ 571 0.637212
+ 572 0.637212
+ 573 0.637212
+ 574 0.0601891
+ 575 0.0601891
+ 576 0.0601891
+ 577 0.0601891
+ 578 0.0601891
+ 579 0.519667
+ 580 0.519667
+ 581 0.519667
+ 582 0.519667
+ 583 0.519667
+ 584 0.519667
+ 585 0.625354
+ 586 0.625354
+ 587 0.625354
+ 588 0.625354
+ 589 0.625354
+ 590 0.625354
+ 591 0.0561587
+ 592 0.0561587
+ 593 0.0561587
+ 594 0.0561587
+ 595 0.0561587
+ 596 0.512534
+ 597 0.512534
+ 598 0.512534
+ 599 0.512534
+ 600 0.512534
+ 601 0.512534
+ 602 0.572912
+ 603 0.572912
+ 604 0.572912
+ 605 0.572912
+ 606 0.572912
+ 607 0.572912
+ 608 0.0574506
+ 609 0.0574506
+ 610 0.0574506
+ 611 0.0574506
+ 612 0.0574506
+ 613 0.0574506
+ 614 0.505162
+ 615 0.505162
+ 616 0.505162
+ 617 0.505162
+ 618 0.505162
+ 619 0.505162
+ 620 0.560628
+ 621 0.560628
+ 622 0.560628
+ 623 0.560628
+ 624 0.560628
+ 625 0.0610238
+ 626 0.0610238
+ 627 0.0610238
+ 628 0.0610238
+ 629 0.0610238
+ 630 0.503603
+ 631 0.503603
+ 632 0.503603
+ 633 0.503603
+ 634 0.503603
+ 635 0.503603
+ 636 0.585366
+ 637 0.585366
+ 638 0.585366
+ 639 0.585366
+ 640 0.585366
+ 641 0.0623888
+ 642 0.0623888
+ 643 0.0623888
+ 644 0.0623888
+ 645 0.0623888
+ 646 0.0623888
+ 647 0.638203
+ 648 0.638203
+ 649 0.638203
+ 650 0.638203
+ 651 0.638203
+ 652 0.638203
+ 653 0.065283
+ 654 0.065283
+ 655 0.065283
+ 656 0.065283
+ 657 0.065283
+ 658 0.510159
+ 659 0.510159
+ 660 0.510159
+ 661 0.510159
+ 662 0.510159
+ 663 0.510159
+ 664 0.118231
+ 665 0.118231
+ 666 0.118231
+ 667 0.118231
+ 668 0.118231
+ 669 0.118231
+ 670 0.583584
+ 671 0.583584
+ 672 0.583584
+ 673 0.583584
+ 674 0.583584
+ 675 0.583584
+ 676 0.0708357
+ 677 0.0708357
+ 678 0.0708357
+ 679 0.0708357
+ 680 0.0708357
+ 681 0.119047
+ 682 0.119047
+ 683 0.119047
+ 684 0.119047
+ 685 0.119047
+ 686 0.119047
+ 687 0.595147
+ 688 0.595147
+ 689 0.595147
+ 690 0.595147
+ 691 0.595147
+ 692 0.595147
+ 693 0.0665772
+ 694 0.0665772
+ 695 0.0665772
+ 696 0.0665772
+ 697 0.0665772
+ 698 0.118746
+ 699 0.118746
+ 700 0.118746
+ 701 0.118746
+ 702 0.118746
+ 703 0.118746
+ 704 0.577364
+ 705 0.577364
+ 706 0.577364
+ 707 0.577364
+ 708 0.577364
+ 709 0.577364
+ 710 0.0681478
+ 711 0.0681478
+ 712 0.0681478
+ 713 0.0681478
+ 714 0.0681478
+ 715 0.0681478
+ 716 0.118146
+ 717 0.118146
+ 718 0.118146
+ 719 0.118146
+ 720 0.118146
+ 721 0.118146
+ 722 0.646413
+ 723 0.646413
+ 724 0.646413
+ 725 0.646413
+ 726 0.646413
+ 727 0.0546473
+ 728 0.0546473
+ 729 0.0546473
+ 730 0.0546473
+ 731 0.0546473
+ 732 0.116752
+ 733 0.116752
+ 734 0.116752
+ 735 0.116752
+ 736 0.116752
+ 737 0.116752
+ 738 0.597644
+ 739 0.597644
+ 740 0.597644
+ 741 0.597644
+ 742 0.597644
+ 743 0.597644
+ 744 0.0610572
+ 745 0.0610572
+ 746 0.0610572
+ 747 0.0610572
+ 748 0.0610572
+ 749 0.0610572
+ 750 0.117041
+ 751 0.117041
+ 752 0.117041
+ 753 0.117041
+ 754 0.117041
+ 755 0.117041
+ 756 0.623138
+ 757 0.623138
+ 758 0.623138
+ 759 0.623138
+ 760 0.623138
+ 761 0.058441
+ 762 0.058441
+ 763 0.058441
+ 764 0.058441
+ 765 0.058441
+ 766 0.058441
+ 767 0.116932
+ 768 0.116932
+ 769 0.116932
+ 770 0.116932
+ 771 0.116932
+ 772 0.116932
+ 773 0.619449
+ 774 0.619449
+ 775 0.619449
+ 776 0.619449
+ 777 0.619449
+ 778 0.619449
+ 779 0.0591191
+ 780 0.0591191
+ 781 0.0591191
+ 782 0.0591191
+ 783 0.0591191
+ 784 0.0591191
+ 785 0.116482
+ 786 0.116482
+ 787 0.116482
+ 788 0.116482
+ 789 0.116482
+ 790 0.116482
+ 791 0.674294
+ 792 0.674294
+ 793 0.674294
+ 794 0.674294
+ 795 0.674294
+ 796 0.0487278
+ 797 0.0487278
+ 798 0.0487278
+ 799 0.0487278
+ 800 0.0487278
+ 801 0.0487278
+ 802 0.115685
+ 803 0.115685
+ 804 0.115685
+ 805 0.115685
+ 806 0.115685
+ 807 0.115685
+ 808 0.5969
+ 809 0.5969
+ 810 0.5969
+ 811 0.5969
+ 812 0.5969
+ 813 0.0602494
+ 814 0.0602494
+ 815 0.0602494
+ 816 0.0602494
+ 817 0.0602494
+ 818 0.0602494
+ 819 0.116266
+ 820 0.116266
+ 821 0.116266
+ 822 0.116266
+ 823 0.116266
+ 824 0.116266
+ 825 0.589349
+ 826 0.589349
+ 827 0.589349
+ 828 0.589349
+ 829 0.589349
+ 830 0.0617826
+ 831 0.0617826
+ 832 0.0617826
+ 833 0.0617826
+ 834 0.0617826
+ 835 0.116548
+ 836 0.116548
+ 837 0.116548
+ 838 0.116548
+ 839 0.116548
+ 840 0.116548
+ 841 0.570853
+ 842 0.570853
+ 843 0.570853
+ 844 0.570853
+ 845 0.570853
+ 846 0.570853
+ 847 0.0593951
+ 848 0.0593951
+ 849 0.0593951
+ 850 0.0593951
+ 851 0.0593951
+ 852 0.404638
+ 853 0.404638
+ 854 0.404638
+ 855 0.404638
+ 856 0.404638
+ 857 0.404638
+ 858 0.404639
+ 859 0.404639
+ 860 0.404639
+ 861 0.404639
+ 862 0.404639
+ 863 0.404639
+ 864 0.5101
+ 865 0.5101
+ 866 0.5101
+ 867 0.5101
+ 868 0.5101
+ 869 0.5101
+ 870 0.116803
+ 871 0.116803
+ 872 0.116803
+ 873 0.116803
+ 874 0.116803
+ 875 0.116803
+ 876 0.404444
+ 877 0.404444
+ 878 0.404444
+ 879 0.404444
+ 880 0.404444
+ 881 0.404444
+ 882 0.404521
+ 883 0.404521
+ 884 0.404521
+ 885 0.404521
+ 886 0.404521
+ 887 0.404521
+ 888 0.506148
+ 889 0.506148
+ 890 0.506148
+ 891 0.506148
+ 892 0.506148
+ 893 0.506148
+ 894 0.118404
+ 895 0.118404
+ 896 0.118404
+ 897 0.118404
+ 898 0.118404
+ 899 0.118404
+ 900 0.404315
+ 901 0.404315
+ 902 0.404315
+ 903 0.404315
+ 904 0.404315
+ 905 0.404315
+ 906 0.404378
+ 907 0.404378
+ 908 0.404378
+ 909 0.404378
+ 910 0.404378
+ 911 0.404378
+ 912 0.505019
+ 913 0.505019
+ 914 0.505019
+ 915 0.505019
+ 916 0.505019
+ 917 0.505019
+ 918 0.118446
+ 919 0.118446
+ 920 0.118446
+ 921 0.118446
+ 922 0.118446
+ 923 0.118446
+ 924 0.404965
+ 925 0.404965
+ 926 0.404965
+ 927 0.404965
+ 928 0.404965
+ 929 0.404965
+ 930 0.404526
+ 931 0.404526
+ 932 0.404526
+ 933 0.404526
+ 934 0.404526
+ 935 0.404526
+ 936 0.507029
+ 937 0.507029
+ 938 0.507029
+ 939 0.507029
+ 940 0.507029
+ 941 0.507029
+ 942 0.118214
+ 943 0.118214
+ 944 0.118214
+ 945 0.118214
+ 946 0.118214
+ 947 0.118214
+ 948 0.405236
+ 949 0.405236
+ 950 0.405236
+ 951 0.405236
+ 952 0.405236
+ 953 0.405236
+ 954 0.405311
+ 955 0.405311
+ 956 0.405311
+ 957 0.405311
+ 958 0.405311
+ 959 0.405311
+ 960 0.512392
+ 961 0.512392
+ 962 0.512392
+ 963 0.512392
+ 964 0.512392
+ 965 0.512392
+ 966 0.116842
+ 967 0.116842
+ 968 0.116842
+ 969 0.116842
+ 970 0.116842
+ 971 0.116842
+ 972 0.40534
+ 973 0.40534
+ 974 0.40534
+ 975 0.40534
+ 976 0.40534
+ 977 0.40534
+ 978 0.40518
+ 979 0.40518
+ 980 0.40518
+ 981 0.40518
+ 982 0.40518
+ 983 0.40518
+ 984 0.510807
+ 985 0.510807
+ 986 0.510807
+ 987 0.510807
+ 988 0.510807
+ 989 0.510807
+ 990 0.116457
+ 991 0.116457
+ 992 0.116457
+ 993 0.116457
+ 994 0.116457
+ 995 0.116457
+ 996 0.405873
+ 997 0.405873
+ 998 0.405873
+ 999 0.405873
+ 1000 0.405873
+ 1001 0.405873
+ 1002 0.405617
+ 1003 0.405617
+ 1004 0.405617
+ 1005 0.405617
+ 1006 0.405617
+ 1007 0.405617
+ 1008 0.513382
+ 1009 0.513382
+ 1010 0.513382
+ 1011 0.513382
+ 1012 0.513382
+ 1013 0.513382
+ 1014 0.116536
+ 1015 0.116536
+ 1016 0.116536
+ 1017 0.116536
+ 1018 0.116536
+ 1019 0.116536
+ 1020 0.406472
+ 1021 0.406472
+ 1022 0.406472
+ 1023 0.406472
+ 1024 0.406472
+ 1025 0.406472
+ 1026 0.406184
+ 1027 0.406184
+ 1028 0.406184
+ 1029 0.406184
+ 1030 0.406184
+ 1031 0.406184
+ 1032 0.516566
+ 1033 0.516566
+ 1034 0.516566
+ 1035 0.516566
+ 1036 0.516566
+ 1037 0.516566
+ 1038 0.11641
+ 1039 0.11641
+ 1040 0.11641
+ 1041 0.11641
+ 1042 0.11641
+ 1043 0.11641
+ 1044 0.405684
+ 1045 0.405684
+ 1046 0.405684
+ 1047 0.405684
+ 1048 0.405684
+ 1049 0.405684
+ 1050 0.406396
+ 1051 0.406396
+ 1052 0.406396
+ 1053 0.406396
+ 1054 0.406396
+ 1055 0.406396
+ 1056 0.517821
+ 1057 0.517821
+ 1058 0.517821
+ 1059 0.517821
+ 1060 0.517821
+ 1061 0.517821
+ 1062 0.115424
+ 1063 0.115424
+ 1064 0.115424
+ 1065 0.115424
+ 1066 0.115424
+ 1067 0.115424
+ 1068 0.404072
+ 1069 0.404072
+ 1070 0.404072
+ 1071 0.404072
+ 1072 0.404072
+ 1073 0.404072
+ 1074 0.404785
+ 1075 0.404785
+ 1076 0.404785
+ 1077 0.404785
+ 1078 0.404785
+ 1079 0.404785
+ 1080 0.507313
+ 1081 0.507313
+ 1082 0.507313
+ 1083 0.507313
+ 1084 0.507313
+ 1085 0.507313
+ 1086 0.115527
+ 1087 0.115527
+ 1088 0.115527
+ 1089 0.115527
+ 1090 0.115527
+ 1091 0.115527
+ 1092 0.115527
+ 1093 0.404256
+ 1094 0.404256
+ 1095 0.404256
+ 1096 0.404256
+ 1097 0.404256
+ 1098 0.404256
+ 1099 0.403785
+ 1100 0.403785
+ 1101 0.403785
+ 1102 0.403785
+ 1103 0.403785
+ 1104 0.403785
+ 1105 0.50667
+ 1106 0.50667
+ 1107 0.50667
+ 1108 0.50667
+ 1109 0.50667
+ 1110 0.50667
+ 1111 0.116069
+ 1112 0.116069
+ 1113 0.116069
+ 1114 0.116069
+ 1115 0.116069
+ 1116 0.116069
+ 1117 0.403544
+ 1118 0.403544
+ 1119 0.403544
+ 1120 0.403544
+ 1121 0.403544
+ 1122 0.403544
+ 1123 0.403687
+ 1124 0.403687
+ 1125 0.403687
+ 1126 0.403687
+ 1127 0.403687
+ 1128 0.403687
+ 1129 0.503588
+ 1130 0.503588
+ 1131 0.503588
+ 1132 0.503588
+ 1133 0.503588
+ 1134 0.503588
+ 1135 0.115894
+ 1136 0.115894
+ 1137 0.115894
+ 1138 0.115894
+ 1139 0.115894
+ 1140 0.115894
+ 1141 0.117496
+ 1142 0.117496
+ 1143 0.117496
+ 1144 0.117496
+ 1145 0.117496
+ 1146 0.117496
+ 1147 0.118709
+ 1148 0.118709
+ 1149 0.118709
+ 1150 0.118709
+ 1151 0.118709
+ 1152 0.118709
+ 1153 0.11847
+ 1154 0.11847
+ 1155 0.11847
+ 1156 0.11847
+ 1157 0.11847
+ 1158 0.11847
+ 1159 0.118171
+ 1160 0.118171
+ 1161 0.118171
+ 1162 0.118171
+ 1163 0.118171
+ 1164 0.118171
+ 1165 0.116581
+ 1166 0.116581
+ 1167 0.116581
+ 1168 0.116581
+ 1169 0.116581
+ 1170 0.116581
+ 1171 0.116727
+ 1172 0.116727
+ 1173 0.116727
+ 1174 0.116727
+ 1175 0.116727
+ 1176 0.116727
+ 1177 0.116657
+ 1178 0.116657
+ 1179 0.116657
+ 1180 0.116657
+ 1181 0.116657
+ 1182 0.116657
+ 1183 0.116443
+ 1184 0.116443
+ 1185 0.116443
+ 1186 0.116443
+ 1187 0.116443
+ 1188 0.116443
+ 1189 0.115361
+ 1190 0.115361
+ 1191 0.115361
+ 1192 0.115361
+ 1193 0.115361
+ 1194 0.115361
+ 1195 0.115911
+ 1196 0.115911
+ 1197 0.115911
+ 1198 0.115911
+ 1199 0.115911
+ 1200 0.115911
+ 1201 0.11626
+ 1202 0.11626
+ 1203 0.11626
+ 1204 0.11626
+ 1205 0.11626
+ 1206 0.11626
+ 1207 0.116221
+ 1208 0.116221
+ 1209 0.116221
+ 1210 0.116221
+ 1211 0.116221
+ 1212 0.116221
+ 1213 0.508977
+ 1214 0.508977
+ 1215 0.508977
+ 1216 0.508977
+ 1217 0.508977
+ 1218 0.508977
+ 1219 0.511594
+ 1220 0.511594
+ 1221 0.511594
+ 1222 0.511594
+ 1223 0.511594
+ 1224 0.511594
+ 1225 0.116476
+ 1226 0.116476
+ 1227 0.116476
+ 1228 0.116476
+ 1229 0.116476
+ 1230 0.116476
+ 1231 0.506393
+ 1232 0.506393
+ 1233 0.506393
+ 1234 0.506393
+ 1235 0.506393
+ 1236 0.506393
+ 1237 0.507048
+ 1238 0.507048
+ 1239 0.507048
+ 1240 0.507048
+ 1241 0.507048
+ 1242 0.507048
+ 1243 0.118316
+ 1244 0.118316
+ 1245 0.118316
+ 1246 0.118316
+ 1247 0.118316
+ 1248 0.118316
+ 1249 0.504857
+ 1250 0.504857
+ 1251 0.504857
+ 1252 0.504857
+ 1253 0.504857
+ 1254 0.504857
+ 1255 0.506248
+ 1256 0.506248
+ 1257 0.506248
+ 1258 0.506248
+ 1259 0.506248
+ 1260 0.506248
+ 1261 0.118738
+ 1262 0.118738
+ 1263 0.118738
+ 1264 0.118738
+ 1265 0.118738
+ 1266 0.118738
+ 1267 0.50935
+ 1268 0.50935
+ 1269 0.50935
+ 1270 0.50935
+ 1271 0.50935
+ 1272 0.50935
+ 1273 0.506054
+ 1274 0.506054
+ 1275 0.506054
+ 1276 0.506054
+ 1277 0.506054
+ 1278 0.506054
+ 1279 0.11847
+ 1280 0.11847
+ 1281 0.11847
+ 1282 0.11847
+ 1283 0.11847
+ 1284 0.11847
+ 1285 0.511842
+ 1286 0.511842
+ 1287 0.511842
+ 1288 0.511842
+ 1289 0.511842
+ 1290 0.511842
+ 1291 0.513364
+ 1292 0.513364
+ 1293 0.513364
+ 1294 0.513364
+ 1295 0.513364
+ 1296 0.513364
+ 1297 0.117473
+ 1298 0.117473
+ 1299 0.117473
+ 1300 0.117473
+ 1301 0.117473
+ 1302 0.117473
+ 1303 0.511786
+ 1304 0.511786
+ 1305 0.511786
+ 1306 0.511786
+ 1307 0.511786
+ 1308 0.511786
+ 1309 0.511063
+ 1310 0.511063
+ 1311 0.511063
+ 1312 0.511063
+ 1313 0.511063
+ 1314 0.511063
+ 1315 0.116543
+ 1316 0.116543
+ 1317 0.116543
+ 1318 0.116543
+ 1319 0.116543
+ 1320 0.116543
+ 1321 0.514116
+ 1322 0.514116
+ 1323 0.514116
+ 1324 0.514116
+ 1325 0.514116
+ 1326 0.514116
+ 1327 0.513524
+ 1328 0.513524
+ 1329 0.513524
+ 1330 0.513524
+ 1331 0.513524
+ 1332 0.513524
+ 1333 0.116773
+ 1334 0.116773
+ 1335 0.116773
+ 1336 0.116773
+ 1337 0.116773
+ 1338 0.116773
+ 1339 0.518506
+ 1340 0.518506
+ 1341 0.518506
+ 1342 0.518506
+ 1343 0.518506
+ 1344 0.518506
+ 1345 0.515716
+ 1346 0.515716
+ 1347 0.515716
+ 1348 0.515716
+ 1349 0.515716
+ 1350 0.515716
+ 1351 0.116662
+ 1352 0.116662
+ 1353 0.116662
+ 1354 0.116662
+ 1355 0.116662
+ 1356 0.116662
+ 1357 0.515539
+ 1358 0.515539
+ 1359 0.515539
+ 1360 0.515539
+ 1361 0.515539
+ 1362 0.515539
+ 1363 0.520186
+ 1364 0.520186
+ 1365 0.520186
+ 1366 0.520186
+ 1367 0.520186
+ 1368 0.520186
+ 1369 0.115923
+ 1370 0.115923
+ 1371 0.115923
+ 1372 0.115923
+ 1373 0.115923
+ 1374 0.115923
+ 1375 0.50603
+ 1376 0.50603
+ 1377 0.50603
+ 1378 0.50603
+ 1379 0.50603
+ 1380 0.50603
+ 1381 0.510017
+ 1382 0.510017
+ 1383 0.510017
+ 1384 0.510017
+ 1385 0.510017
+ 1386 0.510017
+ 1387 0.115599
+ 1388 0.115599
+ 1389 0.115599
+ 1390 0.115599
+ 1391 0.115599
+ 1392 0.508396
+ 1393 0.508396
+ 1394 0.508396
+ 1395 0.508396
+ 1396 0.508396
+ 1397 0.508396
+ 1398 0.505024
+ 1399 0.505024
+ 1400 0.505024
+ 1401 0.505024
+ 1402 0.505024
+ 1403 0.505024
+ 1404 0.116323
+ 1405 0.116323
+ 1406 0.116323
+ 1407 0.116323
+ 1408 0.116323
+ 1409 0.116323
+ 1410 0.503336
+ 1411 0.503336
+ 1412 0.503336
+ 1413 0.503336
+ 1414 0.503336
+ 1415 0.503336
+ 1416 0.504887
+ 1417 0.504887
+ 1418 0.504887
+ 1419 0.504887
+ 1420 0.504887
+ 1421 0.504887
+ 1422 0.116081
+ 1423 0.116081
+ 1424 0.116081
+ 1425 0.116081
+ 1426 0.116081
+ 1427 0.116081
+ }
+ VertexInfluence "Bone_003" 1032 {
+ 4 0.278159
+ 5 0.278159
+ 6 0.278159
+ 7 0.278159
+ 12 0.24515
+ 13 0.24515
+ 14 0.24515
+ 15 0.24515
+ 20 0.23507
+ 21 0.23507
+ 22 0.23507
+ 23 0.23507
+ 28 0.224104
+ 29 0.224104
+ 30 0.224104
+ 31 0.224104
+ 36 0.233595
+ 37 0.233595
+ 38 0.233595
+ 39 0.233595
+ 44 0.258175
+ 45 0.258175
+ 46 0.258175
+ 47 0.258175
+ 52 0.309799
+ 53 0.309799
+ 54 0.309799
+ 55 0.309799
+ 60 0.317662
+ 61 0.317662
+ 62 0.317662
+ 63 0.317662
+ 68 0.320206
+ 69 0.320206
+ 70 0.320206
+ 71 0.320206
+ 76 0.311216
+ 77 0.311216
+ 78 0.311216
+ 79 0.311216
+ 84 0.357141
+ 85 0.357141
+ 86 0.357141
+ 87 0.357141
+ 92 0.321442
+ 93 0.321442
+ 94 0.321442
+ 95 0.321442
+ 103 0.246048
+ 104 0.246048
+ 105 0.246048
+ 106 0.246048
+ 107 0.246048
+ 108 0.246048
+ 109 0.246048
+ 110 0.090729
+ 111 0.090729
+ 112 0.090729
+ 113 0.090729
+ 114 0.090729
+ 115 0.090729
+ 116 0.0887967
+ 117 0.0887967
+ 118 0.0887967
+ 119 0.0887967
+ 120 0.0887967
+ 121 0.0887967
+ 132 0.243541
+ 133 0.243541
+ 134 0.243541
+ 135 0.243541
+ 136 0.243541
+ 137 0.243541
+ 138 0.243541
+ 139 0.243541
+ 140 0.243541
+ 141 0.0878763
+ 142 0.0878763
+ 143 0.0878763
+ 144 0.0878763
+ 145 0.0878763
+ 146 0.0878763
+ 154 0.228804
+ 155 0.228804
+ 156 0.228804
+ 157 0.228804
+ 158 0.228804
+ 159 0.228804
+ 160 0.228804
+ 161 0.228804
+ 162 0.0875306
+ 163 0.0875306
+ 164 0.0875306
+ 165 0.0875306
+ 166 0.0875306
+ 167 0.0875306
+ 178 0.23533
+ 179 0.23533
+ 180 0.23533
+ 181 0.23533
+ 182 0.23533
+ 183 0.23533
+ 184 0.23533
+ 185 0.23533
+ 186 0.0880036
+ 187 0.0880036
+ 188 0.0880036
+ 189 0.0880036
+ 190 0.0880036
+ 191 0.0880036
+ 200 0.23241
+ 201 0.23241
+ 202 0.23241
+ 203 0.23241
+ 204 0.23241
+ 205 0.23241
+ 206 0.23241
+ 207 0.23241
+ 208 0.0895814
+ 209 0.0895814
+ 210 0.0895814
+ 211 0.0895814
+ 212 0.0895814
+ 213 0.0895814
+ 222 0.305104
+ 223 0.305104
+ 224 0.305104
+ 225 0.305104
+ 226 0.305104
+ 227 0.305104
+ 228 0.305104
+ 229 0.305104
+ 230 0.305104
+ 231 0.305104
+ 232 0.0917889
+ 233 0.0917889
+ 234 0.0917889
+ 235 0.0917889
+ 236 0.0917889
+ 237 0.0917889
+ 246 0.325085
+ 247 0.325085
+ 248 0.325085
+ 249 0.325085
+ 250 0.325085
+ 251 0.325085
+ 252 0.325085
+ 253 0.0928968
+ 254 0.0928968
+ 255 0.0928968
+ 256 0.0928968
+ 257 0.0928968
+ 258 0.0928968
+ 268 0.336697
+ 269 0.336697
+ 270 0.336697
+ 271 0.336697
+ 272 0.336697
+ 273 0.336697
+ 274 0.336697
+ 275 0.336697
+ 276 0.336697
+ 277 0.0932003
+ 278 0.0932003
+ 279 0.0932003
+ 280 0.0932003
+ 281 0.0932003
+ 282 0.0932003
+ 290 0.315012
+ 291 0.315012
+ 292 0.315012
+ 293 0.315012
+ 294 0.315012
+ 295 0.315012
+ 296 0.315012
+ 297 0.0936636
+ 298 0.0936636
+ 299 0.0936636
+ 300 0.0936636
+ 301 0.0936636
+ 302 0.0936636
+ 314 0.325298
+ 315 0.325298
+ 316 0.325298
+ 317 0.325298
+ 318 0.325298
+ 319 0.325298
+ 320 0.325298
+ 321 0.325298
+ 322 0.0944848
+ 323 0.0944848
+ 324 0.0944848
+ 325 0.0944848
+ 326 0.0944848
+ 327 0.0944848
+ 336 0.322382
+ 337 0.322382
+ 338 0.322382
+ 339 0.322382
+ 340 0.322382
+ 341 0.322382
+ 342 0.322382
+ 343 0.322382
+ 344 0.322382
+ 345 0.322382
+ 346 0.322382
+ 347 0.322382
+ 348 0.0933232
+ 349 0.0933232
+ 350 0.0933232
+ 351 0.0933232
+ 352 0.0933232
+ 353 0.0933232
+ 363 0.298046
+ 364 0.298046
+ 365 0.298046
+ 366 0.298046
+ 367 0.298046
+ 368 0.298046
+ 369 0.298046
+ 370 0.298046
+ 371 0.0891931
+ 372 0.0891931
+ 373 0.0891931
+ 374 0.0891931
+ 375 0.0891931
+ 376 0.0891931
+ 377 0.0879207
+ 378 0.0879207
+ 379 0.0879207
+ 380 0.0879207
+ 381 0.0879207
+ 382 0.0879207
+ 383 0.0872476
+ 384 0.0872476
+ 385 0.0872476
+ 386 0.0872476
+ 387 0.0872476
+ 388 0.0872476
+ 389 0.0873352
+ 390 0.0873352
+ 391 0.0873352
+ 392 0.0873352
+ 393 0.0873352
+ 394 0.0873352
+ 395 0.0882251
+ 396 0.0882251
+ 397 0.0882251
+ 398 0.0882251
+ 399 0.0882251
+ 400 0.0882251
+ 401 0.0904285
+ 402 0.0904285
+ 403 0.0904285
+ 404 0.0904285
+ 405 0.0904285
+ 406 0.0904285
+ 407 0.092096
+ 408 0.092096
+ 409 0.092096
+ 410 0.092096
+ 411 0.092096
+ 412 0.092096
+ 413 0.0927711
+ 414 0.0927711
+ 415 0.0927711
+ 416 0.0927711
+ 417 0.0927711
+ 418 0.0927711
+ 419 0.0929004
+ 420 0.0929004
+ 421 0.0929004
+ 422 0.0929004
+ 423 0.0929004
+ 424 0.0929004
+ 425 0.0938426
+ 426 0.0938426
+ 427 0.0938426
+ 428 0.0938426
+ 429 0.0938426
+ 430 0.0938426
+ 431 0.0917399
+ 432 0.0917399
+ 433 0.0917399
+ 434 0.0917399
+ 435 0.0917399
+ 436 0.0917399
+ 437 0.0937478
+ 438 0.0937478
+ 439 0.0937478
+ 440 0.0937478
+ 441 0.0937478
+ 442 0.0937478
+ 448 0.235339
+ 449 0.235339
+ 450 0.235339
+ 451 0.235339
+ 452 0.235339
+ 453 0.235339
+ 454 0.351355
+ 455 0.351355
+ 456 0.351355
+ 457 0.351355
+ 458 0.351355
+ 459 0.351355
+ 471 0.240276
+ 472 0.240276
+ 473 0.240276
+ 474 0.240276
+ 475 0.240276
+ 476 0.240276
+ 488 0.229473
+ 489 0.229473
+ 490 0.229473
+ 491 0.229473
+ 492 0.229473
+ 493 0.229473
+ 506 0.22715
+ 507 0.22715
+ 508 0.22715
+ 509 0.22715
+ 510 0.22715
+ 511 0.22715
+ 523 0.230496
+ 524 0.230496
+ 525 0.230496
+ 526 0.230496
+ 527 0.230496
+ 528 6.19344e-005
+ 529 6.19344e-005
+ 530 6.19344e-005
+ 531 6.19344e-005
+ 532 6.19344e-005
+ 533 6.19344e-005
+ 540 0.265018
+ 541 0.265018
+ 542 0.265018
+ 543 0.265018
+ 544 0.265018
+ 545 0.265018
+ 546 0.0012902
+ 547 0.0012902
+ 548 0.0012902
+ 549 0.0012902
+ 550 0.0012902
+ 551 0.0012902
+ 557 0.301875
+ 558 0.301875
+ 559 0.301875
+ 560 0.301875
+ 561 0.301875
+ 562 0.00192118
+ 563 0.00192118
+ 564 0.00192118
+ 565 0.00192118
+ 566 0.00192118
+ 567 0.00192118
+ 574 0.311899
+ 575 0.311899
+ 576 0.311899
+ 577 0.311899
+ 578 0.311899
+ 579 0.00180375
+ 580 0.00180375
+ 581 0.00180375
+ 582 0.00180375
+ 583 0.00180375
+ 584 0.00180375
+ 591 0.302006
+ 592 0.302006
+ 593 0.302006
+ 594 0.302006
+ 595 0.302006
+ 596 0.00225085
+ 597 0.00225085
+ 598 0.00225085
+ 599 0.00225085
+ 600 0.00225085
+ 601 0.00225085
+ 608 0.342038
+ 609 0.342038
+ 610 0.342038
+ 611 0.342038
+ 612 0.342038
+ 613 0.342038
+ 614 0.0025655
+ 615 0.0025655
+ 616 0.0025655
+ 617 0.0025655
+ 618 0.0025655
+ 619 0.0025655
+ 625 0.361833
+ 626 0.361833
+ 627 0.361833
+ 628 0.361833
+ 629 0.361833
+ 630 0.00196952
+ 631 0.00196952
+ 632 0.00196952
+ 633 0.00196952
+ 634 0.00196952
+ 635 0.00196952
+ 641 0.334119
+ 642 0.334119
+ 643 0.334119
+ 644 0.334119
+ 645 0.334119
+ 646 0.334119
+ 653 0.243288
+ 654 0.243288
+ 655 0.243288
+ 656 0.243288
+ 657 0.243288
+ 658 0.000443954
+ 659 0.000443954
+ 660 0.000443954
+ 661 0.000443954
+ 662 0.000443954
+ 663 0.000443954
+ 664 0.343379
+ 665 0.343379
+ 666 0.343379
+ 667 0.343379
+ 668 0.343379
+ 669 0.343379
+ 676 0.23346
+ 677 0.23346
+ 678 0.23346
+ 679 0.23346
+ 680 0.23346
+ 681 0.340088
+ 682 0.340088
+ 683 0.340088
+ 684 0.340088
+ 685 0.340088
+ 686 0.340088
+ 693 0.213244
+ 694 0.213244
+ 695 0.213244
+ 696 0.213244
+ 697 0.213244
+ 698 0.338361
+ 699 0.338361
+ 700 0.338361
+ 701 0.338361
+ 702 0.338361
+ 703 0.338361
+ 710 0.229394
+ 711 0.229394
+ 712 0.229394
+ 713 0.229394
+ 714 0.229394
+ 715 0.229394
+ 716 0.340395
+ 717 0.340395
+ 718 0.340395
+ 719 0.340395
+ 720 0.340395
+ 721 0.340395
+ 727 0.219563
+ 728 0.219563
+ 729 0.219563
+ 730 0.219563
+ 731 0.219563
+ 732 0.346827
+ 733 0.346827
+ 734 0.346827
+ 735 0.346827
+ 736 0.346827
+ 737 0.346827
+ 744 0.290454
+ 745 0.290454
+ 746 0.290454
+ 747 0.290454
+ 748 0.290454
+ 749 0.290454
+ 750 0.35751
+ 751 0.35751
+ 752 0.35751
+ 753 0.35751
+ 754 0.35751
+ 755 0.35751
+ 761 0.296729
+ 762 0.296729
+ 763 0.296729
+ 764 0.296729
+ 765 0.296729
+ 766 0.296729
+ 767 0.36131
+ 768 0.36131
+ 769 0.36131
+ 770 0.36131
+ 771 0.36131
+ 772 0.36131
+ 779 0.311946
+ 780 0.311946
+ 781 0.311946
+ 782 0.311946
+ 783 0.311946
+ 784 0.311946
+ 785 0.361974
+ 786 0.361974
+ 787 0.361974
+ 788 0.361974
+ 789 0.361974
+ 790 0.361974
+ 796 0.274672
+ 797 0.274672
+ 798 0.274672
+ 799 0.274672
+ 800 0.274672
+ 801 0.274672
+ 802 0.362919
+ 803 0.362919
+ 804 0.362919
+ 805 0.362919
+ 806 0.362919
+ 807 0.362919
+ 813 0.363146
+ 814 0.363146
+ 815 0.363146
+ 816 0.363146
+ 817 0.363146
+ 818 0.363146
+ 819 0.369134
+ 820 0.369134
+ 821 0.369134
+ 822 0.369134
+ 823 0.369134
+ 824 0.369134
+ 830 0.345435
+ 831 0.345435
+ 832 0.345435
+ 833 0.345435
+ 834 0.345435
+ 835 0.363526
+ 836 0.363526
+ 837 0.363526
+ 838 0.363526
+ 839 0.363526
+ 840 0.363526
+ 847 0.290937
+ 848 0.290937
+ 849 0.290937
+ 850 0.290937
+ 851 0.290937
+ 852 0.0889319
+ 853 0.0889319
+ 854 0.0889319
+ 855 0.0889319
+ 856 0.0889319
+ 857 0.0889319
+ 858 0.0898799
+ 859 0.0898799
+ 860 0.0898799
+ 861 0.0898799
+ 862 0.0898799
+ 863 0.0898799
+ 870 0.346496
+ 871 0.346496
+ 872 0.346496
+ 873 0.346496
+ 874 0.346496
+ 875 0.346496
+ 876 0.0878627
+ 877 0.0878627
+ 878 0.0878627
+ 879 0.0878627
+ 880 0.0878627
+ 881 0.0878627
+ 882 0.0883173
+ 883 0.0883173
+ 884 0.0883173
+ 885 0.0883173
+ 886 0.0883173
+ 887 0.0883173
+ 894 0.342477
+ 895 0.342477
+ 896 0.342477
+ 897 0.342477
+ 898 0.342477
+ 899 0.342477
+ 900 0.0873327
+ 901 0.0873327
+ 902 0.0873327
+ 903 0.0873327
+ 904 0.0873327
+ 905 0.0873327
+ 906 0.0875221
+ 907 0.0875221
+ 908 0.0875221
+ 909 0.0875221
+ 910 0.0875221
+ 911 0.0875221
+ 918 0.33948
+ 919 0.33948
+ 920 0.33948
+ 921 0.33948
+ 922 0.33948
+ 923 0.33948
+ 924 0.0876242
+ 925 0.0876242
+ 926 0.0876242
+ 927 0.0876242
+ 928 0.0876242
+ 929 0.0876242
+ 930 0.0873905
+ 931 0.0873905
+ 932 0.0873905
+ 933 0.0873905
+ 934 0.0873905
+ 935 0.0873905
+ 942 0.340208
+ 943 0.340208
+ 944 0.340208
+ 945 0.340208
+ 946 0.340208
+ 947 0.340208
+ 948 0.0888228
+ 949 0.0888228
+ 950 0.0888228
+ 951 0.0888228
+ 952 0.0888228
+ 953 0.0888228
+ 954 0.0880492
+ 955 0.0880492
+ 956 0.0880492
+ 957 0.0880492
+ 958 0.0880492
+ 959 0.0880492
+ 966 0.342818
+ 967 0.342818
+ 968 0.342818
+ 969 0.342818
+ 970 0.342818
+ 971 0.342818
+ 972 0.091122
+ 973 0.091122
+ 974 0.091122
+ 975 0.091122
+ 976 0.091122
+ 977 0.091122
+ 978 0.0899847
+ 979 0.0899847
+ 980 0.0899847
+ 981 0.0899847
+ 982 0.0899847
+ 983 0.0899847
+ 984 0.000533675
+ 985 0.000533675
+ 986 0.000533675
+ 987 0.000533675
+ 988 0.000533675
+ 989 0.000533675
+ 990 0.354177
+ 991 0.354177
+ 992 0.354177
+ 993 0.354177
+ 994 0.354177
+ 995 0.354177
+ 996 0.0924905
+ 997 0.0924905
+ 998 0.0924905
+ 999 0.0924905
+ 1000 0.0924905
+ 1001 0.0924905
+ 1002 0.091955
+ 1003 0.091955
+ 1004 0.091955
+ 1005 0.091955
+ 1006 0.091955
+ 1007 0.091955
+ 1008 0.00144082
+ 1009 0.00144082
+ 1010 0.00144082
+ 1011 0.00144082
+ 1012 0.00144082
+ 1013 0.00144082
+ 1014 0.360798
+ 1015 0.360798
+ 1016 0.360798
+ 1017 0.360798
+ 1018 0.360798
+ 1019 0.360798
+ 1020 0.0929747
+ 1021 0.0929747
+ 1022 0.0929747
+ 1023 0.0929747
+ 1024 0.0929747
+ 1025 0.0929747
+ 1026 0.0928345
+ 1027 0.0928345
+ 1028 0.0928345
+ 1029 0.0928345
+ 1030 0.0928345
+ 1031 0.0928345
+ 1032 0.00169879
+ 1033 0.00169879
+ 1034 0.00169879
+ 1035 0.00169879
+ 1036 0.00169879
+ 1037 0.00169879
+ 1038 0.363388
+ 1039 0.363388
+ 1040 0.363388
+ 1041 0.363388
+ 1042 0.363388
+ 1043 0.363388
+ 1044 0.0932003
+ 1045 0.0932003
+ 1046 0.0932003
+ 1047 0.0932003
+ 1048 0.0932003
+ 1049 0.0932003
+ 1050 0.093003
+ 1051 0.093003
+ 1052 0.093003
+ 1053 0.093003
+ 1054 0.093003
+ 1055 0.093003
+ 1056 0.00166017
+ 1057 0.00166017
+ 1058 0.00166017
+ 1059 0.00166017
+ 1060 0.00166017
+ 1061 0.00166017
+ 1062 0.361931
+ 1063 0.361931
+ 1064 0.361931
+ 1065 0.361931
+ 1066 0.361931
+ 1067 0.361931
+ 1068 0.0942117
+ 1069 0.0942117
+ 1070 0.0942117
+ 1071 0.0942117
+ 1072 0.0942117
+ 1073 0.0942117
+ 1074 0.0937487
+ 1075 0.0937487
+ 1076 0.0937487
+ 1077 0.0937487
+ 1078 0.0937487
+ 1079 0.0937487
+ 1080 0.00229049
+ 1081 0.00229049
+ 1082 0.00229049
+ 1083 0.00229049
+ 1084 0.00229049
+ 1085 0.00229049
+ 1086 0.36739
+ 1087 0.36739
+ 1088 0.36739
+ 1089 0.36739
+ 1090 0.36739
+ 1091 0.36739
+ 1092 0.36739
+ 1093 0.0912135
+ 1094 0.0912135
+ 1095 0.0912135
+ 1096 0.0912135
+ 1097 0.0912135
+ 1098 0.0912135
+ 1099 0.0925583
+ 1100 0.0925583
+ 1101 0.0925583
+ 1102 0.0925583
+ 1103 0.0925583
+ 1104 0.0925583
+ 1105 0.00101537
+ 1106 0.00101537
+ 1107 0.00101537
+ 1108 0.00101537
+ 1109 0.00101537
+ 1110 0.00101537
+ 1111 0.358702
+ 1112 0.358702
+ 1113 0.358702
+ 1114 0.358702
+ 1115 0.358702
+ 1116 0.358702
+ 1117 0.0935701
+ 1118 0.0935701
+ 1119 0.0935701
+ 1120 0.0935701
+ 1121 0.0935701
+ 1122 0.0935701
+ 1123 0.0941694
+ 1124 0.0941694
+ 1125 0.0941694
+ 1126 0.0941694
+ 1127 0.0941694
+ 1128 0.0941694
+ 1129 0.00214052
+ 1130 0.00214052
+ 1131 0.00214052
+ 1132 0.00214052
+ 1133 0.00214052
+ 1134 0.00214052
+ 1135 0.367403
+ 1136 0.367403
+ 1137 0.367403
+ 1138 0.367403
+ 1139 0.367403
+ 1140 0.367403
+ 1141 0.344778
+ 1142 0.344778
+ 1143 0.344778
+ 1144 0.344778
+ 1145 0.344778
+ 1146 0.344778
+ 1147 0.341213
+ 1148 0.341213
+ 1149 0.341213
+ 1150 0.341213
+ 1151 0.341213
+ 1152 0.341213
+ 1153 0.338537
+ 1154 0.338537
+ 1155 0.338537
+ 1156 0.338537
+ 1157 0.338537
+ 1158 0.338537
+ 1159 0.340241
+ 1160 0.340241
+ 1161 0.340241
+ 1162 0.340241
+ 1163 0.340241
+ 1164 0.340241
+ 1165 0.344023
+ 1166 0.344023
+ 1167 0.344023
+ 1168 0.344023
+ 1169 0.344023
+ 1170 0.344023
+ 1171 0.355873
+ 1172 0.355873
+ 1173 0.355873
+ 1174 0.355873
+ 1175 0.355873
+ 1176 0.355873
+ 1177 0.360703
+ 1178 0.360703
+ 1179 0.360703
+ 1180 0.360703
+ 1181 0.360703
+ 1182 0.360703
+ 1183 0.362605
+ 1184 0.362605
+ 1185 0.362605
+ 1186 0.362605
+ 1187 0.362605
+ 1188 0.362605
+ 1189 0.361302
+ 1190 0.361302
+ 1191 0.361302
+ 1192 0.361302
+ 1193 0.361302
+ 1194 0.361302
+ 1195 0.369038
+ 1196 0.369038
+ 1197 0.369038
+ 1198 0.369038
+ 1199 0.369038
+ 1200 0.369038
+ 1201 0.355162
+ 1202 0.355162
+ 1203 0.355162
+ 1204 0.355162
+ 1205 0.355162
+ 1206 0.355162
+ 1207 0.366015
+ 1208 0.366015
+ 1209 0.366015
+ 1210 0.366015
+ 1211 0.366015
+ 1212 0.366015
+ 1225 0.34812
+ 1226 0.34812
+ 1227 0.34812
+ 1228 0.34812
+ 1229 0.34812
+ 1230 0.34812
+ 1243 0.342883
+ 1244 0.342883
+ 1245 0.342883
+ 1246 0.342883
+ 1247 0.342883
+ 1248 0.342883
+ 1261 0.339765
+ 1262 0.339765
+ 1263 0.339765
+ 1264 0.339765
+ 1265 0.339765
+ 1266 0.339765
+ 1279 0.33926
+ 1280 0.33926
+ 1281 0.33926
+ 1282 0.33926
+ 1283 0.33926
+ 1284 0.33926
+ 1297 0.341455
+ 1298 0.341455
+ 1299 0.341455
+ 1300 0.341455
+ 1301 0.341455
+ 1302 0.341455
+ 1303 0.000918913
+ 1304 0.000918913
+ 1305 0.000918913
+ 1306 0.000918913
+ 1307 0.000918913
+ 1308 0.000918913
+ 1309 0.000292073
+ 1310 0.000292073
+ 1311 0.000292073
+ 1312 0.000292073
+ 1313 0.000292073
+ 1314 0.000292073
+ 1315 0.350388
+ 1316 0.350388
+ 1317 0.350388
+ 1318 0.350388
+ 1319 0.350388
+ 1320 0.350388
+ 1321 0.00168926
+ 1322 0.00168926
+ 1323 0.00168926
+ 1324 0.00168926
+ 1325 0.00168926
+ 1326 0.00168926
+ 1327 0.00134689
+ 1328 0.00134689
+ 1329 0.00134689
+ 1330 0.00134689
+ 1331 0.00134689
+ 1332 0.00134689
+ 1333 0.359153
+ 1334 0.359153
+ 1335 0.359153
+ 1336 0.359153
+ 1337 0.359153
+ 1338 0.359153
+ 1339 0.00173533
+ 1340 0.00173533
+ 1341 0.00173533
+ 1342 0.00173533
+ 1343 0.00173533
+ 1344 0.00173533
+ 1345 0.00181556
+ 1346 0.00181556
+ 1347 0.00181556
+ 1348 0.00181556
+ 1349 0.00181556
+ 1350 0.00181556
+ 1351 0.362299
+ 1352 0.362299
+ 1353 0.362299
+ 1354 0.362299
+ 1355 0.362299
+ 1356 0.362299
+ 1357 0.00192899
+ 1358 0.00192899
+ 1359 0.00192899
+ 1360 0.00192899
+ 1361 0.00192899
+ 1362 0.00192899
+ 1363 0.00163853
+ 1364 0.00163853
+ 1365 0.00163853
+ 1366 0.00163853
+ 1367 0.00163853
+ 1368 0.00163853
+ 1369 0.361682
+ 1370 0.361682
+ 1371 0.361682
+ 1372 0.361682
+ 1373 0.361682
+ 1374 0.361682
+ 1375 0.00244343
+ 1376 0.00244343
+ 1377 0.00244343
+ 1378 0.00244343
+ 1379 0.00244343
+ 1380 0.00244343
+ 1381 0.00227117
+ 1382 0.00227117
+ 1383 0.00227117
+ 1384 0.00227117
+ 1385 0.00227117
+ 1386 0.00227117
+ 1387 0.365688
+ 1388 0.365688
+ 1389 0.365688
+ 1390 0.365688
+ 1391 0.365688
+ 1392 0.000714292
+ 1393 0.000714292
+ 1394 0.000714292
+ 1395 0.000714292
+ 1396 0.000714292
+ 1397 0.000714292
+ 1398 0.00150722
+ 1399 0.00150722
+ 1400 0.00150722
+ 1401 0.00150722
+ 1402 0.00150722
+ 1403 0.00150722
+ 1404 0.36182
+ 1405 0.36182
+ 1406 0.36182
+ 1407 0.36182
+ 1408 0.36182
+ 1409 0.36182
+ 1410 0.00207305
+ 1411 0.00207305
+ 1412 0.00207305
+ 1413 0.00207305
+ 1414 0.00207305
+ 1415 0.00207305
+ 1416 0.00234353
+ 1417 0.00234353
+ 1418 0.00234353
+ 1419 0.00234353
+ 1420 0.00234353
+ 1421 0.00234353
+ 1422 0.368993
+ 1423 0.368993
+ 1424 0.368993
+ 1425 0.368993
+ 1426 0.368993
+ 1427 0.368993
+ }
+ }
+ SourceGeometry TRUE {
+ osg::Geometry {
+ UniqueID 23
+ PrimitiveSetList 1 {
+ DrawElementsUInt GL_TRIANGLES 0 1428 {
+ 1141 653 12 1147
+ 676 20 1153 693
+ 28 1159 710 36
+ 1165 727 44 1171
+ 744 52 1177 761
+ 60 1183 779 68
+ 1189 796 76 1195
+ 813 84 762 246
+ 268 1201 847 4
+ 1207 830 92 303
+ 168 259 1213 852
+ 116 1219 858 371
+ 1225 448 103 1231
+ 876 141 1237 882
+ 377 1243 471 132
+ 1249 900 162 1255
+ 906 383 1261 488
+ 154 1267 924 186
+ 1273 930 389 1279
+ 506 178 1285 948
+ 208 1291 954 395
+ 1297 523 200 1303
+ 972 232 1309 978
+ 401 1315 540 222
+ 1321 996 253 1327
+ 1002 407 1333 557
+ 247 1339 1020 277
+ 1345 1026 413 1351
+ 574 269 1357 1044
+ 297 1363 1050 419
+ 1369 591 290 1375
+ 1068 322 1381 1074
+ 425 608 314 1086
+ 1392 1093 110 1398
+ 1099 431 1404 641
+ 363 1410 1117 348
+ 1416 1123 437 1422
+ 625 336 853 1142
+ 664 372 870 1143
+ 871 104 654 877
+ 1148 681 378 894
+ 1149 895 133 677
+ 901 1154 698 384
+ 918 1155 919 155
+ 694 925 1160 716
+ 390 942 1161 943
+ 179 711 949 1166
+ 732 396 966 1167
+ 967 201 728 973
+ 1172 750 402 990
+ 1173 991 223 745
+ 997 1178 767 408
+ 1014 1179 1015 248
+ 763 1021 1184 785
+ 414 1038 1185 1039
+ 270 780 1045 1190
+ 802 420 1062 1191
+ 1063 291 797 1069
+ 1196 819 426 1087
+ 1197 1088 315 814
+ 1094 1202 454 432
+ 1111 1203 1112 364
+ 848 1118 1208 835
+ 438 1135 1209 1136
+ 337 831 443 1214
+ 460 96 864 1215
+ 865 373 854 647
+ 1220 866 0 658
+ 1221 659 111 859
+ 860 1226 872 112
+ 455 1227 456 5
+ 449 466 1232 477
+ 122 888 1233 889
+ 379 878 670 1238
+ 890 8 461 1239
+ 462 117 883 884
+ 1244 896 118 665
+ 1245 666 13 472
+ 483 1250 494 147
+ 912 1251 913 385
+ 902 687 1256 914
+ 16 478 1257 479
+ 142 907 908 1262
+ 920 143 682 1263
+ 683 21 489 500
+ 1268 512 169 936
+ 1269 937 391 926
+ 704 1274 938 24
+ 495 1275 496 163
+ 931 932 1280 944
+ 164 699 1281 700
+ 29 507 518 1286
+ 528 192 960 1287
+ 961 397 950 722
+ 1292 962 32 513
+ 1293 514 187 955
+ 956 1298 968 188
+ 717 1299 718 37
+ 524 534 1304 546
+ 214 984 1305 985
+ 403 974 738 1310
+ 986 40 529 1311
+ 530 209 979 980
+ 1316 992 210 733
+ 1317 734 45 541
+ 552 1322 562 238
+ 1008 1323 1009 409
+ 998 756 1328 1010
+ 48 547 1329 548
+ 233 1003 1004 1334
+ 1016 234 751 1335
+ 752 53 558 568
+ 1340 579 260 1032
+ 1341 1033 415 1022
+ 773 1346 1034 56
+ 563 1347 564 254
+ 1027 1028 1352 1040
+ 255 768 1353 769
+ 61 575 585 1358
+ 596 283 1056 1359
+ 1057 421 1046 791
+ 1364 1058 64 580
+ 1365 581 278 1051
+ 1052 1370 1064 279
+ 786 1371 787 69
+ 592 602 1376 614
+ 304 1080 1377 1081
+ 427 1070 808 1382
+ 1082 72 597 1383
+ 598 298 1075 1076
+ 1387 1089 299 803
+ 1388 804 77 609
+ 636 1393 660 354
+ 1105 1394 1106 433
+ 1095 841 1399 1107
+ 88 630 1400 631
+ 349 1100 1101 1405
+ 1113 350 836 1406
+ 837 93 642 620
+ 1411 632 328 1129
+ 1412 1130 439 1119
+ 825 1417 1131 80
+ 615 1418 616 323
+ 1124 1125 1423 1137
+ 324 820 1424 821
+ 85 626 667 1144
+ 14 684 1150 22
+ 701 1156 30 719
+ 1162 38 735 1168
+ 46 753 1174 54
+ 770 1180 62 788
+ 1186 70 805 1192
+ 78 822 1198 86
+ 473 15 655 105
+ 450 365 451 6
+ 849 798 316 610
+ 338 643 832 87
+ 815 627 317 339
+ 816 79 799 611
+ 593 781 292 594
+ 71 782 576 764
+ 271 577 63 765
+ 559 746 249 560
+ 55 747 729 202
+ 542 543 47 730
+ 525 712 203 526
+ 39 713 695 156
+ 508 509 31 696
+ 678 134 490 491
+ 23 679 474 106
+ 135 714 180 204
+ 452 850 366 644
+ 94 833 817 340
+ 628 748 224 250
+ 157 136 341 137
+ 158 492 475 656
+ 107 457 1204 7
+ 159 181 510 783
+ 272 293 182 160
+ 342 318 273 225
+ 343 138 367 800
+ 294 319 295 274
+ 320 251 226 275
+ 205 183 227 321
+ 228 344 229 184
+ 345 206 230 544
+ 139 108 368 346
+ 369 645 838 1210
+ 95 637 1 648
+ 444 671 97 445
+ 9 672 467 688
+ 123 468 17 689
+ 484 705 148 485
+ 25 706 723 193
+ 501 502 33 724
+ 519 739 194 520
+ 41 740 757 239
+ 535 536 49 758
+ 553 774 240 554
+ 57 775 792 284
+ 569 570 65 793
+ 809 305 586 587
+ 73 810 826 329
+ 603 604 81 827
+ 621 842 330 622
+ 89 843 355 638
+ 649 331 306 605
+ 98 124 356 776
+ 261 241 463 1216
+ 119 125 307 357
+ 332 358 308 741
+ 215 195 309 285
+ 588 690 149 126
+ 196 170 503 286
+ 262 571 650 99
+ 359 242 216 537
+ 150 171 127 197
+ 217 172 243 263
+ 218 287 310 264
+ 707 173 151 333
+ 844 360 128 174
+ 311 175 219 265
+ 100 673 129 867
+ 1222 374 873 1228
+ 109 480 1234 144
+ 891 1240 380 897
+ 1246 140 497 1252
+ 165 915 1258 386
+ 921 1264 161 515
+ 1270 189 939 1276
+ 392 945 1282 185
+ 531 1288 211 963
+ 1294 398 969 1300
+ 207 549 1306 235
+ 987 1312 404 993
+ 1318 231 565 1324
+ 256 1011 1330 410
+ 1017 1336 252 582
+ 1342 280 1035 1348
+ 416 1041 1354 276
+ 599 1360 300 1059
+ 1366 422 1065 1372
+ 296 617 1378 325
+ 1083 1384 428 1389
+ 612 1090 661 1395
+ 113 1108 1401 434
+ 1114 1407 370 633
+ 1413 351 1132 1419
+ 440 1138 1425 347
+ 120 855 668 856
+ 375 1145 1146 874
+ 657 145 879 685
+ 880 381 1151 1152
+ 898 680 166 903
+ 702 904 387 1157
+ 1158 922 697 190
+ 927 720 928 393
+ 1163 1164 946 715
+ 212 951 736 952
+ 399 1169 1170 970
+ 731 236 975 754
+ 976 405 1175 1176
+ 994 749 257 999
+ 771 1000 411 1181
+ 1182 1018 766 281
+ 1023 789 1024 417
+ 1187 1188 1042 784
+ 301 1047 806 1048
+ 423 1193 1194 1066
+ 801 326 1071 823
+ 1072 429 1199 1200
+ 1091 818 114 1096
+ 458 1097 435 1205
+ 1206 1115 851 352
+ 1120 839 1121 441
+ 1211 1212 1139 834
+ 10 446 464 447
+ 101 1217 1218 868
+ 857 102 651 869
+ 652 2 1223 1224
+ 662 861 376 862
+ 875 863 115 1229
+ 1230 459 453 18
+ 469 481 470 130
+ 1235 1236 892 881
+ 131 674 893 675
+ 11 1241 1242 465
+ 885 382 886 899
+ 887 121 1247 1248
+ 669 476 26 486
+ 498 487 152 1253
+ 1254 916 905 153
+ 691 917 692 19
+ 1259 1260 482 909
+ 388 910 923 911
+ 146 1265 1266 686
+ 493 34 504 516
+ 505 176 1271 1272
+ 940 929 177 708
+ 941 709 27 1277
+ 1278 499 933 394
+ 934 947 935 167
+ 1283 1284 703 511
+ 42 521 532 522
+ 198 1289 1290 964
+ 953 199 725 965
+ 726 35 1295 1296
+ 517 957 400 958
+ 971 959 191 1301
+ 1302 721 527 50
+ 538 550 539 220
+ 1307 1308 988 977
+ 221 742 989 743
+ 43 1313 1314 533
+ 981 406 982 995
+ 983 213 1319 1320
+ 737 545 58 555
+ 566 556 244 1325
+ 1326 1012 1001 245
+ 759 1013 760 51
+ 1331 1332 551 1005
+ 412 1006 1019 1007
+ 237 1337 1338 755
+ 561 66 572 583
+ 573 266 1343 1344
+ 1036 1025 267 777
+ 1037 778 59 1349
+ 1350 567 1029 418
+ 1030 1043 1031 258
+ 1355 1356 772 578
+ 74 589 600 590
+ 288 1361 1362 1060
+ 1049 289 794 1061
+ 795 67 1367 1368
+ 584 1053 424 1054
+ 1067 1055 282 1373
+ 1374 790 595 82
+ 606 618 607 312
+ 1379 1380 1084 1073
+ 313 811 1085 812
+ 75 1385 1386 601
+ 1077 430 1078 1092
+ 1079 302 1390 1391
+ 807 613 3 639
+ 663 640 361 1396
+ 1397 1109 1098 362
+ 845 1110 846 90
+ 1402 1403 634 1102
+ 436 1103 1116 1104
+ 353 1408 1409 840
+ 646 91 623 635
+ 624 334 1414 1415
+ 1133 1122 335 828
+ 1134 829 83 1420
+ 1421 619 1126 442
+ 1127 1140 1128 327
+ 1426 1427 824 629
+ }
+
+ }
+ VertexData {
+ Array TRUE ArrayID 3 Vec3fArray 1428 {
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 0.0499999
+ 0 0.0499999 0.0499999
+ 0 0.0499999 0.0499999
+ 0 0.0499999 0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 0.0499999
+ 0.0499999 0 0.0499999
+ 0.0499999 0 0.0499999
+ 0.0499999 0 0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 0.0499999
+ 0 -0.0499999 0.0499999
+ 0 -0.0499999 0.0499999
+ 0 -0.0499999 0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 0.0499999
+ -0.0499999 0 0.0499999
+ -0.0499999 0 0.0499999
+ -0.0499999 0 0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 0.0499999
+ -0.025 0.0433012 0.0499999
+ -0.025 0.0433012 0.0499999
+ -0.025 0.0433012 0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ }
+ Indices FALSE
+ Binding BIND_PER_VERTEX
+ Normalize 0
+ }
+ NormalData {
+ Array TRUE ArrayID 4 Vec3fArray 1428 {
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ -0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ -0.707107 -0.707107 0
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ -0.965926 -0.25882 0
+ 0 0 -1
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ 0 0 1
+ -0.965926 0.258819 0
+ 0 0 -1
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ 0 0 1
+ -0.707107 0.707106 0
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ 0 0 1
+ -0.258819 0.965926 0
+ 0 0 -1
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ 0 0 1
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ 0 0 -1
+ -0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ 0 0 1
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ 0 0 -1
+ -0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ 0 0 1
+ 0 0 1
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 -1
+ 0 0 -1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 1
+ 0 0 1
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0 0 1
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ -0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 -1
+ 0 0 -1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 1
+ 0 0 1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ 0 0 1
+ 0 0 1
+ -0.25882 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ }
+ Indices FALSE
+ Binding BIND_PER_VERTEX
+ Normalize 0
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ Matrix {
+ 0.1 0 0 0
+ 0 0.1 0 0
+ 0 0 0.1 0
+ 0 0 -0.05 1
+ }
+
+ }
+ }
+}
diff --git a/SurgSim/Graphics/RenderTests/ImplicitSurfaceRenderTests.cpp b/SurgSim/Graphics/RenderTests/ImplicitSurfaceRenderTests.cpp
new file mode 100644
index 0000000..4e0d2e9
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/ImplicitSurfaceRenderTests.cpp
@@ -0,0 +1,350 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Blocks/ImplicitSurface.h"
+#include "SurgSim/Blocks/PoseInterpolator.h"
+#include "SurgSim/Blocks/ShadowMapping.h"
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Graphics/OsgAxesRepresentation.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/Camera.h"
+#include "SurgSim/Graphics/OsgLight.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Graphics/OsgPlaneRepresentation.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+#include "SurgSim/Graphics/OsgView.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRigidTranslation;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+class ImplicitSurfaceRenderTests : public RenderTest
+{
+
+};
+
+TEST_F(ImplicitSurfaceRenderTests, PointSpriteFluid)
+{
+ Math::Vector4f diffuseColor(0.83f, 0.0f, 0.0f, 1.0f);
+ Math::Vector4f specularColor(0.8f, 0.8f, 0.8f, 1.0f);
+
+ std::array<int, 2> dimensions = {1280, 720};
+ viewElement->getView()->setDimensions(dimensions);
+ viewElement->getCamera()->setPerspectiveProjection(45, 1.7, 0.01, 10.0);
+ viewElement->getCamera()->setAmbientColor(Math::Vector4d(0.2, 0.2, 0.2, 1.0));
+
+
+ auto interpolator = std::make_shared<Blocks::PoseInterpolator>("Interpolator");
+ RigidTransform3d from = makeRigidTransform(
+ Vector3d(0.5, 0.0, -0.5),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 1.0, 0.0));
+ RigidTransform3d to = makeRigidTransform(
+ Vector3d(-0.5, 0.0, -0.5),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 1.0, 0.0));
+ interpolator->setTarget(viewElement);
+ interpolator->setStartingPose(from);
+ interpolator->setDuration(5.0);
+ interpolator->setEndingPose(to);
+ interpolator->setPingPong(true);
+
+ viewElement->setPose(from);
+ viewElement->addComponent(interpolator);
+
+ auto light = std::make_shared<Graphics::OsgLight>("Light");
+ light->setDiffuseColor(Math::Vector4d(1.0, 1.0, 1.0, 1.0));
+ light->setSpecularColor(Math::Vector4d(0.8, 0.8, 0.8, 1.0));
+ light->setLightGroupReference(SurgSim::Graphics::Representation::DefaultGroupName);
+
+ auto lightElement = std::make_shared<Framework::BasicSceneElement>("LightElement");
+ lightElement->setPose(makeRigidTransform(
+ Math::Vector3d(-0.3 , 0.1, -0.5),
+ Math::Vector3d(0.0 ,0.0, 0.0),
+ Math::Vector3d(0.0, 1.0, 0.0)));
+ lightElement->addComponent(light);
+ scene->addSceneElement(lightElement);
+
+ auto axes = std::make_shared<Graphics::OsgAxesRepresentation>("Axes");
+ lightElement->addComponent(axes);
+
+ std::array<double, 6> lightProjection = { -1.0, 1.0, -1.0, 1.0, -1.0, 1.0 };
+ scene->addSceneElements(Blocks::createShadowMapping(viewElement->getCamera(), light,
+ 4096, 1024, lightProjection, 0.002, 0.75, true, 4.0, false));
+
+ scene->addSceneElements(Blocks::createImplicitSurfaceEffect(viewElement->getView(), light,
+ scene->getSceneElement(Blocks::GROUP_SHADOW_CASTER),
+ 0.01f, 800.0f, 4.0,
+ diffuseColor, specularColor,
+ "Textures/CubeMap_reflection_diffuse.png", 0.9,
+ "Textures/CubeMap_reflection_specular.png", 0.1,
+ 100.0f, 0.0005f, 0.75f, false));
+
+ auto cube = std::make_shared<Graphics::OsgBoxRepresentation>("Graphics");
+ cube->setSizeXYZ(0.1, 0.1, 0.1);
+ cube->addGroupReference(Blocks::GROUP_SHADOW_RECEIVER);
+
+ auto material = Graphics::buildMaterial("Shaders/s_mapping_material.vert", "Shaders/s_mapping_material.frag");
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", Math::Vector4f(0.2, 0.2, 0.2, 1.0));
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", Math::Vector4f(1.0, 1.0, 1.0, 1.0));
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", 10.0f);
+ cube->setMaterial(material);
+
+ auto element = std::make_shared<Framework::BasicSceneElement>("Cube");
+ element->setPose(makeRigidTranslation(Math::Vector3d(0.0, 0.0, 0.25)));
+ element->addComponent(cube);
+ element->addComponent(material);
+
+ scene->addSceneElement(element);
+
+ auto sphere = std::make_shared<Graphics::OsgSphereRepresentation>("Graphics");
+ sphere->setRadius(0.1);
+ sphere->addGroupReference(Blocks::GROUP_SHADOW_CASTER);
+ sphere->addGroupReference(Blocks::GROUP_SHADOW_RECEIVER);
+
+ material = Graphics::buildMaterial("Shaders/s_mapping_material.vert", "Shaders/s_mapping_material.frag");
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", diffuseColor);
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", specularColor);
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", 10.0f);
+ sphere->setMaterial(material);
+
+ element = std::make_shared<Framework::BasicSceneElement>("Sphere");
+ element->setPose(makeRigidTranslation(Math::Vector3d(-0.25, 0.0, -0.25)));
+ element->addComponent(sphere);
+ element->addComponent(material);
+
+ scene->addSceneElement(element);
+
+ auto plane = std::make_shared<Graphics::OsgPlaneRepresentation>("Graphics");
+ plane->addGroupReference(Blocks::GROUP_SHADOW_RECEIVER);
+
+ material = Graphics::buildMaterial("Shaders/s_mapping_material.vert", "Shaders/s_mapping_material.frag");
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", Math::Vector4f(0.2, 0.2, 0.2, 1.0));
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", Math::Vector4f(1.0, 1.0, 1.0, 1.0));
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", 10.0f);
+ plane->setMaterial(material);
+
+ element = std::make_shared<Framework::BasicSceneElement>("Plane");
+ element->setPose(makeRigidTranslation(Math::Vector3d(0.0, -0.1, 0.0)));
+ element->addComponent(plane);
+ element->addComponent(material);
+
+ scene->addSceneElement(element);
+
+ // Create the point cloud
+ auto mesh = std::make_shared<Graphics::OsgMeshRepresentation>("Mesh");
+ mesh->loadMesh("Geometry/sphere.ply");
+
+ auto graphics = std::make_shared<Graphics::OsgPointCloudRepresentation>("Cloud");
+
+ for (const auto& vertex : mesh->getMesh()->getVertices())
+ {
+ graphics->getVertices()->addVertex(Graphics::PointCloud::VertexType(vertex));
+ }
+
+ graphics->setGroupReference(Blocks::GROUP_IMPLICIT_SURFACE);
+
+ auto sceneElement = std::make_shared<Framework::BasicSceneElement>("PointSprites");
+ sceneElement->addComponent(graphics);
+
+ scene->addSceneElement(sceneElement);
+
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(5000));
+ runtime->stop();
+}
+
+TEST_F(ImplicitSurfaceRenderTests, StereoFluid)
+{
+ Math::Vector4f diffuseColor(0.83f, 0.0f, 0.0f, 1.0f);
+ Math::Vector4f specularColor(0.8f, 0.8f, 0.8f, 1.0f);
+
+ std::array<int, 2> dimensions = {1280, 720};
+ viewElement->getView()->setDimensions(dimensions);
+ viewElement->getCamera()->setPerspectiveProjection(45, 1.7, 0.01, 10.0);
+ viewElement->getCamera()->setAmbientColor(Math::Vector4d(0.2, 0.2, 0.2, 1.0));
+
+ auto osgView = std::static_pointer_cast<OsgView>(viewElement->getView());
+ osgView->setStereoMode(View::STEREO_MODE_HORIZONTAL_SPLIT);
+ osgView->setDisplayType(View::DISPLAY_TYPE_MONITOR);
+ osgView->setScreenWidth(0.0631);
+ osgView->setScreenHeight(0.071);
+ osgView->setEyeSeparation(0.06);
+ osgView->setScreenDistance(0.10);
+
+ auto interpolator = std::make_shared<Blocks::PoseInterpolator>("Interpolator");
+ RigidTransform3d from = makeRigidTransform(
+ Vector3d(2.0, 0.0, -2.0),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 1.0, 0.0));
+ RigidTransform3d to = makeRigidTransform(
+ Vector3d(-2.0, 0.0, -2.0),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 1.0, 0.0));
+ interpolator->setTarget(viewElement);
+ interpolator->setStartingPose(from);
+ interpolator->setDuration(5.0);
+ interpolator->setEndingPose(to);
+ interpolator->setPingPong(true);
+
+ viewElement->setPose(from);
+ viewElement->addComponent(interpolator);
+
+ auto light = std::make_shared<Graphics::OsgLight>("Light");
+ light->setDiffuseColor(Math::Vector4d(1.0, 1.0, 1.0, 1.0));
+ light->setSpecularColor(Math::Vector4d(0.8, 0.8, 0.8, 1.0));
+ light->setLightGroupReference(SurgSim::Graphics::Representation::DefaultGroupName);
+
+ auto lightElement = std::make_shared<Framework::BasicSceneElement>("LightElement");
+ lightElement->setPose(makeRigidTransform(
+ Math::Vector3d(-0.3 , 0.1, -0.5),
+ Math::Vector3d(0.0 ,0.0, 0.0),
+ Math::Vector3d(0.0, 1.0, 0.0)));
+ lightElement->addComponent(light);
+ scene->addSceneElement(lightElement);
+
+ auto axes = std::make_shared<Graphics::OsgAxesRepresentation>("Axes");
+ lightElement->addComponent(axes);
+
+ std::array<double, 6> lightProjection = { -1.0, 1.0, -1.0, 1.0, -1.0, 1.0 };
+ scene->addSceneElements(Blocks::createShadowMapping(viewElement->getCamera(), light,
+ 4096, 1024, lightProjection, 0.002, 0.75, true, 4.0, false));
+
+ scene->addSceneElements(Blocks::createImplicitSurfaceEffect(viewElement->getView(), light,
+ scene->getSceneElement(Blocks::GROUP_SHADOW_CASTER),
+ 0.02f, 800.0f, 4.0,
+ diffuseColor, specularColor,
+ "Textures/CubeMap_reflection_diffuse.png", 0.9,
+ "Textures/CubeMap_reflection_specular.png", 0.1,
+ 100.0f, 0.0005f, 0.75f, false));
+
+ auto cube = std::make_shared<Graphics::OsgBoxRepresentation>("Graphics");
+ cube->setSizeXYZ(0.1, 0.1, 0.1);
+ cube->addGroupReference(Blocks::GROUP_SHADOW_RECEIVER);
+
+ auto material = Graphics::buildMaterial("Shaders/s_mapping_material.vert", "Shaders/s_mapping_material.frag");
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", Math::Vector4f(0.2, 0.2, 0.2, 1.0));
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", Math::Vector4f(1.0, 1.0, 1.0, 1.0));
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", 10.0f);
+ cube->setMaterial(material);
+
+ auto element = std::make_shared<Framework::BasicSceneElement>("Cube");
+ element->setPose(makeRigidTranslation(Math::Vector3d(0.0, 0.0, 0.25)));
+ element->addComponent(cube);
+ element->addComponent(material);
+
+ scene->addSceneElement(element);
+
+ auto sphere = std::make_shared<Graphics::OsgSphereRepresentation>("Graphics");
+ sphere->setRadius(0.1);
+ sphere->addGroupReference(Blocks::GROUP_SHADOW_CASTER);
+ sphere->addGroupReference(Blocks::GROUP_SHADOW_RECEIVER);
+
+ material = Graphics::buildMaterial("Shaders/s_mapping_material.vert", "Shaders/s_mapping_material.frag");
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", diffuseColor);
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", specularColor);
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", 10.0f);
+ sphere->setMaterial(material);
+
+ element = std::make_shared<Framework::BasicSceneElement>("Sphere");
+ element->setPose(makeRigidTranslation(Math::Vector3d(-0.25, 0.0, -0.25)));
+ element->addComponent(sphere);
+ element->addComponent(material);
+
+ scene->addSceneElement(element);
+
+ auto plane = std::make_shared<Graphics::OsgPlaneRepresentation>("Graphics");
+ plane->addGroupReference(Blocks::GROUP_SHADOW_RECEIVER);
+
+ material = Graphics::buildMaterial("Shaders/s_mapping_material.vert", "Shaders/s_mapping_material.frag");
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", Math::Vector4f(0.2, 0.2, 0.2, 1.0));
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", Math::Vector4f(1.0, 1.0, 1.0, 1.0));
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", 10.0f);
+ plane->setMaterial(material);
+
+ element = std::make_shared<Framework::BasicSceneElement>("Plane");
+ element->setPose(makeRigidTranslation(Math::Vector3d(0.0, -0.1, 0.0)));
+ element->addComponent(plane);
+ element->addComponent(material);
+
+ scene->addSceneElement(element);
+
+ // Create the point cloud
+ auto mesh = std::make_shared<Graphics::OsgMeshRepresentation>("Mesh");
+ mesh->loadMesh("Geometry/sphere.ply");
+
+ auto graphics = std::make_shared<Graphics::OsgPointCloudRepresentation>("Cloud");
+
+ for (const auto& vertex : mesh->getMesh()->getVertices())
+ {
+ graphics->getVertices()->addVertex(Graphics::PointCloud::VertexType(vertex));
+ }
+
+ graphics->setGroupReference(Blocks::GROUP_IMPLICIT_SURFACE);
+
+ auto sceneElement = std::make_shared<Framework::BasicSceneElement>("PointSprites");
+ sceneElement->addComponent(graphics);
+
+ scene->addSceneElement(sceneElement);
+
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(5000));
+ runtime->stop();
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgCameraRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgCameraRenderTests.cpp
index d9dfb17..e035ded 100644
--- a/SurgSim/Graphics/RenderTests/OsgCameraRenderTests.cpp
+++ b/SurgSim/Graphics/RenderTests/OsgCameraRenderTests.cpp
@@ -17,6 +17,7 @@
#include <gtest/gtest.h>
#include "SurgSim/Blocks/SphereElement.h"
#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/TransferPropertiesBehavior.h"
#include "SurgSim/Graphics/OsgManager.h"
#include "SurgSim/Graphics/OsgCamera.h"
#include "SurgSim/Graphics/OsgScreenSpaceQuadRepresentation.h"
@@ -24,7 +25,7 @@
#include "SurgSim/Graphics/OsgBoxRepresentation.h"
#include "SurgSim/Graphics/OsgGroup.h"
#include "SurgSim/Graphics/OsgMaterial.h"
-#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgProgram.h"
#include "SurgSim/Graphics/OsgUniform.h"
#include "SurgSim/Math/Quaternion.h"
@@ -85,15 +86,15 @@ TEST_F(OsgCameraRenderTests, PassTest)
renderPass->setRenderTarget(renderTargetOsg);
renderPass->setRenderOrder(Camera::RENDER_ORDER_PRE_RENDER, 0);
- auto shader = std::make_shared<OsgShader>();
- shader->setFragmentShaderSource(fragmentShaderSource);
- shader->setVertexShaderSource(vertexShaderSource);
+ auto program = std::make_shared<OsgProgram>();
+ program->setFragmentShaderSource(fragmentShaderSource);
+ program->setVertexShaderSource(vertexShaderSource);
auto material1 = std::make_shared<OsgMaterial>("material1");
auto material2 = std::make_shared<OsgMaterial>("material2");
- material1->setShader(shader);
- material2->setShader(shader);
+ material1->setProgram(program);
+ material2->setProgram(program);
viewElement->addComponent(material1);
viewElement->addComponent(material2);
@@ -172,6 +173,83 @@ TEST_F(OsgCameraRenderTests, PassTest)
}
}
+TEST_F(OsgCameraRenderTests, Resize)
+{
+ int width = 1024;
+ int height = 768;
+
+ auto copier = std::make_shared<Framework::TransferPropertiesBehavior>("Copier");
+ copier->connect(viewElement->getView(), "DimensionsDouble", viewElement->getCamera(), "ViewportSize");
+
+ auto graphics = std::make_shared<Graphics::OsgBoxRepresentation>("Graphics");
+ graphics->setSizeXYZ(0.1, 0.1, 0.1);
+
+ auto element = std::make_shared<Framework::BasicSceneElement>("Cube");
+ element->addComponent(graphics);
+ element->setPose(Math::makeRigidTranslation(Vector3d(0.0, 0.0, -0.3)));
+
+ scene->addSceneElement(element);
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+ std::array<int, 2> dimensions = {width + 500, height + 100};
+ viewElement->getView()->setDimensions(dimensions);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+ std::array<int, 2> swapped = {dimensions[1], dimensions[0]};
+ viewElement->getView()->setDimensions(swapped);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+}
+
+TEST_F(OsgCameraRenderTests, MultipleRenderGroups)
+{
+ std::array<int, 2> dimensions = { 400, 400 };
+ viewElement->getView()->setDimensions(dimensions);
+ camera->setRenderGroupReference("Left");
+
+ std::array<int, 2> position = { 500, 100 };
+ auto rightElement = std::make_shared<Graphics::OsgViewElement>("RighViewElement");
+ rightElement->getView()->setDimensions(dimensions);
+ rightElement->getView()->setPosition(position);
+ rightElement->getCamera()->addRenderGroupReference("Right");
+
+ scene->addSceneElement(rightElement);
+
+ // Left camera only
+ auto graphics = std::make_shared<Graphics::OsgBoxRepresentation>("Graphics");
+ graphics->setSizeXYZ(0.1, 0.1, 0.1);
+ graphics->setGroupReference("Left");
+
+ auto element = std::make_shared<Framework::BasicSceneElement>("CubeLeft");
+ element->addComponent(graphics);
+ element->setPose(Math::makeRigidTranslation(Vector3d(-0.2, 0.0, -0.5)));
+
+ scene->addSceneElement(element);
+
+ // Right camera only
+ graphics = std::make_shared<Graphics::OsgBoxRepresentation>("Graphics");
+ graphics->setSizeXYZ(0.1, 0.1, 0.1);
+ graphics->setGroupReference("Right");
+
+ element = std::make_shared<Framework::BasicSceneElement>("CubeRight");
+ element->addComponent(graphics);
+ element->setPose(Math::makeRigidTranslation(Vector3d(0.2, 0.0, -0.5)));
+
+ scene->addSceneElement(element);
+
+ // Both cameras
+ graphics = std::make_shared<Graphics::OsgBoxRepresentation>("Graphics");
+ graphics->setSizeXYZ(0.1, 0.1, 0.1);
+
+ element = std::make_shared<Framework::BasicSceneElement>("CubeBoth");
+ element->addComponent(graphics);
+ element->setPose(Math::makeRigidTranslation(Vector3d(0.0, 0.0, -0.5)));
+
+ scene->addSceneElement(element);
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
+}
+
}; // namespace Graphics
}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgCurveRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgCurveRepresentationRenderTests.cpp
new file mode 100644
index 0000000..daf5ce5
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgCurveRepresentationRenderTests.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 <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/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgLight.h"
+#include "SurgSim/Graphics/OsgCurveRepresentation.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 <osg/LineWidth>
+#include <osg/Hint>
+
+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;
+using SurgSim::Testing::interpolatePose;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+struct OsgCurveRepresentationRenderTests : public RenderTest
+{
+
+
+protected:
+ DataStructures::VerticesPlain makeCube()
+ {
+ typedef DataStructures::VerticesPlain::VertexType Vertex;
+ DataStructures::VerticesPlain result;
+ result.addVertex(Vertex(Vector3d(0.1, -0.1, 0.1)));
+ result.addVertex(Vertex(Vector3d(0.1, -0.1, 0.1)));
+ result.addVertex(Vertex(Vector3d(-0.1, -0.1, 0.1)));
+ result.addVertex(Vertex(Vector3d(-0.1, -0.1, -0.1)));
+ result.addVertex(Vertex(Vector3d(0.1, -0.1, -0.1)));
+
+ result.addVertex(Vertex(Vector3d(0.1, 0.1, 0.1)));
+ result.addVertex(Vertex(Vector3d(-0.1, 0.1, 0.1)));
+ result.addVertex(Vertex(Vector3d(-0.1, 0.1, -0.1)));
+ result.addVertex(Vertex(Vector3d(0.1, 0.1, -0.1)));
+ return result;
+ }
+};
+
+TEST_F(OsgCurveRepresentationRenderTests, DynamicRotate)
+{
+ auto element = std::make_shared<Framework::BasicSceneElement>("Element");
+ auto vertices = makeCube();
+ auto representation = std::make_shared<OsgCurveRepresentation>("Curve");
+ element->addComponent(representation);
+ scene->addSceneElement(element);
+ viewElement->enableManipulator(true);
+ viewElement->setPose(makeRigidTransform(
+ Math::Vector3d(0.0, 0.0, -2.0),
+ Math::Vector3d(0.0, 0.0, 0.0),
+ Math::Vector3d(0.0, 1.0, 0.0)));
+
+ auto material = buildMaterial("Shaders/material_curve.vert", "Shaders/material.frag");
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", Vector4f(0.2, 0.2, 0.9, 1.0));
+
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", Vector4f(0.4, 0.4, 0.4, 1.0));
+
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", 64.0f);
+
+ representation->setMaterial(material);
+ element->addComponent(material);
+
+ auto light = std::make_shared<Graphics::OsgLight>("Light");
+ // light->setLocalPose(Math::makeRigidTranslation(Math::Vector3d(0.5, 0.5, 0.5)));
+ viewElement->addComponent(light);
+
+ /// Run the thread
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ EXPECT_TRUE(viewElement->isInitialized());
+ int numSteps = 10 * 60;
+
+ auto widths = std::make_pair(0.1, 5.0);
+ auto tensions = std::make_pair(0.1, 0.9);
+
+ RigidTransform3d transform = makeRigidTransform(makeRotationQuaternion(0.02, Vector3d(0.0, 1.0, 1.0)),
+ Vector3d(0.0, -0.0, 0.0));
+
+ for (int i = 0; i < numSteps; ++i)
+ {
+
+ double t = static_cast<double>(i) / numSteps;
+ for (size_t i = 0; i < vertices.getNumVertices(); ++i)
+ {
+ vertices.setVertexPosition(i, transform * vertices.getVertexPosition(i));
+ }
+
+
+ representation->setWidth(interpolate(widths.first, widths.second, t));
+ representation->setTension(interpolate(tensions.first, tensions.second, t));
+ representation->updateControlPoints(vertices);
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000.0 / 30.0));
+ }
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+
+}
+
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgMeshRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgMeshRepresentationRenderTests.cpp
index cb98c47..7f8333d 100644
--- a/SurgSim/Graphics/RenderTests/OsgMeshRepresentationRenderTests.cpp
+++ b/SurgSim/Graphics/RenderTests/OsgMeshRepresentationRenderTests.cpp
@@ -29,7 +29,7 @@
#include "SurgSim/Graphics/OsgManager.h"
#include "SurgSim/Graphics/OsgMaterial.h"
#include "SurgSim/Graphics/OsgMeshRepresentation.h"
-#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgProgram.h"
#include "SurgSim/Graphics/OsgUniform.h"
#include "SurgSim/Graphics/OsgViewElement.h"
#include "SurgSim/Graphics/RenderTests/RenderTest.h"
@@ -71,16 +71,6 @@ protected:
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)
@@ -88,6 +78,12 @@ TEST_F(OsgMeshRepresentationRenderTests, BasicCubeTest)
auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("Scene");
scene->addSceneElement(element);
+ // make an empty mesh
+ auto meshRepresentation0 = makeRepresentation("emptymesh");
+ meshRepresentation0->getMesh()->initialize(cubeVertices, cubeColors, std::vector<Vector2d>(), cubeTriangles);
+ meshRepresentation0->setUpdateOptions(MeshRepresentation::UPDATE_OPTION_COLORS |
+ MeshRepresentation::UPDATE_OPTION_VERTICES);
+
SurgSim::Testing::Cube::makeCube(&cubeVertices, &cubeColors, &cubeTextures, &cubeTriangles);
// make a colored cube
@@ -102,7 +98,7 @@ TEST_F(OsgMeshRepresentationRenderTests, BasicCubeTest)
auto material = std::make_shared<OsgMaterial>("material");
auto texture = std::make_shared<OsgTexture2d>();
- texture->loadImage(applicationData->findFile("OsgMeshRepresentationRenderTests/cube.png"));
+ texture->loadImage(applicationData->findFile("Textures/CubeMap_rgb_rotate.png"));
auto uniform2d = std::make_shared<OsgUniform<std::shared_ptr<SurgSim::Graphics::OsgTexture2d>>>("oss_diffuseMap");
uniform2d->set(texture);
@@ -110,6 +106,7 @@ TEST_F(OsgMeshRepresentationRenderTests, BasicCubeTest)
meshRepresentation2->setMaterial(material);
element->addComponent(material);
+ element->addComponent(meshRepresentation0);
element->addComponent(meshRepresentation1);
element->addComponent(meshRepresentation2);
@@ -144,14 +141,14 @@ TEST_F(OsgMeshRepresentationRenderTests, BasicCubeTest)
std::vector<std::shared_ptr<Mesh>> meshes;
meshes.push_back(meshRepresentation1->getMesh());
- meshes.push_back(meshRepresentation2->getMesh());
+ meshes.push_back(std::make_shared<Graphics::Mesh>(*meshRepresentation2->getMesh()));
/// Run the thread
runtime->start();
EXPECT_TRUE(graphicsManager->isInitialized());
EXPECT_TRUE(viewElement->isInitialized());
- boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
int numSteps = 1000;
@@ -172,41 +169,43 @@ TEST_F(OsgMeshRepresentationRenderTests, BasicCubeTest)
newVertices[index] = transform * (cubeVertices[index] * scale);
}
meshes[j]->setVertexPositions(newVertices, true);
+ if (j == 0)
+ {
+ // Not threadsafe update
+ meshes[0]->dirty();
+ }
+ else
+ {
+ // threadsafe update
+ meshRepresentation2->updateMesh(*meshes[1]);
+ }
}
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);
+ /// The total number of steps should complete in 4 seconds
+ boost::this_thread::sleep(boost::posix_time::milliseconds(4000 / numSteps));
}
}
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));
-
+ graphics->loadMesh("Geometry/wound_deformable_with_texture.ply");
// Create material to transport the Textures
auto material = std::make_shared<SurgSim::Graphics::OsgMaterial>("material");
auto texture = std::make_shared<SurgSim::Graphics::OsgTexture2d>();
+
+ std::string textureFilename;
+ ASSERT_TRUE(runtime->getApplicationData()->tryFindFile("Textures/checkered.png", &textureFilename));
texture->loadImage(textureFilename);
auto diffuseMapUniform =
std::make_shared<SurgSim::Graphics::OsgTextureUniform<SurgSim::Graphics::OsgTexture2d>>("diffuseMap");
@@ -232,26 +231,20 @@ TEST_F(OsgMeshRepresentationRenderTests, TextureTest)
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",
+ ASSERT_TRUE(runtime->getApplicationData()->tryFindFile("Textures/wound_deformable.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));
-
+ graphics->loadMesh("Geometry/wound_deformable_with_texture.ply");
// 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 program = SurgSim::Graphics::loadProgram(*runtime->getApplicationData(), "Shaders/ds_mapping_material");
+ ASSERT_TRUE(program != nullptr);
+ material->setProgram(program);
{
auto uniform = std::make_shared<SurgSim::Graphics::OsgUniform<Vector4f>>("diffuseColor");
material->addUniform(uniform);
@@ -385,6 +378,7 @@ TEST_F(OsgMeshRepresentationRenderTests, TriangleDeletionTest)
if (triangle > 0)
{
meshRepresentation1->getMesh()->removeTriangle(triangle);
+ meshRepresentation1->getMesh()->dirty();
}
}
diff --git a/SurgSim/Graphics/RenderTests/OsgOctreeRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgOctreeRepresentationRenderTests.cpp
index ba3ef05..4491ab7 100644
--- a/SurgSim/Graphics/RenderTests/OsgOctreeRepresentationRenderTests.cpp
+++ b/SurgSim/Graphics/RenderTests/OsgOctreeRepresentationRenderTests.cpp
@@ -67,14 +67,14 @@ TEST_F(OsgOctreeRepresentationRenderTests, OctreeSubdivide)
}
};
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);
+ octree->addData(secondLevelPositions[0], 2, emptyData);
+ octree->addData(secondLevelPositions[1], 2, emptyData);
+ octree->addData(secondLevelPositions[3], 2, emptyData);
+ octree->addData(secondLevelPositions[7], 2, emptyData);
+ octree->addData(Vector3d(0.2, 0.2, 0.01), 3, emptyData);
+ octree->addData(Vector3d(-0.2, -0.2, 0.2), 3, emptyData);
+ octree->addData(Vector3d(0.01, 0.01, 0.11), 3, emptyData);
+ octree->addData(Vector3d(0.01, 0.01, 0.11), 4, emptyData);
auto octreeShape = std::make_shared<SurgSim::Math::OctreeShape>(*octree);
diff --git a/SurgSim/Graphics/RenderTests/OsgPointCloudRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgPointCloudRepresentationRenderTests.cpp
index d207e6d..d990065 100644
--- a/SurgSim/Graphics/RenderTests/OsgPointCloudRepresentationRenderTests.cpp
+++ b/SurgSim/Graphics/RenderTests/OsgPointCloudRepresentationRenderTests.cpp
@@ -24,6 +24,7 @@
#include "SurgSim/Framework/Scene.h"
#include "SurgSim/Graphics/OsgBoxRepresentation.h"
#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
#include "SurgSim/Graphics/OsgViewElement.h"
#include "SurgSim/Graphics/PointCloudRepresentation.h"
@@ -201,5 +202,43 @@ TEST_F(OsgPointCloudRepresentationRenderTests, PointSizeAndColor)
boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps));
}
}
+
+TEST_F(OsgPointCloudRepresentationRenderTests, PointSprite)
+{
+ std::vector<Vector3d> vertices = makeCube();
+ auto graphics = std::make_shared<SurgSim::Graphics::OsgPointCloudRepresentation>("Cloud");
+ graphics->setPointSize(10.0f);
+
+ graphics->setLocalPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, 0.0, -0.2)));
+ for (auto vertex : vertices)
+ {
+ graphics->getVertices()->addVertex(SurgSim::Graphics::PointCloud::VertexType(vertex));
+ }
+
+ // Create material to transport the Textures
+ auto material = std::make_shared<SurgSim::Graphics::OsgMaterial>("material");
+ auto texture = std::make_shared<SurgSim::Graphics::OsgTexture2d>();
+ texture->setIsPointSprite(true);
+
+ std::string textureFilename;
+ ASSERT_TRUE(runtime->getApplicationData()->tryFindFile("Textures/checkered.png", &textureFilename));
+ 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>("Blood");
+ sceneElement->addComponent(graphics);
+ sceneElement->addComponent(material);
+
+ scene->addSceneElement(sceneElement);
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+ runtime->stop();
+}
+
}; // namespace Graphics
}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgProgramRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgProgramRenderTests.cpp
new file mode 100644
index 0000000..4fdb078
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgProgramRenderTests.cpp
@@ -0,0 +1,452 @@
+// 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/Blocks/GraphicsUtilities.h"
+#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/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgLight.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Graphics/OsgProgram.h"
+#include "SurgSim/Graphics/OsgRenderTarget.h"
+#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+#include "SurgSim/Graphics/OsgUniform.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/Mesh.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>
+#include <array>
+
+
+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<Program> loadExampleProgram(const SurgSim::Framework::ApplicationData& data)
+{
+ std::shared_ptr<Program> program = std::make_shared<OsgProgram>();
+
+ std::string vertexShaderPath = data.findFile("OsgShaderRenderTests/shader.vert");
+ std::string geometryShaderPath = data.findFile("OsgShaderRenderTests/shader_axis_mirrored.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!";
+
+ program->loadVertexShader(vertexShaderPath);
+ program->loadGeometryShader(geometryShaderPath);
+ program->loadFragmentShader(fragmentShaderPath);
+
+ return program;
+}
+
+
+std::shared_ptr<Material> createShinyMaterial(const SurgSim::Framework::ApplicationData& data)
+{
+ auto material = std::make_shared<SurgSim::Graphics::OsgMaterial>("material");
+ auto program = SurgSim::Graphics::loadProgram(data, "Shaders/material");
+ material->setProgram(program);
+
+ 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;
+}
+
+std::shared_ptr<OsgTextureCubeMap> loadAxisCubeMap(
+ const SurgSim::Framework::ApplicationData& data,
+ const std::string& prefix)
+{
+ std::array<std::string, 6> filenames;
+
+ bool success = data.tryFindFile(prefix + "NegativeX.png", &filenames[0]);
+ success = data.tryFindFile(prefix + "PositiveX.png", &filenames[1]) && success;
+ success = data.tryFindFile(prefix + "NegativeY.png", &filenames[2]) && success;
+ success = data.tryFindFile(prefix + "PositiveY.png", &filenames[3]) && success;
+ success = data.tryFindFile(prefix + "NegativeZ.png", &filenames[4]) && success;
+ success = data.tryFindFile(prefix + "PositiveZ.png", &filenames[5]) && success;
+
+ EXPECT_TRUE(success) << "One or more files are missing";
+
+ auto result = std::make_shared<OsgTextureCubeMap>();
+ result->loadImageFaces(filenames[0], filenames[1], filenames[2], filenames[3], filenames[4], filenames[5]);
+ return result;
+}
+
+void add2DTexture(std::shared_ptr<OsgMaterial> material,
+ const std::string& uniform,
+ int unit,
+ const std::string& filename)
+{
+ std::string path;
+ EXPECT_TRUE(Framework::Runtime::getApplicationData()->tryFindFile(filename, &path));
+ auto texture = std::make_shared<SurgSim::Graphics::OsgTexture2d>();
+ texture->loadImage(path);
+ auto textureUniform = std::make_shared<OsgTextureUniform<OsgTexture2d>>(uniform);
+ textureUniform->set(texture);
+ textureUniform->setMinimumTextureUnit(unit);
+ material->addUniform(textureUniform);
+}
+
+
+struct OsgProgramRenderTests : public RenderTest
+{
+ void SetUp()
+ {
+ RenderTest::SetUp();
+
+ // Light
+ auto 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);
+
+ // Camera
+ viewElement->getCamera()->setAmbientColor(SurgSim::Math::Vector4d(0.1, 0.1, 0.1, 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->enableManipulator(true);
+ }
+
+ void run(size_t time = 500)
+ {
+ // Action
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(time));
+ runtime->stop();
+ }
+};
+
+/// 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(OsgProgramRenderTests, 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 material to the sphere
+ std::shared_ptr<OsgMaterial> material = std::make_shared<OsgMaterial>("material");
+ std::shared_ptr<Program> program = loadExampleProgram(*applicationData);
+
+ material->setProgram(program);
+ sphereRepresentation->setMaterial(material);
+
+ viewElement->addComponent(sphereRepresentation);
+ viewElement->addComponent(material);
+
+ run();
+}
+
+TEST_F(OsgProgramRenderTests, Shiny)
+{
+ /// 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);
+
+ run();
+}
+
+TEST_F(OsgProgramRenderTests, TexturedShiny)
+{
+ // 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 program = SurgSim::Graphics::loadProgram(*runtime->getApplicationData(), "Shaders/ds_mapping_material");
+ ASSERT_TRUE(program != nullptr);
+ material->setProgram(program);
+
+ 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);
+
+ run();
+}
+
+
+
+TEST_F(OsgProgramRenderTests, Metal)
+{
+ // Multiple objects used for testing the shader, utilize if needed
+ std::shared_ptr<SphereRepresentation> sphere = std::make_shared<OsgSphereRepresentation>("sphere");
+ sphere->setRadius(0.25);
+
+ std::shared_ptr<BoxRepresentation> cube = std::make_shared<OsgBoxRepresentation>("box");
+
+ auto scenery = std::make_shared<OsgSceneryRepresentation>("scenery");
+ scenery->loadModel("OsgShaderRenderTests/L_forcep.obj");
+
+ // Assign the object used for testing to the representation
+ auto representation = std::dynamic_pointer_cast<Representation>(sphere);
+
+ auto material = std::make_shared<OsgMaterial>("material");
+ auto program = SurgSim::Graphics::loadProgram(*runtime->getApplicationData(), "Shaders/s_mapping_metal");
+ ASSERT_TRUE(program != nullptr);
+ material->setProgram(program);
+
+ 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", 1024.0f);
+
+ material->addUniform("float", "specularPercent");
+ material->setValue("specularPercent", 1.0f);
+
+ material->addUniform("float", "diffusePercent");
+ material->setValue("diffusePercent", 0.0f);
+
+ std::string filename;
+ // Provide a fake shadow map, it's all black so no shadow contribution
+ {
+ EXPECT_TRUE(runtime->getApplicationData()->tryFindFile("Textures/black.png", &filename));
+ auto texture = std::make_shared<SurgSim::Graphics::OsgTexture2d>();
+ texture->loadImage(filename);
+ auto textureUniform = std::make_shared<OsgTextureUniform<OsgTexture2d>>("shadowMap");
+ textureUniform->set(texture);
+ textureUniform->setMinimumTextureUnit(8);
+ material->addUniform(textureUniform);
+ }
+
+ {
+ // Provide the Diffuse environment map
+ // Axis map is used for testing mapping
+ // auto texture = loadAxisCubeMap(*runtime->getApplicationData(), "Textures/");
+ EXPECT_TRUE(runtime->getApplicationData()->tryFindFile(
+ "Textures/CubeMap_reflection_diffuse.png", &filename));
+ auto texture = std::make_shared<OsgTextureCubeMap>();
+ texture->loadImage(filename);
+ material->addUniform("samplerCube", "diffuseEnvMap");
+ material->setValue("diffuseEnvMap", texture);
+ }
+
+
+ {
+ // Provide the Specular environment map
+ // Axis map is used for testing mapping
+ // auto texture = loadAxisCubeMap(*runtime->getApplicationData(), "Textures/");
+ EXPECT_TRUE(runtime->getApplicationData()->tryFindFile(
+ "Textures/CubeMap_reflection_specular.png", &filename));
+ auto texture = std::make_shared<OsgTextureCubeMap>();
+ texture->loadImage(filename);
+ material->addUniform("samplerCube", "specularEnvMap");
+ material->setValue("specularEnvMap", texture);
+ }
+
+ representation->setMaterial(material);
+
+ auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Sphere");
+ sceneElement->addComponent(representation);
+ sceneElement->addComponent(material);
+ sceneElement->addComponent(std::make_shared<SurgSim::Graphics::OsgAxesRepresentation>("axes"));
+ scene->addSceneElement(sceneElement);
+
+ run();
+}
+
+TEST_F(OsgProgramRenderTests, NormalMap)
+{
+ // Assign the object used for testing to the representation
+ auto graphics = std::make_shared<OsgSceneryRepresentation>("scenery");
+ graphics->loadModel("Geometry/cube.osgt");
+
+ auto representation = std::dynamic_pointer_cast<Representation>(graphics);
+ representation->setGenerateTangents(true);
+
+ auto material = std::make_shared<OsgMaterial>("material");
+ auto program = SurgSim::Graphics::loadProgram(*runtime->getApplicationData(), "Shaders/dns_mapping_material");
+ ASSERT_TRUE(program != nullptr);
+ material->setProgram(program);
+
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", SurgSim::Math::Vector4f(1.0, 1.0, 1.0, 1.0));
+
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", SurgSim::Math::Vector4f(1.0, 1.0, 1.0, 1.0));
+
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", 1.0f);
+
+ std::string filename;
+
+ add2DTexture(material, "shadowMap", 8, "Textures/black.png");
+ add2DTexture(material, "diffuseMap", 0, "Textures/checkered.png");
+ add2DTexture(material, "normalMap", 1, "Textures/bricks.png");
+ representation->setMaterial(material);
+
+ auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Graphics");
+ sceneElement->addComponent(representation);
+ sceneElement->addComponent(material);
+ sceneElement->addComponent(std::make_shared<SurgSim::Graphics::OsgAxesRepresentation>("axes"));
+ scene->addSceneElement(sceneElement);
+
+
+ viewElement->setPose(makeRigidTransform(Vector3d(0.5, 0.5, -0.5),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 1.0, 0.0)));
+
+ run();
+}
+
+TEST_F(OsgProgramRenderTests, BlurShader)
+{
+ auto element = std::make_shared<Framework::BasicSceneElement>("Graphics");
+
+ auto texture1 = std::make_shared<Graphics::OsgTexture2d>();
+ std::string filename;
+ ASSERT_TRUE(Runtime::getApplicationData()->tryFindFile("Textures/checkered.png", &filename));
+ texture1->loadImage(filename);
+
+
+ auto texture2 = std::make_shared<Graphics::OsgTexture2d>();
+ ASSERT_TRUE(Runtime::getApplicationData()->tryFindFile("Textures/checkered.png", &filename));
+ texture2->loadImage(filename);
+
+ // Material
+ auto material = Graphics::buildMaterial("Shaders/gauss_blur_horizontal.vert",
+ "Shaders/gauss_blur.frag");
+ material->addUniform("float", "width");
+ material->setValue("width", 1024.0f);
+ material->addUniform("float", "blurRadius");
+ material->setValue("blurRadius", 16.0f);
+ material->getProgram()->setGlobalScope(true);
+
+ auto graphics = std::make_shared<Graphics::OsgScreenSpaceQuadRepresentation>("Quad");
+ graphics->setSize(1024, 1024);
+ graphics->setLocation(0, 0);
+ graphics->setTexture(texture1);
+ graphics->setGroupReference("BlurrPass");
+ element->addComponent(graphics);
+
+ auto passCamera = std::make_shared<Graphics::OsgCamera>("BlurrPassCamera");
+ auto osgCamera = passCamera->getOsgCamera();
+ passCamera->setRenderGroupReference("BlurrPass");
+ passCamera->setGroupReference(Graphics::Representation::DefaultGroupName);
+ osgCamera->setViewport(0, 0, 1024, 1024);
+ osgCamera->setProjectionMatrixAsOrtho2D(0, 1024, 0, 1024);
+ passCamera->setMaterial(material);
+
+ auto renderTarget = std::make_shared<Graphics::OsgRenderTarget2d>(1024, 1024, 1.0, 1, false);
+ passCamera->setRenderTarget(renderTarget);
+ element->addComponent(passCamera);
+
+
+ graphics = std::make_shared<Graphics::OsgScreenSpaceQuadRepresentation>("DebugQuad");
+ graphics->setLocation(512, 512);
+ graphics->setSize(256, 256);
+ graphics->setTexture(passCamera->getRenderTarget()->getColorTarget(0));
+ element->addComponent(graphics);
+
+ element->addComponent(material);
+
+ scene->addSceneElement(element);
+
+ run();
+}
+
+}; // namespace Graphics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgRepresentationRenderTests.cpp
index 113d3cd..18f686c 100644
--- a/SurgSim/Graphics/RenderTests/OsgRepresentationRenderTests.cpp
+++ b/SurgSim/Graphics/RenderTests/OsgRepresentationRenderTests.cpp
@@ -25,7 +25,9 @@
#include "SurgSim/Graphics/OsgCylinderRepresentation.h"
#include "SurgSim/Graphics/OsgManager.h"
#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/Vector.h"
@@ -33,6 +35,8 @@
#include <random>
+#include <osgUtil/SmoothingVisitor>
+
using SurgSim::Framework::Runtime;
using SurgSim::Framework::Scene;
using SurgSim::Framework::SceneElement;
@@ -49,33 +53,8 @@ namespace SurgSim
namespace Graphics
{
-struct OsgRepresentationRenderTests : public ::testing::Test
+struct OsgRepresentationRenderTests : public RenderTest
{
- 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:
};
@@ -127,7 +106,6 @@ TEST_F(OsgRepresentationRenderTests, RepresentationTest)
/// Run the thread
runtime->start();
- EXPECT_TRUE(manager->isInitialized());
boxRepresentation->setLocalPose(makeRigidTransform(Quaterniond::Identity(), boxPosition));
capsuleRepresentation->setLocalPose(makeRigidTransform(Quaterniond::Identity(), capsulePosition));
@@ -158,6 +136,140 @@ TEST_F(OsgRepresentationRenderTests, RepresentationTest)
boost::this_thread::sleep(boost::posix_time::milliseconds(1500));
}
+class LineGeometryVisitor : public osg::NodeVisitor
+{
+public :
+ LineGeometryVisitor() :
+ NodeVisitor(NodeVisitor::TRAVERSE_ALL_CHILDREN),
+ m_normalsScale(0.1)
+ {
+ }
+
+ virtual ~LineGeometryVisitor() {}
+
+ void apply(osg::Node& node) // NOLINT
+ {
+ traverse(node);
+ }
+
+ void apply(osg::Geode& geode) // NOLINT
+ {
+ // Only deal with 1 geometry for now ...
+ if (geode.getNumDrawables() > 0)
+ {
+ osg::Geometry* curGeom = geode.getDrawable(0)->asGeometry();
+ if (curGeom)
+ {
+ osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>(curGeom->getVertexArray());
+
+ auto normals = curGeom->getNormalArray();
+ geode.addDrawable(buildGeometry(vertices, normals, osg::Vec4(0.0, 0.0, 1.0, 1.0)));
+
+ auto tangents = curGeom->getVertexAttribArray(TANGENT_VERTEX_ATTRIBUTE_ID);
+
+ auto cast = dynamic_cast<osg::Vec4Array*>(tangents);
+ ASSERT_NE(nullptr, cast);
+ ASSERT_EQ(vertices->size(), cast->size());
+ geode.addDrawable(buildGeometry(vertices, tangents, osg::Vec4(1.0, 0.0, 0.0, 1.0)));
+
+ auto bitangents = curGeom->getVertexAttribArray(BITANGENT_VERTEX_ATTRIBUTE_ID);
+ cast = dynamic_cast<osg::Vec4Array*>(bitangents);
+ ASSERT_NE(nullptr, cast);
+ ASSERT_EQ(vertices->size(), cast->size());
+ geode.addDrawable(buildGeometry(vertices, bitangents, osg::Vec4(0.0, 1.0, 0.0, 1.0)));
+
+ }
+ }
+ }
+
+ osg::Geometry* buildGeometry(osg::Vec3Array* geomVertices, osg::Array* directions, osg::Vec4 color)
+ {
+ // create Geometry object to store all the vertices and lines primitive.
+ osg::Geometry* linesGeom = new osg::Geometry();
+
+ osg::Vec3Array* vertices = new osg::Vec3Array(geomVertices->size() * 2);
+ osg::Vec3 direction;
+ for (size_t i = 0; i < geomVertices->size(); ++i)
+ {
+
+ (*vertices)[i * 2] = (*geomVertices)[i];
+ switch (directions->getType())
+ {
+ case osg::Array::Vec3ArrayType:
+ direction = static_cast<const osg::Vec3Array&>(*directions)[i];
+ break;
+ case osg::Array::Vec4ArrayType:
+ for (int j = 0; j < 3; ++j)
+ {
+ direction[j] = static_cast<const osg::Vec4Array&>(*directions)[i].ptr()[j];
+ }
+ break;
+ default:
+ SURGSIM_FAILURE() << "Unhandled Array type.";
+ }
+ (*vertices)[i * 2 + 1] = (*geomVertices)[i] + direction * m_normalsScale;
+ }
+
+ // pass the created vertex array to the points geometry object.
+ linesGeom->setVertexArray(vertices);
+
+ // set the colors as before, plus using the above
+ osg::Vec4Array* colors = new osg::Vec4Array;
+ colors->push_back(color);
+ linesGeom->setColorArray(colors, osg::Array::BIND_OVERALL);
+
+ // set the normal in the same way color.
+ osg::Vec3Array* normals = new osg::Vec3Array;
+ normals->push_back(osg::Vec3(0.0f, -1.0f, 0.0f));
+ linesGeom->setNormalArray(normals, osg::Array::BIND_OVERALL);
+
+ linesGeom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, vertices->size()));
+
+ osg::StateSet* state = linesGeom->getOrCreateStateSet();
+ state->setMode(GL_LIGHTING, osg::StateAttribute::PROTECTED | osg::StateAttribute::OFF);
+
+ return linesGeom;
+ }
+
+private:
+ float m_normalsScale;
+};
+
+
+TEST_F(OsgRepresentationRenderTests, TangentTest)
+{
+ auto element = std::make_shared<Framework::BasicSceneElement>("sphere");
+ auto graphics = std::make_shared<OsgSceneryRepresentation>("sphere");
+ graphics->loadModel("Geometry/sphere0_5.obj");
+ //graphics->setDrawAsWireFrame(true);
+
+ viewElement->enableManipulator(true);
+
+ camera->setLocalPose(Math::makeRigidTransform(
+ Vector3d(0.0, 0.0, -4.0),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 1.0, 0.0)));
+
+ osg::ref_ptr<osg::Node> node = graphics->getOsgNode();
+ // Generate normals
+ osgUtil::SmoothingVisitor sv;
+ node->accept(sv);
+
+ graphics->setGenerateTangents(true);
+
+ LineGeometryVisitor visitor;
+ node->accept(visitor);
+
+ element->addComponent(graphics);
+ scene->addSceneElement(element);
+
+ runtime->start();
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(500));
+ runtime->stop();
+}
+
+
}; // namespace Graphics
}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgSceneryRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgSceneryRepresentationRenderTests.cpp
index f70506e..b0a7a8b 100644
--- a/SurgSim/Graphics/RenderTests/OsgSceneryRepresentationRenderTests.cpp
+++ b/SurgSim/Graphics/RenderTests/OsgSceneryRepresentationRenderTests.cpp
@@ -16,6 +16,7 @@
/// \file
/// Render Tests for OsgSceneryRepresentation class.
+#include "SurgSim/Graphics/OsgModel.h"
#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
#include "SurgSim/Graphics/RenderTests/RenderTest.h"
#include "SurgSim/Math/Quaternion.h"
@@ -60,13 +61,15 @@ TEST_F(OsgSceneryRepresentationRenderTests, RenderTest)
int numSteps = 100;
auto sceneryObject1 = std::make_shared<OsgSceneryRepresentation>("Torus1");
- sceneryObject1->setFileName("OsgSceneryRepresentationTests/Torus.obj");
+ sceneryObject1->loadModel("Geometry/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");
+ auto torus = std::make_shared<OsgModel>();
+ torus->load("Geometry/Torus.osgb");
+ sceneryObject2->setModel(torus);
sceneryObject2->setLocalPose(SurgSim::Math::makeRigidTransform(
SurgSim::Math::Quaterniond::Identity(), startPosition2));
viewElement->addComponent(sceneryObject2);
@@ -85,6 +88,8 @@ TEST_F(OsgSceneryRepresentationRenderTests, RenderTest)
/// The total number of steps should complete in 1 second
boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / numSteps));
}
+
+ std::cout << sceneryObject2->encode();
}
diff --git a/SurgSim/Graphics/RenderTests/OsgScreenSpaceQuadRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgScreenSpaceQuadRenderTests.cpp
index d13f27e..8c3686f 100644
--- a/SurgSim/Graphics/RenderTests/OsgScreenSpaceQuadRenderTests.cpp
+++ b/SurgSim/Graphics/RenderTests/OsgScreenSpaceQuadRenderTests.cpp
@@ -101,8 +101,8 @@ TEST_F(OsgScreenSpaceQuadRenderTests, InitTest)
TEST_F(OsgScreenSpaceQuadRenderTests, TextureTest)
{
- std::string checkerTexturePath = applicationData->findFile("OsgScreenSpaceQuadRenderTests/CheckerBoard.png");
- std::string rectangleTexturePath = applicationData->findFile("OsgScreenSpaceQuadRenderTests/Rectangle.png");
+ std::string checkerTexturePath = applicationData->findFile("Textures/CheckerBoard.png");
+ std::string rectangleTexturePath = applicationData->findFile("Textures/Rectangle.png");
EXPECT_NE("", checkerTexturePath) << "Could not find checker texture shader!";
EXPECT_NE("", rectangleTexturePath) << "Could not find rectangle texture!";
diff --git a/SurgSim/Graphics/RenderTests/OsgShaderRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgShaderRenderTests.cpp
deleted file mode 100644
index 17c731c..0000000
--- a/SurgSim/Graphics/RenderTests/OsgShaderRenderTests.cpp
+++ /dev/null
@@ -1,253 +0,0 @@
-// 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/OsgSkeletonRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgSkeletonRepresentationRenderTests.cpp
new file mode 100644
index 0000000..17e0843
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgSkeletonRepresentationRenderTests.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.
+
+#include <memory>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Graphics/Camera.h"
+#include "SurgSim/Graphics/OsgAxesRepresentation.h"
+#include "SurgSim/Graphics/OsgLight.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgProgram.h"
+#include "SurgSim/Graphics/OsgSkeletonRepresentation.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 OsgSkeletonRepresentationRenderTests : public RenderTest
+{
+};
+
+TEST_F(OsgSkeletonRepresentationRenderTests, BasicTest)
+{
+ auto graphics = std::make_shared<SurgSim::Graphics::OsgSkeletonRepresentation>("Skeleton");
+ graphics->loadModel("OsgSkeletonRepresentationRenderTests/rigged_cylinder.osgt");
+ graphics->setSkinningShaderFileName("Shaders/skinning.vert");
+ auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Rigged Cylinder");
+ sceneElement->addComponent(graphics);
+ scene->addSceneElement(sceneElement);
+
+ viewElement->setPose(SurgSim::Math::makeRigidTransform(
+ Vector3d(-0.3, 0.3, -0.3),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 0.0, 1.0)));
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
+
+ std::pair<RigidTransform3d, RigidTransform3d> rootTransform;
+ rootTransform.first =
+ makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 1.0, 1.0)), Vector3d::Zero());
+ rootTransform.second =
+ makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 1.0, 1.0)), Vector3d::Zero());
+
+ std::pair<RigidTransform3d, RigidTransform3d> boneTransform;
+ boneTransform.first =
+ makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 0.0, 0.0)), Vector3d::Zero());
+ boneTransform.second =
+ makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 0.0, 0.0)), Vector3d(0.0, 4.0, 0.0));
+
+ int numSteps = 1000;
+
+ for (int i = 0; i < numSteps; ++i)
+ {
+ double t = static_cast<double>(i) / numSteps;
+
+ RigidTransform3d pose = interpolate(rootTransform, t);
+ sceneElement->setPose(pose);
+
+ pose = interpolate(boneTransform, t / 3.0);
+ graphics->setBonePose("Bone", RigidTransform3d::Identity());
+ graphics->setBonePose("Bone_001", pose);
+ graphics->setBonePose("Bone_002", pose);
+ graphics->setBonePose("Bone_003", pose);
+
+ /// The total number of steps should complete in 5 seconds
+ boost::this_thread::sleep(boost::posix_time::milliseconds(5000 / numSteps));
+ }
+
+ rootTransform.first =
+ makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 1.0, 1.0)), Vector3d::Zero());
+ rootTransform.second =
+ makeRigidTransform(makeRotationQuaternion(M_2_PI, Vector3d(1.0, 1.0, 1.0)), Vector3d::Zero());
+
+ boneTransform.first =
+ makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 0.0, 0.0)), Vector3d::Zero());
+ boneTransform.second =
+ makeRigidTransform(makeRotationQuaternion(M_PI, Vector3d(1.0, 0.0, 0.0)), Vector3d::Zero());
+
+ for (int i = 0; i < numSteps; ++i)
+ {
+ double t = static_cast<double>(i) / numSteps;
+
+ RigidTransform3d pose = interpolate(rootTransform, t);
+ sceneElement->setPose(pose);
+
+ pose = interpolate(boneTransform, t / 3.0);
+ graphics->setBonePose("Bone", pose);
+ graphics->setBonePose("Bone_001", pose);
+ graphics->setBonePose("Bone_002", pose);
+ graphics->setBonePose("Bone_003", pose);
+
+ /// The total number of steps should complete in 5 seconds
+ boost::this_thread::sleep(boost::posix_time::milliseconds(5000 / numSteps));
+ }
+
+ runtime->stop();
+}
+
+TEST_F(OsgSkeletonRepresentationRenderTests, WithNeutralPoseTest)
+{
+ auto graphics = std::make_shared<SurgSim::Graphics::OsgSkeletonRepresentation>("Skeleton");
+ graphics->loadModel("OsgSkeletonRepresentationRenderTests/rigged_cylinder.osgt");
+ graphics->setSkinningShaderFileName("Shaders/skinning.vert");
+ graphics->setNeutralBonePose("Bone",
+ makeRigidTransform(makeRotationQuaternion(M_PI_4, Vector3d(1.0, 1.0, 1.0)), Vector3d::Zero()));
+
+ auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Rigged Cylinder");
+ sceneElement->addComponent(graphics);
+ scene->addSceneElement(sceneElement);
+
+ viewElement->setPose(SurgSim::Math::makeRigidTransform(
+ Vector3d(-0.3, 0.3, -0.3),
+ Vector3d(0.0, 0.0, 0.0),
+ Vector3d(0.0, 0.0, 1.0)));
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
+
+ std::pair<RigidTransform3d, RigidTransform3d> rootTransform;
+ rootTransform.first =
+ makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 1.0, 1.0)), Vector3d::Zero());
+ rootTransform.second =
+ makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 1.0, 1.0)), Vector3d::Zero());
+
+ std::pair<RigidTransform3d, RigidTransform3d> boneTransform;
+ boneTransform.first =
+ makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 0.0, 0.0)), Vector3d::Zero());
+ boneTransform.second =
+ makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 0.0, 0.0)), Vector3d(0.0, 4.0, 0.0));
+
+ int numSteps = 1000;
+
+ for (int i = 0; i < numSteps; ++i)
+ {
+ double t = static_cast<double>(i) / numSteps;
+
+ RigidTransform3d pose = interpolate(rootTransform, t);
+ sceneElement->setPose(pose);
+
+ pose = interpolate(boneTransform, t / 3.0);
+ graphics->setBonePose("Bone", RigidTransform3d::Identity());
+ graphics->setBonePose("Bone_001", pose);
+ graphics->setBonePose("Bone_002", pose);
+ graphics->setBonePose("Bone_003", pose);
+
+ /// The total number of steps should complete in 5 seconds
+ boost::this_thread::sleep(boost::posix_time::milliseconds(5000 / numSteps));
+ }
+
+ runtime->stop();
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgSphereRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgSphereRepresentationRenderTests.cpp
index 05144a9..70d1b98 100644
--- a/SurgSim/Graphics/RenderTests/OsgSphereRepresentationRenderTests.cpp
+++ b/SurgSim/Graphics/RenderTests/OsgSphereRepresentationRenderTests.cpp
@@ -19,11 +19,13 @@
#include "SurgSim/Graphics/OsgManager.h"
#include "SurgSim/Graphics/OsgSphereRepresentation.h"
#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/OsgCamera.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 "SurgSim/Graphics/RenderTests/RenderTest.h"
#include <gtest/gtest.h>
@@ -43,8 +45,12 @@ namespace SurgSim
namespace Graphics
{
+struct OsgSphereRepresentationRenderTests : public RenderTest
+{
+};
+
/// Pops up a window with two spheres that are translating and changing radius
-TEST(OsgSphereRepresentationRenderTests, MovingSpheresTest)
+TEST_F(OsgSphereRepresentationRenderTests, MovingSpheresTest)
{
/// Initial sphere 1 position
Vector3d startPosition1(-0.1, 0.0, -0.2);
@@ -67,17 +73,6 @@ TEST(OsgSphereRepresentationRenderTests, MovingSpheresTest)
/// 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");
@@ -88,7 +83,7 @@ TEST(OsgSphereRepresentationRenderTests, MovingSpheresTest)
/// 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)
@@ -97,10 +92,10 @@ TEST(OsgSphereRepresentationRenderTests, MovingSpheresTest)
double t = static_cast<double>(i) / numSteps;
/// Interpolate position and radius
sphereRepresentation1->setLocalPose(makeRigidTransform(Quaterniond::Identity(), (1 - t) * startPosition1 +
- t * endPosition1));
+ t * endPosition1));
sphereRepresentation1->setRadius((1 - t) * startRadius1 + t * endRadius1);
sphereRepresentation2->setLocalPose(makeRigidTransform(Quaterniond::Identity(), (1 - t) * startPosition2 +
- t * endPosition2));
+ 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));
diff --git a/SurgSim/Graphics/RenderTests/OsgTextRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgTextRepresentationRenderTests.cpp
new file mode 100644
index 0000000..2062b50
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/OsgTextRepresentationRenderTests.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 <gtest/gtest.h>
+
+#include "SurgSim/Blocks/PoseInterpolator.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/BehaviorManager.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgCamera.h"
+#include "SurgSim/Graphics/OsgTextRepresentation.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/OsgAxesRepresentation.h"
+#include "SurgSim/Graphics/View.h"
+
+#include <osg/Notify>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+struct OsgTextRepresentationRenderTests : public ::testing::Test
+{
+public:
+
+ void SetUp()
+ {
+ runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ graphicsManager = std::make_shared<SurgSim::Graphics::OsgManager>();
+ runtime->addManager(graphicsManager);
+ runtime->addManager(std::make_shared<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 TearDown()
+ {
+ runtime->stop();
+ }
+
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime;
+ std::shared_ptr<OsgManager> graphicsManager;
+ std::shared_ptr<SurgSim::Framework::Scene> scene;
+ std::shared_ptr<OsgViewElement> viewElement;
+};
+
+
+TEST_F(OsgTextRepresentationRenderTests, Operation)
+{
+ auto text = std::make_shared<OsgTextRepresentation>("HUD");
+ text->setText("HelloWorld");
+ viewElement->addComponent(text);
+
+ runtime->start();
+ EXPECT_TRUE(graphicsManager->isInitialized());
+ EXPECT_TRUE(viewElement->isInitialized());
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1500));
+
+ auto dimensions = viewElement->getView()->getDimensions();
+ text->setLocation(dimensions[0] / 2.0, dimensions[1] / 2.0);
+ text->setText("Hello Again");
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1500));
+
+ text->setColor(SurgSim::Math::Vector4d(1.0, 0.5, 0.5, 1.0));
+ text->setMaximumWidth(400.0);
+ text->setFontSize(72.0);
+ text->setText("This is a really long line that should be broken apart");
+ text->setDrawBackground(true);
+ text->setBackgroundColor(Math::Vector4d(0.3, 0.3, 0.3, 1.0));
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1500));
+}
+
+TEST_F(OsgTextRepresentationRenderTests, WorldSpace)
+{
+ auto element = std::make_shared<Framework::BasicSceneElement>("Box");
+ auto axis = std::make_shared<Graphics::OsgAxesRepresentation>("Axes");
+ axis->setSize(0.1);
+ element->addComponent(axis);
+ auto box = std::make_shared<OsgBoxRepresentation>("Box");
+ box->setSize(Math::Vector3d(0.05, 0.05, 0.05));
+ element->addComponent(box);
+ scene->addSceneElement(element);
+
+ auto interpolator = std::make_shared<Blocks::PoseInterpolator>("Interpolator");
+ interpolator->setTarget(viewElement);
+ auto startingPose = Math::makeRigidTransform(
+ Math::Vector3d(0.5, 0.5, 0.0),
+ Math::Vector3d(0.0, 0.0, 0.0),
+ Math::Vector3d(0.0, 1.0, 0.0));
+ interpolator->setStartingPose(startingPose);
+ interpolator->setEndingPose(Math::makeRigidTransform(
+ Math::Vector3d(-0.5, 0.5, 0.0),
+ Math::Vector3d(0.0, 0.0, 0.0),
+ Math::Vector3d(0.0, 1.0, 0.0)));
+ interpolator->setPingPong(true);
+ interpolator->setDuration(6.0);
+ viewElement->addComponent(interpolator);
+
+ auto text = std::make_shared<OsgTextRepresentation>("Text");
+ text->setText("This is a multiline text, it should be well readable and right in front of the camera");
+ text->setUseScreenSpace(false);
+ text->setMaximumWidth(0.4);
+ text->setFontSize(0.02);
+ text->setLocalPose(Math::makeRigidTranslation(Math::Vector3d(0.0, 0.0, -1.0)));
+ viewElement->addComponent(text);
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1500));
+
+ runtime->stop();
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/OsgVectorFieldRepresentationRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgVectorFieldRepresentationRenderTests.cpp
index 28cf3cf..5d25934 100644
--- a/SurgSim/Graphics/RenderTests/OsgVectorFieldRepresentationRenderTests.cpp
+++ b/SurgSim/Graphics/RenderTests/OsgVectorFieldRepresentationRenderTests.cpp
@@ -130,11 +130,12 @@ protected:
TEST_F(OsgVectorFieldRepresentationRenderTests, AddVectors)
{
- auto vectorRepresentation = std::make_shared<OsgVectorFieldRepresentation>("vector field representation");
+ auto vectorRepresentation = std::make_shared<OsgVectorFieldRepresentation>("VectorField");
auto points = makeStartingPoints();
auto colors = makeStartingColors();
auto vectors = makeVectors(points, colors);
- auto vectorField = vectorRepresentation->getVectorField();
+
+ SurgSim::Graphics::VectorField vectorField;
vectorRepresentation->setLocalPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(0.0, 0.0, -8.0)));
viewElement->addComponent(vectorRepresentation);
@@ -149,7 +150,8 @@ TEST_F(OsgVectorFieldRepresentationRenderTests, AddVectors)
auto v = std::begin(vectors);
for (; it != std::end(points); ++it, ++v)
{
- vectorField->addVertex(Vertex<SurgSim::Graphics::VectorFieldData>((*it), *v));
+ vectorField.addVertex(Vertex<SurgSim::Graphics::VectorFieldData>((*it), *v));
+ vectorRepresentation->updateVectorField(vectorField);
boost::this_thread::sleep(boost::posix_time::milliseconds(250));
}
}
diff --git a/SurgSim/Graphics/RenderTests/OsgViewElementRenderTests.cpp b/SurgSim/Graphics/RenderTests/OsgViewElementRenderTests.cpp
index 20e1e4f..45400f1 100644
--- a/SurgSim/Graphics/RenderTests/OsgViewElementRenderTests.cpp
+++ b/SurgSim/Graphics/RenderTests/OsgViewElementRenderTests.cpp
@@ -106,11 +106,20 @@ TEST(OsgViewElementRenderTest, FullScreenView)
scene->addSceneElement(viewElement);
auto osgView = std::static_pointer_cast<OsgView>(viewElement->getView());
+ std::array<int, 2> dimensions = { 0, 0 };
+ osgView->setDimensions(dimensions);
+ EXPECT_EQ(0, osgView->getDimensions()[0]);
+ EXPECT_EQ(0, osgView->getDimensions()[1]);
osgView->setFullScreen(true);
+ // The actual value depends on the system, it's hard to test for it here
+ EXPECT_NE(0, osgView->getDimensions()[0]);
+ EXPECT_NE(0, osgView->getDimensions()[1]);
+
runtime->start();
boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
runtime->stop();
+
}
TEST(OsgViewElementRenderTest, StereoView)
diff --git a/SurgSim/Graphics/RenderTests/PaintBehaviorRenderTests.cpp b/SurgSim/Graphics/RenderTests/PaintBehaviorRenderTests.cpp
new file mode 100644
index 0000000..ba0e3f4
--- /dev/null
+++ b/SurgSim/Graphics/RenderTests/PaintBehaviorRenderTests.cpp
@@ -0,0 +1,181 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/GraphicsUtilities.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Graphics/OsgLight.h"
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Graphics/OsgTexture2d.h"
+#include "SurgSim/Graphics/PaintBehavior.h"
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRigidTranslation;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+class PaintBehaviorRenderTests : public RenderTest
+{
+
+};
+
+TEST_F(PaintBehaviorRenderTests, PaintTest)
+{
+ viewElement->getCamera()->setLocalPose(makeRigidTransform(Math::Vector3d(0.0, 0.2, 0.0),
+ Math::Vector3d(0.0, 0.0, 0.0),
+ Math::Vector3d(0.0, 0.0, -1.0)));
+ viewElement->getCamera()->setAmbientColor(Math::Vector4d(0.2, 0.2, 0.2, 1.0));
+
+ auto light = std::make_shared<Graphics::OsgLight>("Light");
+ light->setDiffuseColor(Math::Vector4d(1.0, 1.0, 1.0, 1.0));
+ light->setSpecularColor(Math::Vector4d(0.8, 0.8, 0.8, 1.0));
+ light->setLightGroupReference(Graphics::Representation::DefaultGroupName);
+
+ auto lightElement = std::make_shared<Framework::BasicSceneElement>("LightElement");
+ lightElement->setPose(makeRigidTranslation(Math::Vector3d(2.0, 2.0, 2.0)));
+ lightElement->addComponent(light);
+ scene->addSceneElement(lightElement);
+
+ auto element = std::make_shared<Framework::BasicSceneElement>("Cube");
+ auto graphics = std::make_shared<Graphics::OsgMeshRepresentation>("Graphics");
+ graphics->loadMesh("Geometry/wound_deformable_with_texture.ply");
+
+ auto material = Graphics::buildMaterial("Shaders/ds_mapping_material.vert", "Shaders/ds_mapping_multitexture.frag");
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", Math::Vector4f(1.0, 1.0, 1.0, 1.0));
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", Math::Vector4f(1.0, 1.0, 1.0, 1.0));
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", 10.0f);
+
+ auto texture = std::make_shared<Graphics::OsgTexture2d>();
+ auto path = applicationData->findFile("Textures/wound_deformable.png");
+ texture->loadImage(path);
+ material->addUniform("sampler2D", "diffuseMap");
+ material->setValue("diffuseMap", texture);
+ Blocks::enable2DTexture(material, "shadowMap", Graphics::SHADOW_TEXTURE_UNIT, "Textures/black.png");
+
+ graphics->setMaterial(material);
+
+ element->addComponent(graphics);
+ element->addComponent(material);
+
+ auto paintBehavior = std::make_shared<Graphics::PaintBehavior>("Decals");
+ paintBehavior->setRepresentation(graphics);
+ paintBehavior->setTextureSize(2048, 2048);
+ paintBehavior->setRadius(0.05);
+ paintBehavior->setColor(Math::Vector4d(1.0, 0.0, 0.0, 0.75));
+
+ std::vector<DataStructures::IndexedLocalCoordinate> coords;
+ for (size_t i = 0; i < 10; i++)
+ {
+ Math::Vector coord(3);
+ coord << 0.5, 0.5, 0.5;
+ DataStructures::IndexedLocalCoordinate localCoordinate(50 + i, coord);
+ coords.push_back(localCoordinate);
+ }
+ paintBehavior->setCoordinates(coords);
+
+ element->addComponent(paintBehavior);
+
+ scene->addSceneElement(element);
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(5000));
+ runtime->stop();
+}
+
+TEST_F(PaintBehaviorRenderTests, AntiAliasTest)
+{
+ viewElement->getCamera()->setLocalPose(makeRigidTransform(Math::Vector3d(0.0, 0.2, 0.0),
+ Math::Vector3d(0.0, 0.0, 0.0),
+ Math::Vector3d(0.0, 0.0, -1.0)));
+ viewElement->getCamera()->setAmbientColor(Math::Vector4d(0.2, 0.2, 0.2, 1.0));
+
+ auto light = std::make_shared<Graphics::OsgLight>("Light");
+ light->setDiffuseColor(Math::Vector4d(1.0, 1.0, 1.0, 1.0));
+ light->setSpecularColor(Math::Vector4d(0.8, 0.8, 0.8, 1.0));
+ light->setLightGroupReference(Graphics::Representation::DefaultGroupName);
+
+ auto lightElement = std::make_shared<Framework::BasicSceneElement>("LightElement");
+ lightElement->setPose(makeRigidTranslation(Math::Vector3d(2.0, 2.0, 2.0)));
+ lightElement->addComponent(light);
+ scene->addSceneElement(lightElement);
+
+ auto element = std::make_shared<Framework::BasicSceneElement>("Cube");
+ auto graphics = std::make_shared<Graphics::OsgMeshRepresentation>("Graphics");
+ graphics->loadMesh("Geometry/wound_deformable_with_texture.ply");
+
+ auto material = Graphics::buildMaterial("Shaders/ds_mapping_material.vert", "Shaders/ds_mapping_multitexture.frag");
+ material->addUniform("vec4", "diffuseColor");
+ material->setValue("diffuseColor", Math::Vector4f(1.0, 1.0, 1.0, 1.0));
+ material->addUniform("vec4", "specularColor");
+ material->setValue("specularColor", Math::Vector4f(1.0, 1.0, 1.0, 1.0));
+ material->addUniform("float", "shininess");
+ material->setValue("shininess", 10.0f);
+
+ auto texture = std::make_shared<Graphics::OsgTexture2d>();
+ auto path = applicationData->findFile("Textures/wound_deformable.png");
+ texture->loadImage(path);
+ material->addUniform("sampler2D", "diffuseMap");
+ material->setValue("diffuseMap", texture);
+ Blocks::enable2DTexture(material, "shadowMap", Graphics::SHADOW_TEXTURE_UNIT, "Textures/black.png");
+
+ graphics->setMaterial(material);
+
+ element->addComponent(graphics);
+ element->addComponent(material);
+
+ auto paintBehavior = std::make_shared<Graphics::PaintBehavior>("Decals");
+ paintBehavior->setRepresentation(graphics);
+ paintBehavior->setTextureSize(2048, 2048);
+ paintBehavior->setRadius(0.05);
+ paintBehavior->setColor(Math::Vector4d(1.0, 0.0, 0.0, 0.75));
+ paintBehavior->setAntiAlias(true);
+
+ std::vector<DataStructures::IndexedLocalCoordinate> coords;
+ for (size_t i = 0; i < 10; i++)
+ {
+ Math::Vector coord(3);
+ coord << 0.5, 0.5, 0.5;
+ DataStructures::IndexedLocalCoordinate localCoordinate(50 + i, coord);
+ coords.push_back(localCoordinate);
+ }
+ paintBehavior->setCoordinates(coords);
+
+ element->addComponent(paintBehavior);
+
+ scene->addSceneElement(element);
+
+ runtime->start();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(5000));
+ runtime->stop();
+}
+
+}; // namespace Graphics
+}; // namespace SurgSim
diff --git a/SurgSim/Graphics/RenderTests/RenderTest.cpp b/SurgSim/Graphics/RenderTests/RenderTest.cpp
index 864e047..bb41643 100644
--- a/SurgSim/Graphics/RenderTests/RenderTest.cpp
+++ b/SurgSim/Graphics/RenderTests/RenderTest.cpp
@@ -13,7 +13,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "SurgSim/Graphics/RenderTests//RenderTest.h"
+#include "SurgSim/Graphics/RenderTests/RenderTest.h"
#include "SurgSim/Framework/BehaviorManager.h"
#include "SurgSim/Framework/Runtime.h"
@@ -51,6 +51,8 @@ void RenderTest::SetUp()
viewElement->getView()->setPosition(position);
viewElement->getView()->setWindowBorderEnabled(true);
+ camera = std::dynamic_pointer_cast<OsgCamera>(viewElement->getCamera());
+
scene->addSceneElement(viewElement);
}
diff --git a/SurgSim/Graphics/RenderTests/RenderTest.h b/SurgSim/Graphics/RenderTests/RenderTest.h
index 878a1e6..53322ee 100644
--- a/SurgSim/Graphics/RenderTests/RenderTest.h
+++ b/SurgSim/Graphics/RenderTests/RenderTest.h
@@ -23,6 +23,7 @@
#include "SurgSim/Framework/Scene.h"
#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Graphics/OsgCamera.h"
#include "SurgSim/Graphics/OsgView.h"
#include "SurgSim/Graphics/OsgViewElement.h"
#include "SurgSim/Graphics/OsgManager.h"
@@ -61,6 +62,7 @@ public:
std::shared_ptr<SurgSim::Framework::Scene> scene;
std::shared_ptr<OsgViewElement> viewElement;
std::shared_ptr<const SurgSim::Framework::ApplicationData> applicationData;
+ std::shared_ptr<OsgCamera> camera;
};
diff --git a/SurgSim/Graphics/RenderTests/config.txt.in b/SurgSim/Graphics/RenderTests/config.txt.in
index f614965..1f4c944 100644
--- a/SurgSim/Graphics/RenderTests/config.txt.in
+++ b/SurgSim/Graphics/RenderTests/config.txt.in
@@ -1,3 +1,3 @@
-${SURGSIM_SOURCE_DIR}/Data/
-${SURGSIM_SOURCE_DIR}/SurgSim/Testing/
-${CMAKE_CURRENT_SOURCE_DIR}/Data/
\ No newline at end of file
+${PROJECT_BINARY_DIR}/Data/
+${CMAKE_CURRENT_BINARY_DIR}/Data/
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data/
diff --git a/SurgSim/Graphics/Representation.cpp b/SurgSim/Graphics/Representation.cpp
index 096e83f..2e0da1e 100644
--- a/SurgSim/Graphics/Representation.cpp
+++ b/SurgSim/Graphics/Representation.cpp
@@ -28,14 +28,14 @@ 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::Framework::Representation(name)
{
- 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, std::vector<std::string>, GroupReferences,
+ getGroupReferences, setGroupReferences);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, bool, DrawAsWireFrame,
getDrawAsWireFrame, setDrawAsWireFrame);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, bool, GenerateTangents,
+ isGeneratingTangents, setGenerateTangents);
addGroupReference(DefaultGroupName);
}
@@ -74,7 +74,7 @@ void Representation::setGroupReferences(const std::vector<std::string>& groups)
}
}
-std::vector<std::string> Representation::getGroupReferences()
+std::vector<std::string> Representation::getGroupReferences() const
{
return std::vector<std::string>(std::begin(m_groups), std::end(m_groups));
}
@@ -122,11 +122,5 @@ 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
index cf63a3d..189ff00 100644
--- a/SurgSim/Graphics/Representation.h
+++ b/SurgSim/Graphics/Representation.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -50,21 +50,10 @@ public:
/// 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;
+ virtual bool setMaterial(std::shared_ptr<SurgSim::Framework::Component> material) = 0;
/// Gets the material that defines the visual appearance of the representation
/// \return Graphics material
@@ -81,6 +70,13 @@ public:
/// \return True if this representation is rendered as a wireframe; false if not.
virtual bool getDrawAsWireFrame() const = 0;
+ /// Enable or disable the generation of tangents.
+ /// \param value true enables tangent generation, false otherwise
+ virtual void setGenerateTangents(bool value) = 0;
+
+ /// \return whether tangent generation is on or off
+ virtual bool isGeneratingTangents() const = 0;
+
/// Updates the representation
/// \param dt The time in seconds of the preceding timestep.
virtual void update(double dt) = 0;
@@ -107,7 +103,7 @@ public:
/// Gets group references.
/// \return The group references.
- std::vector<std::string> getGroupReferences();
+ std::vector<std::string> getGroupReferences() const;
/// Function to remove an unwanted reference.
/// \param group The name of the reference that should be removed
@@ -117,11 +113,7 @@ public:
/// 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;
@@ -129,11 +121,9 @@ private:
/// \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.cpp b/SurgSim/Graphics/SceneryRepresentation.cpp
new file mode 100644
index 0000000..41e3d7c
--- /dev/null
+++ b/SurgSim/Graphics/SceneryRepresentation.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/Graphics/SceneryRepresentation.h"
+
+#include "SurgSim/Framework/Asset.h"
+#include "SurgSim/Graphics/Model.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+
+SceneryRepresentation::SceneryRepresentation(const std::string& name) : Representation(name)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SceneryRepresentation, std::shared_ptr<SurgSim::Framework::Asset>,
+ Model , getModel, setModel);
+ SURGSIM_ADD_SETTER(SceneryRepresentation, std::string, ModelFileName, loadModel);
+}
+
+
+}
+}
+
diff --git a/SurgSim/Graphics/SceneryRepresentation.h b/SurgSim/Graphics/SceneryRepresentation.h
index cf064e2..b04f690 100644
--- a/SurgSim/Graphics/SceneryRepresentation.h
+++ b/SurgSim/Graphics/SceneryRepresentation.h
@@ -22,10 +22,16 @@
namespace SurgSim
{
+namespace Framework
+{
+class Asset;
+}
namespace Graphics
{
+class Model;
+
/// Base class defining the interface for a Graphics Scenery Object.
class SceneryRepresentation : public virtual Representation
{
@@ -33,18 +39,19 @@ 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;
+ explicit SceneryRepresentation(const std::string& name);
- /// Set file name of the object to be loaded
+ /// Convenience function to trigger the load of the model with the given filename, if successful, this will
+ /// replace the old model
/// \param fileName Name of the file to be loaded
- virtual void setFileName(const std::string& fileName) = 0;
+ virtual void loadModel(const std::string& fileName) = 0;
+
+ /// Set the current model to the model passed
+ /// \param model to be used for this scenery representation, this will replace the old model
+ virtual void setModel(std::shared_ptr<SurgSim::Framework::Asset> model) = 0;
+
+ /// \return the current model.
+ virtual std::shared_ptr<Model> getModel() const = 0;
};
}; // namespace Graphics
diff --git a/SurgSim/Graphics/ScreenSpaceQuadRepresentation.h b/SurgSim/Graphics/ScreenSpaceQuadRepresentation.h
index bcd0804..9fdf711 100644
--- a/SurgSim/Graphics/ScreenSpaceQuadRepresentation.h
+++ b/SurgSim/Graphics/ScreenSpaceQuadRepresentation.h
@@ -16,8 +16,13 @@
#ifndef SURGSIM_GRAPHICS_SCREENSPACEQUADREPRESENTATION_H
#define SURGSIM_GRAPHICS_SCREENSPACEQUADREPRESENTATION_H
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
#include "SurgSim/Graphics/Representation.h"
+#include <array>
+
namespace SurgSim
{
namespace Graphics
@@ -36,7 +41,6 @@ public:
/// \param name The name.
explicit ScreenSpaceQuadRepresentation(const std::string name) : Representation(name)
{
-
}
~ScreenSpaceQuadRepresentation()
@@ -69,9 +73,15 @@ public:
/// \return true if it succeeds, false if it fails.
virtual bool setTexture(std::shared_ptr<Texture> texture) = 0;
+ /// \return the pose of the quad, this disregards the SceneElement pose
+ SurgSim::Math::RigidTransform3d getPose() const override
+ {
+ return getLocalPose();
+ }
+
};
}; // Graphics
}; // SurgSim
-#endif
\ No newline at end of file
+#endif
diff --git a/SurgSim/Graphics/Shader.h b/SurgSim/Graphics/Shader.h
deleted file mode 100644
index 5d5e89f..0000000
--- a/SurgSim/Graphics/Shader.h
+++ /dev/null
@@ -1,123 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/SkeletonRepresentation.h b/SurgSim/Graphics/SkeletonRepresentation.h
new file mode 100644
index 0000000..8006b35
--- /dev/null
+++ b/SurgSim/Graphics/SkeletonRepresentation.h
@@ -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.
+
+#ifndef SURGSIM_GRAPHICS_SKELETONREPRESENTATION_H
+#define SURGSIM_GRAPHICS_SKELETONREPRESENTATION_H
+
+#include <string>
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Graphics/Model.h"
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+/// Skeleton representation is used to move a mesh based on the movements of
+/// pre-selected control points (bones).
+class SkeletonRepresentation : public virtual Representation
+{
+public:
+
+ /// Constructor.
+ /// \param name The name of the representation.
+ explicit SkeletonRepresentation(const std::string& name): Representation(name)
+ {
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SkeletonRepresentation, std::shared_ptr<SurgSim::Framework::Asset>,
+ Model, getModel, setModel);
+
+ // Enables the alternative use of the model file instead of the actual mesh object
+ DecoderType decoder = std::bind(&SkeletonRepresentation::loadModel,
+ this,
+ std::bind(&YAML::Node::as<std::string>, std::placeholders::_1));
+ setDecoder("ModelFileName", decoder);
+
+ SetterType setter = std::bind(&SkeletonRepresentation::loadModel,
+ this,
+ std::bind(SurgSim::Framework::convert<std::string>, std::placeholders::_1));
+
+ setSetter("ModelFileName", setter);
+
+ typedef std::map<std::string, SurgSim::Math::RigidTransform3d> PoseMap;
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SkeletonRepresentation, PoseMap,
+ NeutralPoses, getNeutralBonePoses, setNeutralBonePoses);
+ }
+
+ /// Convenience function to trigger the load of the model with the given filename, if successful, this will
+ /// replace the old model
+ /// \param fileName Name of the file to be loaded
+ virtual void loadModel(const std::string& fileName) = 0;
+
+ /// Set the current model to the model passed
+ /// \param model to be used for this scenery representation, this will replace the old model
+ virtual void setModel(std::shared_ptr<SurgSim::Framework::Asset> model) = 0;
+
+ /// \return the current model.
+ virtual std::shared_ptr<Model> getModel() const = 0;
+
+ /// Set the pose for a given bone.
+ /// \param name The name of the bone.
+ /// \param pose The pose of the bone.
+ virtual void setBonePose(const std::string& name, const SurgSim::Math::RigidTransform3d& pose) = 0;
+
+ /// Get the pose for a given bone.
+ /// \param name The name of the bone.
+ /// \return pose The pose of the bone.
+ virtual SurgSim::Math::RigidTransform3d getBonePose(const std::string& name) const = 0;
+
+ /// Set the neutral pose for a given bone.
+ /// \param name The name of the bone.
+ /// \param pose The neutral pose of the bone.
+ virtual void setNeutralBonePose(const std::string& name, const SurgSim::Math::RigidTransform3d& pose) = 0;
+
+ /// Get the neutral pose for a given bone.
+ /// \param name The name of the bone.
+ /// \return pose The neutral pose of the bone.
+ virtual SurgSim::Math::RigidTransform3d getNeutralBonePose(const std::string& name) const = 0;
+
+protected:
+ /// Set neutral poses for a set of bones.
+ /// \param poses A map of bone names and neutral poses
+ virtual void setNeutralBonePoses(const std::map<std::string, SurgSim::Math::RigidTransform3d>& poses) = 0;
+
+ /// Get all the neutral poses
+ /// \return A map of bone names and neutral poses
+ virtual std::map<std::string, SurgSim::Math::RigidTransform3d> getNeutralBonePoses() const = 0;
+};
+
+}; // namespace Graphics
+}; // namespace SurgSim
+
+#endif // SURGSIM_GRAPHICS_SKELETONREPRESENTATION_H
diff --git a/SurgSim/Graphics/TangentSpaceGenerator.cpp b/SurgSim/Graphics/TangentSpaceGenerator.cpp
new file mode 100644
index 0000000..6de18ea
--- /dev/null
+++ b/SurgSim/Graphics/TangentSpaceGenerator.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.
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Graphics/TangentSpaceGenerator.h"
+
+#include <osg/TriangleIndexFunctor>
+#include <osg/Vec3>
+#include <osg/Array>
+
+namespace
+{
+
+void orthogonalize(const osg::Vec3& normal, osg::Vec4* tangent, osg::Vec4* bitangent,
+ bool createOrthonormalBasis = false)
+{
+ SURGSIM_ASSERT(tangent != nullptr) << "Tanget parameter can't be nullptr.";
+ SURGSIM_ASSERT(bitangent != nullptr) << "BiTangent parameter can't be nullptr.";
+
+ osg::Vec4 normal4(normal, 0.0);
+ // Gram-Schmidt orthogonalize the tangent
+ (*tangent) = (*tangent) - normal4 * (normal4 * (*tangent));
+ tangent->normalize();
+
+ if (createOrthonormalBasis)
+ {
+ osg::Vec3 tangent3 = osg::Vec3(tangent->x(), tangent->y(), tangent->z());
+ osg::Vec3 bitangent3 = osg::Vec3(bitangent->x(), bitangent->y(), bitangent->z());
+ osg::Vec3 cross = normal ^ tangent3;
+
+ // Calculate handedness
+ float handedness = 1.0f;
+ if ((cross * bitangent3) < 0.0f)
+ {
+ handedness = -1.0f;
+ }
+
+ (*bitangent) = osg::Vec4(cross * handedness, 0.0f);
+ }
+ else
+ {
+ // Gram-Schmidt orthogonalize the bitangent
+ (*bitangent) = (*bitangent) - normal4 * (normal4 * (*bitangent));
+ bitangent->normalize();
+ }
+}
+
+}
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+GenerateTangentSpaceTriangleIndexFunctor::GenerateTangentSpaceTriangleIndexFunctor() :
+ m_vertexArray(nullptr),
+ m_normalArray(nullptr),
+ m_textureCoordArray(nullptr),
+ m_tangentArray(nullptr),
+ m_bitangentArray(nullptr),
+ m_createOrthonormalBasis(false)
+{
+}
+
+void GenerateTangentSpaceTriangleIndexFunctor::setBasisOrthonormality(bool orthonormal)
+{
+ m_createOrthonormalBasis = orthonormal;
+}
+bool GenerateTangentSpaceTriangleIndexFunctor::getBasisOrthonormality()
+{
+ return m_createOrthonormalBasis;
+}
+
+void GenerateTangentSpaceTriangleIndexFunctor::set(
+ const osg::Vec3Array* vertexArray,
+ const osg::Vec3Array* normalArray,
+ const osg::Vec2Array* textureCoordArray,
+ osg::Vec4Array* tangentArray,
+ osg::Vec4Array* bitangentArray)
+{
+
+ SURGSIM_ASSERT(vertexArray != nullptr) << "Need vertex array to generate normals!";
+ size_t numVertices = vertexArray->size();
+
+ SURGSIM_ASSERT(normalArray != nullptr) << "Need normal array to store normals!";
+ SURGSIM_ASSERT(normalArray->size() == numVertices)
+ << "Size of normal array must match the number of vertices to generate tangent space!";
+
+ SURGSIM_ASSERT(tangentArray != nullptr) << "Need tangent array to store tangent space!";
+ SURGSIM_ASSERT(tangentArray->size() == numVertices)
+ << "Size of tangent array must match the number of vertices to generate tangent space!";
+
+ SURGSIM_ASSERT(bitangentArray != nullptr) << "Need bitangent array to store tangent space!";
+ SURGSIM_ASSERT(bitangentArray->size() == numVertices)
+ << "Size of bitangent array must match the number of vertices to generate tangent space!";
+
+ m_vertexArray = vertexArray;
+ m_normalArray = normalArray;
+ m_textureCoordArray = textureCoordArray;
+ m_tangentArray = tangentArray;
+ m_bitangentArray = bitangentArray;
+}
+
+void GenerateTangentSpaceTriangleIndexFunctor::orthogonalize()
+{
+ size_t numVertices = m_vertexArray->size();
+
+ for (size_t vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex)
+ {
+ ::orthogonalize((*m_normalArray)[vertexIndex],
+ &(*m_tangentArray)[vertexIndex],
+ &(*m_bitangentArray)[vertexIndex],
+ m_createOrthonormalBasis);
+ }
+}
+
+void GenerateTangentSpaceTriangleIndexFunctor::reset()
+{
+ size_t numVertices = m_vertexArray->size();
+
+ for (size_t vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex)
+ {
+ (*m_tangentArray)[vertexIndex].set(0.0f, 0.0f, 0.0f, 0.0f);
+ (*m_bitangentArray)[vertexIndex].set(0.0f, 0.0f, 0.0f, 0.0f);
+ }
+}
+
+void GenerateTangentSpaceTriangleIndexFunctor::operator()(unsigned int vertexIndex1, unsigned int vertexIndex2,
+ unsigned int 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];
+
+ const osg::Vec2& w1 = (*m_textureCoordArray)[vertexIndex1];
+ const osg::Vec2& w2 = (*m_textureCoordArray)[vertexIndex2];
+ const osg::Vec2& w3 = (*m_textureCoordArray)[vertexIndex3];
+
+ float x1 = v2.x() - v1.x();
+ float x2 = v3.x() - v1.x();
+ float y1 = v2.y() - v1.y();
+ float y2 = v3.y() - v1.y();
+ float z1 = v2.z() - v1.z();
+ float z2 = v3.z() - v1.z();
+
+ float s1 = w2.x() - w1.x();
+ float s2 = w3.x() - w1.x();
+ float t1 = w2.y() - w1.y();
+ float t2 = w3.y() - w1.y();
+
+ float r = 1.0f / (s1 * t2 - s2 * t1);
+ osg::Vec4 tangent((t2 * x1 - t1 * x2) * r, (t2 * y1 - t1 * y2) * r, (t2 * z1 - t1 * z2) * r, 0.0f);
+ osg::Vec4 bitangent((s1 * x2 - s2 * x1) * r, (s1 * y2 - s2 * y1) * r, (s1 * z2 - s2 * z1) * r, 0.0f);
+
+ (*m_tangentArray)[vertexIndex1] += tangent;
+ (*m_bitangentArray)[vertexIndex1] += bitangent;
+
+ (*m_tangentArray)[vertexIndex2] += tangent;
+ (*m_bitangentArray)[vertexIndex2] += bitangent;
+
+ (*m_tangentArray)[vertexIndex3] += tangent;
+ (*m_bitangentArray)[vertexIndex3] += bitangent;
+}
+
+TangentSpaceGenerator::TangentSpaceGenerator(int textureCoordUnit, int tangentAttribIndex, int bitangentAttribIndex) :
+ osg::NodeVisitor(),
+ m_textureCoordUnit(textureCoordUnit),
+ m_tangentAttribIndex(tangentAttribIndex),
+ m_bitangentAttribIndex(bitangentAttribIndex)
+{
+ setTraversalMode(osg::NodeVisitor::TRAVERSE_ALL_CHILDREN);
+}
+
+TangentSpaceGenerator::~TangentSpaceGenerator()
+{
+}
+
+void TangentSpaceGenerator::setBasisOrthonormality(bool orthonormal)
+{
+ m_createOrthonormalBasis = orthonormal;
+}
+bool TangentSpaceGenerator::getBasisOrthonormality()
+{
+ return m_createOrthonormalBasis;
+}
+
+void TangentSpaceGenerator::apply(osg::Geode& geode)
+{
+ for (unsigned int i = 0; i < geode.getNumDrawables(); i++)
+ {
+ osg::Geometry* geometry = geode.getDrawable(i)->asGeometry();
+ if (geometry)
+ {
+ generateTangentSpace(geometry, m_textureCoordUnit, m_tangentAttribIndex, m_bitangentAttribIndex,
+ m_createOrthonormalBasis);
+ }
+ }
+}
+
+void TangentSpaceGenerator::generateTangentSpace(osg::Geometry* geometry, int textureCoordUnit, int tangentAttribIndex,
+ int bitangentAttribIndex,
+ bool orthonormal)
+{
+
+ auto logger = SurgSim::Framework::Logger::getLogger("Graphics/TangetSpaceGenerator");
+
+ osg::Vec3Array* vertexArray = dynamic_cast<osg::Vec3Array*>(geometry->getVertexArray());
+ if (vertexArray == nullptr)
+ {
+ SURGSIM_LOG_WARNING(logger) << "No Vertices found, could not produce tangents.";
+ return;
+ }
+
+ osg::Vec3Array* normalArray = dynamic_cast<osg::Vec3Array*>(geometry->getNormalArray());
+ if (normalArray == nullptr || normalArray->size() != vertexArray->size())
+ {
+ SURGSIM_LOG_WARNING(logger) << "No Normals found, or mismatch in array sizes, could not produce tangents.";
+ return;
+ }
+
+ osg::Vec2Array* textureCoordArray =
+ dynamic_cast<osg::Vec2Array*>(geometry->getTexCoordArray(textureCoordUnit));
+ if (textureCoordArray == nullptr || textureCoordArray->size() != vertexArray->size())
+ {
+ SURGSIM_LOG_WARNING(logger)
+ << "No Texture Coordinates found, or mismatch in array sizes could not produce tangents.";
+ return;
+ }
+
+ bool didUpdateGeom = false;
+
+ osg::Vec4Array* tangentArray =
+ dynamic_cast<osg::Vec4Array*>(geometry->getVertexAttribArray(tangentAttribIndex));
+ if (tangentArray == nullptr || tangentArray->size() != vertexArray->size())
+ {
+ tangentArray = new osg::Vec4Array(vertexArray->size());
+ geometry->setVertexAttribArray(tangentAttribIndex, tangentArray);
+ geometry->setVertexAttribBinding(tangentAttribIndex, osg::Geometry::BIND_PER_VERTEX);
+ didUpdateGeom = true;
+ }
+
+ osg::Vec4Array* bitangentArray =
+ dynamic_cast<osg::Vec4Array*>(geometry->getVertexAttribArray(bitangentAttribIndex));
+ if (bitangentArray == nullptr || bitangentArray->size() != vertexArray->size())
+ {
+ bitangentArray = new osg::Vec4Array(vertexArray->size());
+ geometry->setVertexAttribArray(bitangentAttribIndex, bitangentArray);
+ geometry->setVertexAttribBinding(bitangentAttribIndex, osg::Geometry::BIND_PER_VERTEX);
+ didUpdateGeom = true;
+ }
+
+ osg::TriangleIndexFunctor<GenerateTangentSpaceTriangleIndexFunctor> tangentSpaceGenerator;
+ tangentSpaceGenerator.setBasisOrthonormality(orthonormal);
+ tangentSpaceGenerator.set(vertexArray, normalArray, textureCoordArray, tangentArray, bitangentArray);
+ tangentSpaceGenerator.reset();
+ geometry->accept(tangentSpaceGenerator);
+ tangentSpaceGenerator.orthogonalize();
+
+ if (didUpdateGeom)
+ {
+ // geometry->dirtyDisplayList();
+ geometry->setUseDisplayList(false);
+ geometry->dirtyBound();
+ geometry->setDataVariance(osg::Object::DYNAMIC);
+ }
+}
+
+
+}
+}
diff --git a/SurgSim/Graphics/TangentSpaceGenerator.h b/SurgSim/Graphics/TangentSpaceGenerator.h
new file mode 100644
index 0000000..b28c4a4
--- /dev/null
+++ b/SurgSim/Graphics/TangentSpaceGenerator.h
@@ -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.
+
+#ifndef SURGSIM_GRAPHICS_TANGENTSPACEGENERATOR_H
+#define SURGSIM_GRAPHICS_TANGENTSPACEGENERATOR_H
+
+#include <osg/NodeVisitor>
+#include <osg/Geode>
+#include <osg/Geometry>
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+/// Triangle index functor which calculates the tangent space basis vectors for the vertices
+/// of a geometry from texture coordinates
+class GenerateTangentSpaceTriangleIndexFunctor
+{
+public:
+ /// Constructor
+ GenerateTangentSpaceTriangleIndexFunctor();
+
+ /// Sets whether the three tangent space basis vectors are made to be orthonormal;
+ /// otherwise, each tangent is separately orthonormal to the normal, but not to each other
+ /// \param orthonormal Whether or not to create a fully orthonormal basis
+ void setBasisOrthonormality(bool orthonormal);
+
+ /// \return Gets whether the three tangent space basis vectors are made to be orthonormal; otherwise,
+ /// each tangent is separately orthonormal to the normal, but not to each other
+ bool getBasisOrthonormality();
+
+ /// Sets the arrays required to generate tangent space basis vectors
+ /// \param vertexArray Array containing vertex positions
+ /// \param normalArray Array containing vertex normals
+ /// \param textureCoordArray Array containing texture coordinates
+ /// \param tangentArray Array to store calculated tangents
+ /// \param bitangentArray Array to store calculated bitangents
+ void set(const osg::Vec3Array* vertexArray,
+ const osg::Vec3Array* normalArray,
+ const osg::Vec2Array* textureCoordArray,
+ osg::Vec4Array* tangentArray,
+ osg::Vec4Array* bitangentArray);
+
+ /// Orthogonalize and normalize the calculated tangent space basis vectors
+ void orthogonalize();
+
+ /// Resets all calculated tangent space basis vectors to 0.
+ void reset();
+
+ /// Calculates the triangle tangent space basis vectors and adds it to each adjacent vertex's tangent
+ // space basis vectors.
+ /// \param vertexIndex1 First triangle vertex index
+ /// \param vertexIndex2 Second triangle vertex index
+ /// \param vertexIndex3 Third triangle vertex index
+ void operator()(unsigned int vertexIndex1, unsigned int vertexIndex2, unsigned int vertexIndex3);
+
+private:
+ /// Array containing vertex positions
+ const osg::Vec3Array* m_vertexArray;
+
+ /// Array containing normals
+ const osg::Vec3Array* m_normalArray;
+
+ /// Array containing texture coordinates
+ const osg::Vec2Array* m_textureCoordArray;
+
+ /// Array storing calculated tangents
+ osg::Vec4Array* m_tangentArray;
+
+ /// Array storing calculated bitangents
+ osg::Vec4Array* m_bitangentArray;
+
+ /// Whether or not to create a fully orthonormal basis; otherwise, each tangent is separately orthonormal
+ /// to the normal, but not to each other
+ bool m_createOrthonormalBasis;
+};
+
+/// Node visitor which calculates the tangent space basis vectors from the texture coordinates of any
+/// geometry it encounters
+class TangentSpaceGenerator : public osg::NodeVisitor
+{
+public:
+ /// Constructor
+ /// \param textureCoordUnit Texture unit of texture coordinates to use for calculating the tangent space
+ /// \param tangentAttribIndex Index of the vertex attribute array to store the calculated tangents
+ /// \param bitangentAttribIndex Index of the vertex attribute array to store the calculated bitangents
+ TangentSpaceGenerator(int textureCoordUnit, int tangentAttribIndex, int bitangentAttribIndex);
+
+ /// Destructor
+ virtual ~TangentSpaceGenerator();
+
+ /// Sets whether the three tangent space basis vectors are made to be orthonormal;
+ /// otherwise, each tangent is separately orthonormal to the normal, but not to each other
+ /// \param orthonormal Whether or not to create a fully orthonormal basis
+ void setBasisOrthonormality(bool orthonormal);
+
+ /// Gets whether the three tangent space basis vectors are made to be orthonormal; otherwise,
+ /// each tangent is separately orthonormal to the normal, but not to each other
+ bool getBasisOrthonormality();
+
+ /// Generates tangent space vectors for all geometry in the geode
+ /// \param geode Geode to generate tangent space vectors
+ void apply(osg::Geode& geode) override; // NOLINT
+
+ /// Generates tangent space basis vectors for the geometry
+ /// \param geometry Geometry to generate normals
+ /// \param textureCoordUnit Texture unit of texture coordinates to use for calculating the tangent space
+ /// \param tangentAttribIndex Index of the vertex attribute array to store the calculated tangents
+ /// \param bitangentAttribIndex Index of the vertex attribute array to store the calculated bi-tangents
+ /// \param orthonormal Whether or not to create a fully orthonormal basis; otherwise, each tangent is separately
+ /// orthonormal to the normal, but not to each other
+ static void generateTangentSpace(osg::Geometry* geometry,
+ int textureCoordUnit,
+ int tangentAttribIndex,
+ int bitangentAttribIndex,
+ bool orthonormal);
+
+private:
+ /// Texture unit of texture coordinates to use for calculating the tangent space
+ int m_textureCoordUnit;
+
+ /// Index of the vertex attribute array to store the calculated tangents
+ int m_tangentAttribIndex;
+
+ /// Index of the vertex attribute array to store the calculated bitangents
+ int m_bitangentAttribIndex;
+
+ /// Whether or not to create a fully orthonormal basis; otherwise, each tangent is separately orthonormal to
+ /// the normal, but not to each other
+ bool m_createOrthonormalBasis;
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Graphics/TextRepresentation.cpp b/SurgSim/Graphics/TextRepresentation.cpp
new file mode 100644
index 0000000..052261b
--- /dev/null
+++ b/SurgSim/Graphics/TextRepresentation.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/TextRepresentation.h"
+#include "SurgSim/Graphics/Font.h"
+#include "SurgSim/Framework/Asset.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
+#include "SurgSim/Math/MathConvert.h"
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TextRepresentation::TextRepresentation(const std::string name) : Representation(name)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(TextRepresentation, std::shared_ptr<SurgSim::Framework::Asset>,
+ Font, getFont, setFont);
+
+ SURGSIM_ADD_SETTER(TextRepresentation, std::string, FontFileName, loadFont);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(TextRepresentation, std::string, Text, getText, setText);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(TextRepresentation, double, FontSize, getFontSize, setFontSize);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(TextRepresentation, SurgSim::DataStructures::OptionalValue<double>,
+ MaximumWidth, getOptionalMaximumWidth, setOptionalMaximumWidth);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SurgSim::Graphics::TextRepresentation, SurgSim::Math::Vector4d,
+ Color, getColor, setColor);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SurgSim::Graphics::TextRepresentation, bool, UseScreenSpace,
+ isUsingScreenSpace, setUseScreenSpace);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SurgSim::Graphics::TextRepresentation, bool, DrawBackground,
+ isDrawingBackground, setDrawBackground);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SurgSim::Graphics::TextRepresentation, Math::Vector4d, BackgroundColor,
+ getBackgroundColor, setBackgroundColor);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SurgSim::Graphics::TextRepresentation, double, BackgroundMargin,
+ getBackgroundMargin, setBackgroundMargin);
+
+}
+
+}
+}
+
diff --git a/SurgSim/Graphics/TextRepresentation.h b/SurgSim/Graphics/TextRepresentation.h
new file mode 100644
index 0000000..e8c58bd
--- /dev/null
+++ b/SurgSim/Graphics/TextRepresentation.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_TEXTREPRESENTATION_H
+#define SURGSIM_GRAPHICS_TEXTREPRESENTATION_H
+
+#include <memory>
+
+#include "SurgSim/Graphics/Representation.h"
+#include "SurgSim/DataStructures/OptionalValue.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Framework
+{
+class Asset;
+}
+namespace Graphics
+{
+
+class Font;
+
+/// A text to be displayed 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, has a default font but can also
+/// received a separate font.
+class TextRepresentation : public virtual Representation
+{
+public:
+ /// Constructor.
+ /// \param name The name.
+ explicit TextRepresentation(const std::string name);
+
+ /// Destructor
+ virtual ~TextRepresentation() {}
+
+ /// Load the font with the given file name, this will overwrite the current font
+ /// \param fileName the file that should be used to load the font
+ virtual void loadFont(const std::string& fileName) = 0;
+
+ /// Replace the current font with the one passed
+ /// \param font the new font to be used
+ virtual void setFont(std::shared_ptr<SurgSim::Framework::Asset> font) = 0;
+
+ /// \return the current font
+ virtual std::shared_ptr<Font> getFont() const = 0;
+
+ /// 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) const = 0;
+
+ /// Sets the text to be shown on the screen.
+ /// \param text the text to be used.
+ virtual void setText(const std::string& text) = 0;
+
+ /// \return the current text
+ virtual std::string getText() const = 0;
+
+ /// Sets a maximum width to the text display, the text should be broken up into
+ /// multiple lines if the it is longer than width, if no value is given, or values <= 0 are used
+ /// the width is assumed to be unlimited
+ /// \param width the value to be used.
+ virtual void setMaximumWidth(double width) = 0;
+
+ /// \return the maximum width to be used if <=0 then the width is considered unlimited
+ virtual double getMaximumWidth() = 0;
+
+ /// Set the vertical size of the font
+ /// \param size of the font
+ virtual void setFontSize(double size) = 0;
+
+ /// \return the current font size
+ virtual double getFontSize() const = 0;
+
+ /// Set the color for the text.
+ /// \param color the color to be used.
+ virtual void setColor(SurgSim::Math::Vector4d color) = 0;
+
+ /// \return the current text color
+ virtual SurgSim::Math::Vector4d getColor() const = 0;
+
+ /// If set to true all the coordinate values are in screen-space coordinates (i.e. pixels), world spaces otherwise
+ /// \note ScreeenSpace is default, when using world coordinates the font size has to be adjusted to an appropriately
+ /// small value (e.g. 0.01m)
+ /// \param value whether to use screen space or world space coordinates
+ virtual void setUseScreenSpace(bool value) = 0;
+
+ /// \return whether using screen space coordinates
+ virtual bool isUsingScreenSpace() const = 0;
+
+ /// Draw a filled background behind the text
+ /// \param value whether to fill the background behind the text
+ virtual void setDrawBackground(bool value) = 0;
+
+ /// \return whether the background is being drawn or not
+ virtual bool isDrawingBackground() const = 0;
+
+ /// Set the color of the background (if drawn)
+ /// \param color The color to use.
+ virtual void setBackgroundColor(Math::Vector4d color) = 0;
+
+ /// \return the color that the background uses
+ virtual Math::Vector4d getBackgroundColor() = 0;
+
+ /// Set the margin between background and text
+ /// \note If world coordinates is being used, this is also world coordinates and should be appropriately small,
+ /// if set too big, the background will filled the whole screen
+ /// \param margin the margin between text and background.
+ virtual void setBackgroundMargin(double margin) = 0;
+
+ /// \return the margin between the background and the text
+ virtual double getBackgroundMargin() const = 0;
+
+protected:
+ /// Optionally sets a maximum width to the text display, the text should be broken up into
+ /// multiple lines if the it is longer than width, if no value is given, or values <= 0 are used
+ /// the width is assumed to be unlimited
+ /// \param width The width to be used, if width has no value, the maximum width is assumed to be unlimited
+ virtual void setOptionalMaximumWidth(SurgSim::DataStructures::OptionalValue<double> width) = 0;
+
+ /// Get the current status of the width
+ /// \return the current maximum width, if there is no value the width is unlimited
+ virtual SurgSim::DataStructures::OptionalValue<double> getOptionalMaximumWidth() = 0;
+};
+
+}; // Graphics
+}; // SurgSim
+
+#endif // SURGSIM_GRAPHICS_TEXTREPRESENTATION_H
diff --git a/SurgSim/Graphics/Texture.cpp b/SurgSim/Graphics/Texture.cpp
new file mode 100644
index 0000000..f1bebbe
--- /dev/null
+++ b/SurgSim/Graphics/Texture.cpp
@@ -0,0 +1,38 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Texture.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+Texture::Texture() : m_isPointSprite(false)
+{
+}
+
+void Texture::setIsPointSprite(bool value)
+{
+ m_isPointSprite = value;
+}
+
+bool Texture::isPointSprite() const
+{
+ return m_isPointSprite;
+}
+
+} // namespace Graphics
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/Texture.h b/SurgSim/Graphics/Texture.h
index 41bcb61..09aeb00 100644
--- a/SurgSim/Graphics/Texture.h
+++ b/SurgSim/Graphics/Texture.h
@@ -28,6 +28,8 @@ namespace Graphics
class Texture
{
public:
+ Texture();
+
/// Destructor
virtual ~Texture()
{
@@ -40,6 +42,15 @@ public:
/// Removes the image from the texture
virtual void clearImage() = 0;
+
+ /// Set point sprite flag on/off
+ void setIsPointSprite(bool value);
+
+ /// Get point sprite flag state
+ bool isPointSprite() const;
+
+private:
+ bool m_isPointSprite;
};
}; // namespace Graphics
diff --git a/SurgSim/Graphics/UnitTests/CMakeLists.txt b/SurgSim/Graphics/UnitTests/CMakeLists.txt
index 19546da..483a914 100644
--- a/SurgSim/Graphics/UnitTests/CMakeLists.txt
+++ b/SurgSim/Graphics/UnitTests/CMakeLists.txt
@@ -22,9 +22,11 @@ set(UNIT_TEST_SOURCES
GroupTests.cpp
ManagerTests.cpp
MeshTests.cpp
+ OsgAxesRepresentationTests.cpp
OsgBoxRepresentationTests.cpp
OsgCameraTests.cpp
OsgCapsuleRepresentationTests.cpp
+ OsgCurveRepresentationTests.cpp
OsgCylinderRepresentationTests.cpp
OsgGroupTests.cpp
OsgLightTests.cpp
@@ -37,14 +39,16 @@ set(UNIT_TEST_SOURCES
OsgPlaneRepresentationTests.cpp
OsgPlaneTests.cpp
OsgPointCloudRepresentationTests.cpp
+ OsgProgramTests.cpp
OsgQuaternionConversionsTests.cpp
OsgRenderTargetTests.cpp
OsgRepresentationTests.cpp
OsgRigidTransformConversionsTests.cpp
OsgSceneryRepresentationTests.cpp
OsgScreenSpaceQuadTests.cpp
- OsgShaderTests.cpp
+ OsgSkeletonRepresentationTests.cpp
OsgSphereRepresentationTests.cpp
+ OsgTextRepresentationTests.cpp
OsgTexture1dTests.cpp
OsgTexture2dTests.cpp
OsgTexture3dTests.cpp
@@ -60,6 +64,7 @@ set(UNIT_TEST_SOURCES
OsgVectorFieldRepresentationTests.cpp
OsgViewElementTests.cpp
OsgViewTests.cpp
+ PaintBehaviorTests.cpp
RenderPassTests.cpp
ViewElementTests.cpp
ViewTests.cpp
@@ -77,9 +82,6 @@ configure_file(
)
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
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.frag b/SurgSim/Graphics/UnitTests/Data/OsgProgramTests/shader.frag
similarity index 100%
rename from SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.frag
rename to SurgSim/Graphics/UnitTests/Data/OsgProgramTests/shader.frag
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.geom b/SurgSim/Graphics/UnitTests/Data/OsgProgramTests/shader.geom
similarity index 100%
rename from SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.geom
rename to SurgSim/Graphics/UnitTests/Data/OsgProgramTests/shader.geom
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.vert b/SurgSim/Graphics/UnitTests/Data/OsgProgramTests/shader.vert
similarity index 100%
rename from SurgSim/Graphics/UnitTests/Data/OsgShaderTests/shader.vert
rename to SurgSim/Graphics/UnitTests/Data/OsgProgramTests/shader.vert
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgSkeletonRepresentationTests/rigged_cylinder.osgt b/SurgSim/Graphics/UnitTests/Data/OsgSkeletonRepresentationTests/rigged_cylinder.osgt
new file mode 100644
index 0000000..ce5e96b
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/Data/OsgSkeletonRepresentationTests/rigged_cylinder.osgt
@@ -0,0 +1,11743 @@
+#Ascii Scene
+#Version 100
+#Generator OpenSceneGraph 3.2.0
+
+osg::Group {
+ UniqueID 1
+ Name "Collada visual scene group"
+ StateSet TRUE {
+ osg::StateSet {
+ UniqueID 2
+ DataVariance STATIC
+ }
+ }
+ Children 1 {
+ osg::MatrixTransform {
+ UniqueID 3
+ Name "Armature"
+ StateSet TRUE {
+ osg::StateSet {
+ UniqueID 4
+ DataVariance STATIC
+ ModeList 1 {
+ GL_RESCALE_NORMAL ON|OVERRIDE
+ }
+ }
+ }
+ Children 1 {
+ osgAnimation::Skeleton {
+ UniqueID 5
+ DataVariance DYNAMIC
+ UpdateCallback TRUE {
+ osgAnimation::UpdateSkeleton {
+ UniqueID 6
+ }
+ }
+ Children 2 {
+ osgAnimation::Bone {
+ UniqueID 7
+ Name "Bone"
+ DataVariance DYNAMIC
+ UpdateCallback TRUE {
+ osgAnimation::UpdateBone {
+ UniqueID 8
+ Name "Bone"
+ StackedTransforms 1 {
+ osgAnimation::StackedMatrixElement {
+ UniqueID 9
+ Name "transform"
+ Matrix {
+ 1 0 0 0
+ 0 0 1 0
+ 0 -1 0 0
+ 0 0 0 1
+ }
+
+ }
+ }
+ }
+ }
+ Children 1 {
+ osgAnimation::Bone {
+ UniqueID 10
+ Name "Bone_002"
+ DataVariance DYNAMIC
+ UpdateCallback TRUE {
+ osgAnimation::UpdateBone {
+ UniqueID 11
+ Name "Bone_002"
+ StackedTransforms 1 {
+ osgAnimation::StackedMatrixElement {
+ UniqueID 12
+ Name "transform"
+ Matrix {
+ 1 0 0 0
+ 0 1 0 0
+ 0 0 1 0
+ 0 0.25 0 1
+ }
+
+ }
+ }
+ }
+ }
+ Children 1 {
+ osgAnimation::Bone {
+ UniqueID 13
+ Name "Bone_001"
+ DataVariance DYNAMIC
+ UpdateCallback TRUE {
+ osgAnimation::UpdateBone {
+ UniqueID 14
+ Name "Bone_001"
+ StackedTransforms 1 {
+ osgAnimation::StackedMatrixElement {
+ UniqueID 15
+ Name "transform"
+ Matrix {
+ 1 0 0 0
+ 0 1 0 0
+ 0 0 1 0
+ 0 0.25 0 1
+ }
+
+ }
+ }
+ }
+ }
+ Children 1 {
+ osgAnimation::Bone {
+ UniqueID 16
+ Name "Bone_003"
+ DataVariance DYNAMIC
+ UpdateCallback TRUE {
+ osgAnimation::UpdateBone {
+ UniqueID 17
+ Name "Bone_003"
+ StackedTransforms 1 {
+ osgAnimation::StackedMatrixElement {
+ UniqueID 18
+ Name "transform"
+ Matrix {
+ 1 0 0 0
+ 0 1 0 0
+ 0 0 1 0
+ 0 0.25 0 1
+ }
+
+ }
+ }
+ }
+ }
+ Matrix {
+ 1 0 0 0
+ 0 1 0 0
+ 0 0 1 0
+ 0 0.25 0 1
+ }
+
+ InvBindMatrixInSkeletonSpace {
+ 10 0 0 0
+ 0 0 -10 0
+ 0 10 0 0
+ 0 -0.25 0 1
+ }
+
+ }
+ }
+ Matrix {
+ 1 0 0 0
+ 0 1 0 0
+ 0 0 1 0
+ 0 0.25 0 1
+ }
+
+ InvBindMatrixInSkeletonSpace {
+ 10 0 0 0
+ 0 0 -10 0
+ 0 10 0 0
+ 0 0 0 1
+ }
+
+ }
+ }
+ Matrix {
+ 1 0 0 0
+ 0 1 0 0
+ 0 0 1 0
+ 0 0.25 0 1
+ }
+
+ InvBindMatrixInSkeletonSpace {
+ 10 0 0 0
+ 0 0 -10 0
+ 0 10 0 0
+ 0 0.25 0 1
+ }
+
+ }
+ }
+ Matrix {
+ 1 0 0 0
+ 0 0 1 0
+ 0 -1 0 0
+ 0 0 0 1
+ }
+
+ InvBindMatrixInSkeletonSpace {
+ 10 0 0 0
+ 0 0 -10 0
+ 0 10 0 0
+ 0 0.5 0 1
+ }
+
+ }
+ osg::Geode {
+ UniqueID 19
+ DataVariance DYNAMIC
+ Drawables 1 {
+ osgAnimation::RigGeometry {
+ UniqueID 20
+ DataVariance DYNAMIC
+ ComputeBoundingBoxCallback TRUE {
+ osg::ComputeBoundingBoxCallback {
+ UniqueID 21
+ }
+ }
+ SupportsDisplayList FALSE
+ UseDisplayList FALSE
+ UseVertexBufferObjects TRUE
+ UpdateCallback TRUE {
+ osg::UpdateCallback {
+ UniqueID 22
+ }
+ }
+ PrimitiveSetList 1 {
+ DrawElementsUInt GL_TRIANGLES 0 1428 {
+ 1141 653 12 1147
+ 676 20 1153 693
+ 28 1159 710 36
+ 1165 727 44 1171
+ 744 52 1177 761
+ 60 1183 779 68
+ 1189 796 76 1195
+ 813 84 762 246
+ 268 1201 847 4
+ 1207 830 92 303
+ 168 259 1213 852
+ 116 1219 858 371
+ 1225 448 103 1231
+ 876 141 1237 882
+ 377 1243 471 132
+ 1249 900 162 1255
+ 906 383 1261 488
+ 154 1267 924 186
+ 1273 930 389 1279
+ 506 178 1285 948
+ 208 1291 954 395
+ 1297 523 200 1303
+ 972 232 1309 978
+ 401 1315 540 222
+ 1321 996 253 1327
+ 1002 407 1333 557
+ 247 1339 1020 277
+ 1345 1026 413 1351
+ 574 269 1357 1044
+ 297 1363 1050 419
+ 1369 591 290 1375
+ 1068 322 1381 1074
+ 425 608 314 1086
+ 1392 1093 110 1398
+ 1099 431 1404 641
+ 363 1410 1117 348
+ 1416 1123 437 1422
+ 625 336 853 1142
+ 664 372 870 1143
+ 871 104 654 877
+ 1148 681 378 894
+ 1149 895 133 677
+ 901 1154 698 384
+ 918 1155 919 155
+ 694 925 1160 716
+ 390 942 1161 943
+ 179 711 949 1166
+ 732 396 966 1167
+ 967 201 728 973
+ 1172 750 402 990
+ 1173 991 223 745
+ 997 1178 767 408
+ 1014 1179 1015 248
+ 763 1021 1184 785
+ 414 1038 1185 1039
+ 270 780 1045 1190
+ 802 420 1062 1191
+ 1063 291 797 1069
+ 1196 819 426 1087
+ 1197 1088 315 814
+ 1094 1202 454 432
+ 1111 1203 1112 364
+ 848 1118 1208 835
+ 438 1135 1209 1136
+ 337 831 443 1214
+ 460 96 864 1215
+ 865 373 854 647
+ 1220 866 0 658
+ 1221 659 111 859
+ 860 1226 872 112
+ 455 1227 456 5
+ 449 466 1232 477
+ 122 888 1233 889
+ 379 878 670 1238
+ 890 8 461 1239
+ 462 117 883 884
+ 1244 896 118 665
+ 1245 666 13 472
+ 483 1250 494 147
+ 912 1251 913 385
+ 902 687 1256 914
+ 16 478 1257 479
+ 142 907 908 1262
+ 920 143 682 1263
+ 683 21 489 500
+ 1268 512 169 936
+ 1269 937 391 926
+ 704 1274 938 24
+ 495 1275 496 163
+ 931 932 1280 944
+ 164 699 1281 700
+ 29 507 518 1286
+ 528 192 960 1287
+ 961 397 950 722
+ 1292 962 32 513
+ 1293 514 187 955
+ 956 1298 968 188
+ 717 1299 718 37
+ 524 534 1304 546
+ 214 984 1305 985
+ 403 974 738 1310
+ 986 40 529 1311
+ 530 209 979 980
+ 1316 992 210 733
+ 1317 734 45 541
+ 552 1322 562 238
+ 1008 1323 1009 409
+ 998 756 1328 1010
+ 48 547 1329 548
+ 233 1003 1004 1334
+ 1016 234 751 1335
+ 752 53 558 568
+ 1340 579 260 1032
+ 1341 1033 415 1022
+ 773 1346 1034 56
+ 563 1347 564 254
+ 1027 1028 1352 1040
+ 255 768 1353 769
+ 61 575 585 1358
+ 596 283 1056 1359
+ 1057 421 1046 791
+ 1364 1058 64 580
+ 1365 581 278 1051
+ 1052 1370 1064 279
+ 786 1371 787 69
+ 592 602 1376 614
+ 304 1080 1377 1081
+ 427 1070 808 1382
+ 1082 72 597 1383
+ 598 298 1075 1076
+ 1387 1089 299 803
+ 1388 804 77 609
+ 636 1393 660 354
+ 1105 1394 1106 433
+ 1095 841 1399 1107
+ 88 630 1400 631
+ 349 1100 1101 1405
+ 1113 350 836 1406
+ 837 93 642 620
+ 1411 632 328 1129
+ 1412 1130 439 1119
+ 825 1417 1131 80
+ 615 1418 616 323
+ 1124 1125 1423 1137
+ 324 820 1424 821
+ 85 626 667 1144
+ 14 684 1150 22
+ 701 1156 30 719
+ 1162 38 735 1168
+ 46 753 1174 54
+ 770 1180 62 788
+ 1186 70 805 1192
+ 78 822 1198 86
+ 473 15 655 105
+ 450 365 451 6
+ 849 798 316 610
+ 338 643 832 87
+ 815 627 317 339
+ 816 79 799 611
+ 593 781 292 594
+ 71 782 576 764
+ 271 577 63 765
+ 559 746 249 560
+ 55 747 729 202
+ 542 543 47 730
+ 525 712 203 526
+ 39 713 695 156
+ 508 509 31 696
+ 678 134 490 491
+ 23 679 474 106
+ 135 714 180 204
+ 452 850 366 644
+ 94 833 817 340
+ 628 748 224 250
+ 157 136 341 137
+ 158 492 475 656
+ 107 457 1204 7
+ 159 181 510 783
+ 272 293 182 160
+ 342 318 273 225
+ 343 138 367 800
+ 294 319 295 274
+ 320 251 226 275
+ 205 183 227 321
+ 228 344 229 184
+ 345 206 230 544
+ 139 108 368 346
+ 369 645 838 1210
+ 95 637 1 648
+ 444 671 97 445
+ 9 672 467 688
+ 123 468 17 689
+ 484 705 148 485
+ 25 706 723 193
+ 501 502 33 724
+ 519 739 194 520
+ 41 740 757 239
+ 535 536 49 758
+ 553 774 240 554
+ 57 775 792 284
+ 569 570 65 793
+ 809 305 586 587
+ 73 810 826 329
+ 603 604 81 827
+ 621 842 330 622
+ 89 843 355 638
+ 649 331 306 605
+ 98 124 356 776
+ 261 241 463 1216
+ 119 125 307 357
+ 332 358 308 741
+ 215 195 309 285
+ 588 690 149 126
+ 196 170 503 286
+ 262 571 650 99
+ 359 242 216 537
+ 150 171 127 197
+ 217 172 243 263
+ 218 287 310 264
+ 707 173 151 333
+ 844 360 128 174
+ 311 175 219 265
+ 100 673 129 867
+ 1222 374 873 1228
+ 109 480 1234 144
+ 891 1240 380 897
+ 1246 140 497 1252
+ 165 915 1258 386
+ 921 1264 161 515
+ 1270 189 939 1276
+ 392 945 1282 185
+ 531 1288 211 963
+ 1294 398 969 1300
+ 207 549 1306 235
+ 987 1312 404 993
+ 1318 231 565 1324
+ 256 1011 1330 410
+ 1017 1336 252 582
+ 1342 280 1035 1348
+ 416 1041 1354 276
+ 599 1360 300 1059
+ 1366 422 1065 1372
+ 296 617 1378 325
+ 1083 1384 428 1389
+ 612 1090 661 1395
+ 113 1108 1401 434
+ 1114 1407 370 633
+ 1413 351 1132 1419
+ 440 1138 1425 347
+ 120 855 668 856
+ 375 1145 1146 874
+ 657 145 879 685
+ 880 381 1151 1152
+ 898 680 166 903
+ 702 904 387 1157
+ 1158 922 697 190
+ 927 720 928 393
+ 1163 1164 946 715
+ 212 951 736 952
+ 399 1169 1170 970
+ 731 236 975 754
+ 976 405 1175 1176
+ 994 749 257 999
+ 771 1000 411 1181
+ 1182 1018 766 281
+ 1023 789 1024 417
+ 1187 1188 1042 784
+ 301 1047 806 1048
+ 423 1193 1194 1066
+ 801 326 1071 823
+ 1072 429 1199 1200
+ 1091 818 114 1096
+ 458 1097 435 1205
+ 1206 1115 851 352
+ 1120 839 1121 441
+ 1211 1212 1139 834
+ 10 446 464 447
+ 101 1217 1218 868
+ 857 102 651 869
+ 652 2 1223 1224
+ 662 861 376 862
+ 875 863 115 1229
+ 1230 459 453 18
+ 469 481 470 130
+ 1235 1236 892 881
+ 131 674 893 675
+ 11 1241 1242 465
+ 885 382 886 899
+ 887 121 1247 1248
+ 669 476 26 486
+ 498 487 152 1253
+ 1254 916 905 153
+ 691 917 692 19
+ 1259 1260 482 909
+ 388 910 923 911
+ 146 1265 1266 686
+ 493 34 504 516
+ 505 176 1271 1272
+ 940 929 177 708
+ 941 709 27 1277
+ 1278 499 933 394
+ 934 947 935 167
+ 1283 1284 703 511
+ 42 521 532 522
+ 198 1289 1290 964
+ 953 199 725 965
+ 726 35 1295 1296
+ 517 957 400 958
+ 971 959 191 1301
+ 1302 721 527 50
+ 538 550 539 220
+ 1307 1308 988 977
+ 221 742 989 743
+ 43 1313 1314 533
+ 981 406 982 995
+ 983 213 1319 1320
+ 737 545 58 555
+ 566 556 244 1325
+ 1326 1012 1001 245
+ 759 1013 760 51
+ 1331 1332 551 1005
+ 412 1006 1019 1007
+ 237 1337 1338 755
+ 561 66 572 583
+ 573 266 1343 1344
+ 1036 1025 267 777
+ 1037 778 59 1349
+ 1350 567 1029 418
+ 1030 1043 1031 258
+ 1355 1356 772 578
+ 74 589 600 590
+ 288 1361 1362 1060
+ 1049 289 794 1061
+ 795 67 1367 1368
+ 584 1053 424 1054
+ 1067 1055 282 1373
+ 1374 790 595 82
+ 606 618 607 312
+ 1379 1380 1084 1073
+ 313 811 1085 812
+ 75 1385 1386 601
+ 1077 430 1078 1092
+ 1079 302 1390 1391
+ 807 613 3 639
+ 663 640 361 1396
+ 1397 1109 1098 362
+ 845 1110 846 90
+ 1402 1403 634 1102
+ 436 1103 1116 1104
+ 353 1408 1409 840
+ 646 91 623 635
+ 624 334 1414 1415
+ 1133 1122 335 828
+ 1134 829 83 1420
+ 1421 619 1126 442
+ 1127 1140 1128 327
+ 1426 1427 824 629
+ }
+
+ }
+ VertexData {
+ Array TRUE ArrayID 1 Vec3fArray 1428 {
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 0.0499999
+ 0 0.0499999 0.0499999
+ 0 0.0499999 0.0499999
+ 0 0.0499999 0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 0.0499999
+ 0.0499999 0 0.0499999
+ 0.0499999 0 0.0499999
+ 0.0499999 0 0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 0.0499999
+ 0 -0.0499999 0.0499999
+ 0 -0.0499999 0.0499999
+ 0 -0.0499999 0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 0.0499999
+ -0.0499999 0 0.0499999
+ -0.0499999 0 0.0499999
+ -0.0499999 0 0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 0.0499999
+ -0.025 0.0433012 0.0499999
+ -0.025 0.0433012 0.0499999
+ -0.025 0.0433012 0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ }
+ Indices FALSE
+ Binding BIND_PER_VERTEX
+ Normalize 0
+ }
+ NormalData {
+ Array TRUE ArrayID 2 Vec3fArray 1428 {
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ -0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ -0.707107 -0.707107 0
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ -0.965926 -0.25882 0
+ 0 0 -1
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ 0 0 1
+ -0.965926 0.258819 0
+ 0 0 -1
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ 0 0 1
+ -0.707107 0.707106 0
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ 0 0 1
+ -0.258819 0.965926 0
+ 0 0 -1
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ 0 0 1
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ 0 0 -1
+ -0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ 0 0 1
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ 0 0 -1
+ -0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ 0 0 1
+ 0 0 1
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 -1
+ 0 0 -1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 1
+ 0 0 1
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0 0 1
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ -0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 -1
+ 0 0 -1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 1
+ 0 0 1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ 0 0 1
+ 0 0 1
+ -0.25882 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ }
+ Indices FALSE
+ Binding BIND_PER_VERTEX
+ Normalize 0
+ }
+ InfluenceMap 4 {
+ VertexInfluence "Bone" 1146 {
+ 0 0.34816
+ 1 0.34816
+ 2 0.34816
+ 3 0.34816
+ 8 0.367419
+ 9 0.367419
+ 10 0.367419
+ 11 0.367419
+ 16 0.364055
+ 17 0.364055
+ 18 0.364055
+ 19 0.364055
+ 24 0.379784
+ 25 0.379784
+ 26 0.379784
+ 27 0.379784
+ 32 0.342064
+ 33 0.342064
+ 34 0.342064
+ 35 0.342064
+ 40 0.355754
+ 41 0.355754
+ 42 0.355754
+ 43 0.355754
+ 48 0.340748
+ 49 0.340748
+ 50 0.340748
+ 51 0.340748
+ 56 0.337092
+ 57 0.337092
+ 58 0.337092
+ 59 0.337092
+ 64 0.305945
+ 65 0.305945
+ 66 0.305945
+ 67 0.305945
+ 72 0.344481
+ 73 0.344481
+ 74 0.344481
+ 75 0.344481
+ 80 0.372747
+ 81 0.372747
+ 82 0.372747
+ 83 0.372747
+ 88 0.386408
+ 89 0.386408
+ 90 0.386408
+ 91 0.386408
+ 96 0.358124
+ 97 0.358124
+ 98 0.358124
+ 99 0.358124
+ 100 0.358124
+ 101 0.358124
+ 102 0.358124
+ 110 0.096263
+ 111 0.096263
+ 112 0.096263
+ 113 0.096263
+ 114 0.096263
+ 115 0.096263
+ 116 0.0964708
+ 117 0.0964708
+ 118 0.0964708
+ 119 0.0964708
+ 120 0.0964708
+ 121 0.0964708
+ 122 0.378926
+ 123 0.378926
+ 124 0.378926
+ 125 0.378926
+ 126 0.378926
+ 127 0.378926
+ 128 0.378926
+ 129 0.378926
+ 130 0.378926
+ 131 0.378926
+ 141 0.0968001
+ 142 0.0968001
+ 143 0.0968001
+ 144 0.0968001
+ 145 0.0968001
+ 146 0.0968001
+ 147 0.38288
+ 148 0.38288
+ 149 0.38288
+ 150 0.38288
+ 151 0.38288
+ 152 0.38288
+ 153 0.38288
+ 162 0.0968163
+ 163 0.0968163
+ 164 0.0968163
+ 165 0.0968163
+ 166 0.0968163
+ 167 0.0968163
+ 168 0.380374
+ 169 0.380374
+ 170 0.380374
+ 171 0.380374
+ 172 0.380374
+ 173 0.380374
+ 174 0.380374
+ 175 0.380374
+ 176 0.380374
+ 177 0.380374
+ 186 0.0958862
+ 187 0.0958862
+ 188 0.0958862
+ 189 0.0958862
+ 190 0.0958862
+ 191 0.0958862
+ 192 0.349886
+ 193 0.349886
+ 194 0.349886
+ 195 0.349886
+ 196 0.349886
+ 197 0.349886
+ 198 0.349886
+ 199 0.349886
+ 208 0.0954361
+ 209 0.0954361
+ 210 0.0954361
+ 211 0.0954361
+ 212 0.0954361
+ 213 0.0954361
+ 214 0.363129
+ 215 0.363129
+ 216 0.363129
+ 217 0.363129
+ 218 0.363129
+ 219 0.363129
+ 220 0.363129
+ 221 0.363129
+ 232 0.0949529
+ 233 0.0949529
+ 234 0.0949529
+ 235 0.0949529
+ 236 0.0949529
+ 237 0.0949529
+ 238 0.347336
+ 239 0.347336
+ 240 0.347336
+ 241 0.347336
+ 242 0.347336
+ 243 0.347336
+ 244 0.347336
+ 245 0.347336
+ 253 0.0943446
+ 254 0.0943446
+ 255 0.0943446
+ 256 0.0943446
+ 257 0.0943446
+ 258 0.0943446
+ 259 0.339065
+ 260 0.339065
+ 261 0.339065
+ 262 0.339065
+ 263 0.339065
+ 264 0.339065
+ 265 0.339065
+ 266 0.339065
+ 267 0.339065
+ 277 0.0938559
+ 278 0.0938559
+ 279 0.0938559
+ 280 0.0938559
+ 281 0.0938559
+ 282 0.0938559
+ 283 0.324145
+ 284 0.324145
+ 285 0.324145
+ 286 0.324145
+ 287 0.324145
+ 288 0.324145
+ 289 0.324145
+ 297 0.09501
+ 298 0.09501
+ 299 0.09501
+ 300 0.09501
+ 301 0.09501
+ 302 0.09501
+ 303 0.378378
+ 304 0.378378
+ 305 0.378378
+ 306 0.378378
+ 307 0.378378
+ 308 0.378378
+ 309 0.378378
+ 310 0.378378
+ 311 0.378378
+ 312 0.378378
+ 313 0.378378
+ 322 0.0965146
+ 323 0.0965146
+ 324 0.0965146
+ 325 0.0965146
+ 326 0.0965146
+ 327 0.0965146
+ 328 0.388315
+ 329 0.388315
+ 330 0.388315
+ 331 0.388315
+ 332 0.388315
+ 333 0.388315
+ 334 0.388315
+ 335 0.388315
+ 348 0.0969059
+ 349 0.0969059
+ 350 0.0969059
+ 351 0.0969059
+ 352 0.0969059
+ 353 0.0969059
+ 354 0.365767
+ 355 0.365767
+ 356 0.365767
+ 357 0.365767
+ 358 0.365767
+ 359 0.365767
+ 360 0.365767
+ 361 0.365767
+ 362 0.365767
+ 371 0.0958394
+ 372 0.0958394
+ 373 0.0958394
+ 374 0.0958394
+ 375 0.0958394
+ 376 0.0958394
+ 377 0.0962926
+ 378 0.0962926
+ 379 0.0962926
+ 380 0.0962926
+ 381 0.0962926
+ 382 0.0962926
+ 383 0.0965003
+ 384 0.0965003
+ 385 0.0965003
+ 386 0.0965003
+ 387 0.0965003
+ 388 0.0965003
+ 389 0.0960521
+ 390 0.0960521
+ 391 0.0960521
+ 392 0.0960521
+ 393 0.0960521
+ 394 0.0960521
+ 395 0.0951623
+ 396 0.0951623
+ 397 0.0951623
+ 398 0.0951623
+ 399 0.0951623
+ 400 0.0951623
+ 401 0.0948754
+ 402 0.0948754
+ 403 0.0948754
+ 404 0.0948754
+ 405 0.0948754
+ 406 0.0948754
+ 407 0.0942636
+ 408 0.0942636
+ 409 0.0942636
+ 410 0.0942636
+ 411 0.0942636
+ 412 0.0942636
+ 413 0.0936536
+ 414 0.0936536
+ 415 0.0936536
+ 416 0.0936536
+ 417 0.0936536
+ 418 0.0936536
+ 419 0.0938172
+ 420 0.0938172
+ 421 0.0938172
+ 422 0.0938172
+ 423 0.0938172
+ 424 0.0938172
+ 425 0.0955049
+ 426 0.0955049
+ 427 0.0955049
+ 428 0.0955049
+ 429 0.0955049
+ 430 0.0955049
+ 431 0.096215
+ 432 0.096215
+ 433 0.096215
+ 434 0.096215
+ 435 0.096215
+ 436 0.096215
+ 437 0.0964742
+ 438 0.0964742
+ 439 0.0964742
+ 440 0.0964742
+ 441 0.0964742
+ 442 0.0964742
+ 443 0.354635
+ 444 0.354635
+ 445 0.354635
+ 446 0.354635
+ 447 0.354635
+ 454 0.00430191
+ 455 0.00430191
+ 456 0.00430191
+ 457 0.00430191
+ 458 0.00430191
+ 459 0.00430191
+ 460 0.374686
+ 461 0.374686
+ 462 0.374686
+ 463 0.374686
+ 464 0.374686
+ 465 0.374686
+ 466 0.357486
+ 467 0.357486
+ 468 0.357486
+ 469 0.357486
+ 470 0.357486
+ 477 0.375587
+ 478 0.375587
+ 479 0.375587
+ 480 0.375587
+ 481 0.375587
+ 482 0.375587
+ 483 0.373781
+ 484 0.373781
+ 485 0.373781
+ 486 0.373781
+ 487 0.373781
+ 494 0.376868
+ 495 0.376868
+ 496 0.376868
+ 497 0.376868
+ 498 0.376868
+ 499 0.376868
+ 500 0.354076
+ 501 0.354076
+ 502 0.354076
+ 503 0.354076
+ 504 0.354076
+ 505 0.354076
+ 512 0.371253
+ 513 0.371253
+ 514 0.371253
+ 515 0.371253
+ 516 0.371253
+ 517 0.371253
+ 518 0.344402
+ 519 0.344402
+ 520 0.344402
+ 521 0.344402
+ 522 0.344402
+ 528 0.371254
+ 529 0.371254
+ 530 0.371254
+ 531 0.371254
+ 532 0.371254
+ 533 0.371254
+ 534 0.337368
+ 535 0.337368
+ 536 0.337368
+ 537 0.337368
+ 538 0.337368
+ 539 0.337368
+ 546 0.368766
+ 547 0.368766
+ 548 0.368766
+ 549 0.368766
+ 550 0.368766
+ 551 0.368766
+ 552 0.32735
+ 553 0.32735
+ 554 0.32735
+ 555 0.32735
+ 556 0.32735
+ 562 0.366421
+ 563 0.366421
+ 564 0.366421
+ 565 0.366421
+ 566 0.366421
+ 567 0.366421
+ 568 0.307046
+ 569 0.307046
+ 570 0.307046
+ 571 0.307046
+ 572 0.307046
+ 573 0.307046
+ 579 0.362455
+ 580 0.362455
+ 581 0.362455
+ 582 0.362455
+ 583 0.362455
+ 584 0.362455
+ 585 0.319194
+ 586 0.319194
+ 587 0.319194
+ 588 0.319194
+ 589 0.319194
+ 590 0.319194
+ 596 0.368885
+ 597 0.368885
+ 598 0.368885
+ 599 0.368885
+ 600 0.368885
+ 601 0.368885
+ 602 0.368849
+ 603 0.368849
+ 604 0.368849
+ 605 0.368849
+ 606 0.368849
+ 607 0.368849
+ 614 0.375813
+ 615 0.375813
+ 616 0.375813
+ 617 0.375813
+ 618 0.375813
+ 619 0.375813
+ 620 0.38096
+ 621 0.38096
+ 622 0.38096
+ 623 0.38096
+ 624 0.38096
+ 630 0.377718
+ 631 0.377718
+ 632 0.377718
+ 633 0.377718
+ 634 0.377718
+ 635 0.377718
+ 636 0.359173
+ 637 0.359173
+ 638 0.359173
+ 639 0.359173
+ 640 0.359173
+ 647 0.313242
+ 648 0.313242
+ 649 0.313242
+ 650 0.313242
+ 651 0.313242
+ 652 0.313242
+ 658 0.372815
+ 659 0.372815
+ 660 0.372815
+ 661 0.372815
+ 662 0.372815
+ 663 0.372815
+ 664 0.00514126
+ 665 0.00514126
+ 666 0.00514126
+ 667 0.00514126
+ 668 0.00514126
+ 669 0.00514126
+ 670 0.357776
+ 671 0.357776
+ 672 0.357776
+ 673 0.357776
+ 674 0.357776
+ 675 0.357776
+ 681 0.00567305
+ 682 0.00567305
+ 683 0.00567305
+ 684 0.00567305
+ 685 0.00567305
+ 686 0.00567305
+ 687 0.347794
+ 688 0.347794
+ 689 0.347794
+ 690 0.347794
+ 691 0.347794
+ 692 0.347794
+ 698 0.00545162
+ 699 0.00545162
+ 700 0.00545162
+ 701 0.00545162
+ 702 0.00545162
+ 703 0.00545162
+ 704 0.364472
+ 705 0.364472
+ 706 0.364472
+ 707 0.364472
+ 708 0.364472
+ 709 0.364472
+ 716 0.00471312
+ 717 0.00471312
+ 718 0.00471312
+ 719 0.00471312
+ 720 0.00471312
+ 721 0.00471312
+ 722 0.305746
+ 723 0.305746
+ 724 0.305746
+ 725 0.305746
+ 726 0.305746
+ 732 0.00370216
+ 733 0.00370216
+ 734 0.00370216
+ 735 0.00370216
+ 736 0.00370216
+ 737 0.00370216
+ 738 0.343717
+ 739 0.343717
+ 740 0.343717
+ 741 0.343717
+ 742 0.343717
+ 743 0.343717
+ 750 0.00352383
+ 751 0.00352383
+ 752 0.00352383
+ 753 0.00352383
+ 754 0.00352383
+ 755 0.00352383
+ 756 0.319341
+ 757 0.319341
+ 758 0.319341
+ 759 0.319341
+ 760 0.319341
+ 767 0.00311261
+ 768 0.00311261
+ 769 0.00311261
+ 770 0.00311261
+ 771 0.00311261
+ 772 0.00311261
+ 773 0.321807
+ 774 0.321807
+ 775 0.321807
+ 776 0.321807
+ 777 0.321807
+ 778 0.321807
+ 785 0.00276154
+ 786 0.00276154
+ 787 0.00276154
+ 788 0.00276154
+ 789 0.00276154
+ 790 0.00276154
+ 791 0.277267
+ 792 0.277267
+ 793 0.277267
+ 794 0.277267
+ 795 0.277267
+ 802 0.00300527
+ 803 0.00300527
+ 804 0.00300527
+ 805 0.00300527
+ 806 0.00300527
+ 807 0.00300527
+ 808 0.345333
+ 809 0.345333
+ 810 0.345333
+ 811 0.345333
+ 812 0.345333
+ 819 0.00410753
+ 820 0.00410753
+ 821 0.00410753
+ 822 0.00410753
+ 823 0.00410753
+ 824 0.00410753
+ 825 0.354611
+ 826 0.354611
+ 827 0.354611
+ 828 0.354611
+ 829 0.354611
+ 835 0.00450843
+ 836 0.00450843
+ 837 0.00450843
+ 838 0.00450843
+ 839 0.00450843
+ 840 0.00450843
+ 841 0.371096
+ 842 0.371096
+ 843 0.371096
+ 844 0.371096
+ 845 0.371096
+ 846 0.371096
+ 852 0.0961208
+ 853 0.0961208
+ 854 0.0961208
+ 855 0.0961208
+ 856 0.0961208
+ 857 0.0961208
+ 858 0.0959629
+ 859 0.0959629
+ 860 0.0959629
+ 861 0.0959629
+ 862 0.0959629
+ 863 0.0959629
+ 864 0.373473
+ 865 0.373473
+ 866 0.373473
+ 867 0.373473
+ 868 0.373473
+ 869 0.373473
+ 870 0.00435418
+ 871 0.00435418
+ 872 0.00435418
+ 873 0.00435418
+ 874 0.00435418
+ 875 0.00435418
+ 876 0.0965277
+ 877 0.0965277
+ 878 0.0965277
+ 879 0.0965277
+ 880 0.0965277
+ 881 0.0965277
+ 882 0.0963668
+ 883 0.0963668
+ 884 0.0963668
+ 885 0.0963668
+ 886 0.0963668
+ 887 0.0963668
+ 888 0.376317
+ 889 0.376317
+ 890 0.376317
+ 891 0.376317
+ 892 0.376317
+ 893 0.376317
+ 894 0.00527549
+ 895 0.00527549
+ 896 0.00527549
+ 897 0.00527549
+ 898 0.00527549
+ 899 0.00527549
+ 900 0.0966689
+ 901 0.0966689
+ 902 0.0966689
+ 903 0.0966689
+ 904 0.0966689
+ 905 0.0966689
+ 906 0.0966253
+ 907 0.0966253
+ 908 0.0966253
+ 909 0.0966253
+ 910 0.0966253
+ 911 0.0966253
+ 912 0.377324
+ 913 0.377324
+ 914 0.377324
+ 915 0.377324
+ 916 0.377324
+ 917 0.377324
+ 918 0.00535691
+ 919 0.00535691
+ 920 0.00535691
+ 921 0.00535691
+ 922 0.00535691
+ 923 0.00535691
+ 924 0.0959472
+ 925 0.0959472
+ 926 0.0959472
+ 927 0.0959472
+ 928 0.0959472
+ 929 0.0959472
+ 930 0.0964418
+ 931 0.0964418
+ 932 0.0964418
+ 933 0.0964418
+ 934 0.0964418
+ 935 0.0964418
+ 936 0.375737
+ 937 0.375737
+ 938 0.375737
+ 939 0.375737
+ 940 0.375737
+ 941 0.375737
+ 942 0.00496679
+ 943 0.00496679
+ 944 0.00496679
+ 945 0.00496679
+ 946 0.00496679
+ 947 0.00496679
+ 948 0.0952691
+ 949 0.0952691
+ 950 0.0952691
+ 951 0.0952691
+ 952 0.0952691
+ 953 0.0952691
+ 954 0.0954447
+ 955 0.0954447
+ 956 0.0954447
+ 957 0.0954447
+ 958 0.0954447
+ 959 0.0954447
+ 960 0.370982
+ 961 0.370982
+ 962 0.370982
+ 963 0.370982
+ 964 0.370982
+ 965 0.370982
+ 966 0.00385237
+ 967 0.00385237
+ 968 0.00385237
+ 969 0.00385237
+ 970 0.00385237
+ 971 0.00385237
+ 972 0.0948983
+ 973 0.0948983
+ 974 0.0948983
+ 975 0.0948983
+ 976 0.0948983
+ 977 0.0948983
+ 978 0.095146
+ 979 0.095146
+ 980 0.095146
+ 981 0.095146
+ 982 0.095146
+ 983 0.095146
+ 984 0.371512
+ 985 0.371512
+ 986 0.371512
+ 987 0.371512
+ 988 0.371512
+ 989 0.371512
+ 990 0.00338936
+ 991 0.00338936
+ 992 0.00338936
+ 993 0.00338936
+ 994 0.00338936
+ 995 0.00338936
+ 996 0.0942812
+ 997 0.0942812
+ 998 0.0942812
+ 999 0.0942812
+ 1000 0.0942812
+ 1001 0.0942812
+ 1002 0.0945738
+ 1003 0.0945738
+ 1004 0.0945738
+ 1005 0.0945738
+ 1006 0.0945738
+ 1007 0.0945738
+ 1008 0.368375
+ 1009 0.368375
+ 1010 0.368375
+ 1011 0.368375
+ 1012 0.368375
+ 1013 0.368375
+ 1014 0.00306702
+ 1015 0.00306702
+ 1016 0.00306702
+ 1017 0.00306702
+ 1018 0.00306702
+ 1019 0.00306702
+ 1020 0.0936896
+ 1021 0.0936896
+ 1022 0.0936896
+ 1023 0.0936896
+ 1024 0.0936896
+ 1025 0.0936896
+ 1026 0.0939704
+ 1027 0.0939704
+ 1028 0.0939704
+ 1029 0.0939704
+ 1030 0.0939704
+ 1031 0.0939704
+ 1032 0.365541
+ 1033 0.365541
+ 1034 0.365541
+ 1035 0.365541
+ 1036 0.365541
+ 1037 0.365541
+ 1038 0.00270522
+ 1039 0.00270522
+ 1040 0.00270522
+ 1041 0.00270522
+ 1042 0.00270522
+ 1043 0.00270522
+ 1044 0.0943534
+ 1045 0.0943534
+ 1046 0.0943534
+ 1047 0.0943534
+ 1048 0.0943534
+ 1049 0.0943534
+ 1050 0.0937264
+ 1051 0.0937264
+ 1052 0.0937264
+ 1053 0.0937264
+ 1054 0.0937264
+ 1055 0.0937264
+ 1056 0.364966
+ 1057 0.364966
+ 1058 0.364966
+ 1059 0.364966
+ 1060 0.364966
+ 1061 0.364966
+ 1062 0.00244218
+ 1063 0.00244218
+ 1064 0.00244218
+ 1065 0.00244218
+ 1066 0.00244218
+ 1067 0.00244218
+ 1068 0.0960259
+ 1069 0.0960259
+ 1070 0.0960259
+ 1071 0.0960259
+ 1072 0.0960259
+ 1073 0.0960259
+ 1074 0.0952469
+ 1075 0.0952469
+ 1076 0.0952469
+ 1077 0.0952469
+ 1078 0.0952469
+ 1079 0.0952469
+ 1080 0.374353
+ 1081 0.374353
+ 1082 0.374353
+ 1083 0.374353
+ 1084 0.374353
+ 1085 0.374353
+ 1086 0.00335479
+ 1087 0.00335479
+ 1088 0.00335479
+ 1089 0.00335479
+ 1090 0.00335479
+ 1091 0.00335479
+ 1092 0.00335479
+ 1093 0.0962022
+ 1094 0.0962022
+ 1095 0.0962022
+ 1096 0.0962022
+ 1097 0.0962022
+ 1098 0.0962022
+ 1099 0.096563
+ 1100 0.096563
+ 1101 0.096563
+ 1102 0.096563
+ 1103 0.096563
+ 1104 0.096563
+ 1105 0.37611
+ 1106 0.37611
+ 1107 0.37611
+ 1108 0.37611
+ 1109 0.37611
+ 1110 0.37611
+ 1111 0.0041514
+ 1112 0.0041514
+ 1113 0.0041514
+ 1114 0.0041514
+ 1115 0.0041514
+ 1116 0.0041514
+ 1117 0.0967142
+ 1118 0.0967142
+ 1119 0.0967142
+ 1120 0.0967142
+ 1121 0.0967142
+ 1122 0.0967142
+ 1123 0.0964939
+ 1124 0.0964939
+ 1125 0.0964939
+ 1126 0.0964939
+ 1127 0.0964939
+ 1128 0.0964939
+ 1129 0.378176
+ 1130 0.378176
+ 1131 0.378176
+ 1132 0.378176
+ 1133 0.378176
+ 1134 0.378176
+ 1135 0.0041101
+ 1136 0.0041101
+ 1137 0.0041101
+ 1138 0.0041101
+ 1139 0.0041101
+ 1140 0.0041101
+ 1141 0.00472903
+ 1142 0.00472903
+ 1143 0.00472903
+ 1144 0.00472903
+ 1145 0.00472903
+ 1146 0.00472903
+ 1147 0.00546944
+ 1148 0.00546944
+ 1149 0.00546944
+ 1150 0.00546944
+ 1151 0.00546944
+ 1152 0.00546944
+ 1153 0.00535601
+ 1154 0.00535601
+ 1155 0.00535601
+ 1156 0.00535601
+ 1157 0.00535601
+ 1158 0.00535601
+ 1159 0.00483459
+ 1160 0.00483459
+ 1161 0.00483459
+ 1162 0.00483459
+ 1163 0.00483459
+ 1164 0.00483459
+ 1165 0.00366831
+ 1166 0.00366831
+ 1167 0.00366831
+ 1168 0.00366831
+ 1169 0.00366831
+ 1170 0.00366831
+ 1171 0.00344825
+ 1172 0.00344825
+ 1173 0.00344825
+ 1174 0.00344825
+ 1175 0.00344825
+ 1176 0.00344825
+ 1177 0.00304955
+ 1178 0.00304955
+ 1179 0.00304955
+ 1180 0.00304955
+ 1181 0.00304955
+ 1182 0.00304955
+ 1183 0.00270885
+ 1184 0.00270885
+ 1185 0.00270885
+ 1186 0.00270885
+ 1187 0.00270885
+ 1188 0.00270885
+ 1189 0.00261372
+ 1190 0.00261372
+ 1191 0.00261372
+ 1192 0.00261372
+ 1193 0.00261372
+ 1194 0.00261372
+ 1195 0.00375396
+ 1196 0.00375396
+ 1197 0.00375396
+ 1198 0.00375396
+ 1199 0.00375396
+ 1200 0.00375396
+ 1201 0.00419354
+ 1202 0.00419354
+ 1203 0.00419354
+ 1204 0.00419354
+ 1205 0.00419354
+ 1206 0.00419354
+ 1207 0.00432968
+ 1208 0.00432968
+ 1209 0.00432968
+ 1210 0.00432968
+ 1211 0.00432968
+ 1212 0.00432968
+ 1213 0.374055
+ 1214 0.374055
+ 1215 0.374055
+ 1216 0.374055
+ 1217 0.374055
+ 1218 0.374055
+ 1219 0.372065
+ 1220 0.372065
+ 1221 0.372065
+ 1222 0.372065
+ 1223 0.372065
+ 1224 0.372065
+ 1225 0.00420994
+ 1226 0.00420994
+ 1227 0.00420994
+ 1228 0.00420994
+ 1229 0.00420994
+ 1230 0.00420994
+ 1231 0.375788
+ 1232 0.375788
+ 1233 0.375788
+ 1234 0.375788
+ 1235 0.375788
+ 1236 0.375788
+ 1237 0.375401
+ 1238 0.375401
+ 1239 0.375401
+ 1240 0.375401
+ 1241 0.375401
+ 1242 0.375401
+ 1243 0.0052079
+ 1244 0.0052079
+ 1245 0.0052079
+ 1246 0.0052079
+ 1247 0.0052079
+ 1248 0.0052079
+ 1249 0.377278
+ 1250 0.377278
+ 1251 0.377278
+ 1252 0.377278
+ 1253 0.377278
+ 1254 0.377278
+ 1255 0.375941
+ 1256 0.375941
+ 1257 0.375941
+ 1258 0.375941
+ 1259 0.375941
+ 1260 0.375941
+ 1261 0.00551689
+ 1262 0.00551689
+ 1263 0.00551689
+ 1264 0.00551689
+ 1265 0.00551689
+ 1266 0.00551689
+ 1267 0.373505
+ 1268 0.373505
+ 1269 0.373505
+ 1270 0.373505
+ 1271 0.373505
+ 1272 0.373505
+ 1273 0.376296
+ 1274 0.376296
+ 1275 0.376296
+ 1276 0.376296
+ 1277 0.376296
+ 1278 0.376296
+ 1279 0.00521576
+ 1280 0.00521576
+ 1281 0.00521576
+ 1282 0.00521576
+ 1283 0.00521576
+ 1284 0.00521576
+ 1285 0.371128
+ 1286 0.371128
+ 1287 0.371128
+ 1288 0.371128
+ 1289 0.371128
+ 1290 0.371128
+ 1291 0.369947
+ 1292 0.369947
+ 1293 0.369947
+ 1294 0.369947
+ 1295 0.369947
+ 1296 0.369947
+ 1297 0.00425982
+ 1298 0.00425982
+ 1299 0.00425982
+ 1300 0.00425982
+ 1301 0.00425982
+ 1302 0.00425982
+ 1303 0.370019
+ 1304 0.370019
+ 1305 0.370019
+ 1306 0.370019
+ 1307 0.370019
+ 1308 0.370019
+ 1309 0.37133
+ 1310 0.37133
+ 1311 0.37133
+ 1312 0.37133
+ 1313 0.37133
+ 1314 0.37133
+ 1315 0.003519
+ 1316 0.003519
+ 1317 0.003519
+ 1318 0.003519
+ 1319 0.003519
+ 1320 0.003519
+ 1321 0.367272
+ 1322 0.367272
+ 1323 0.367272
+ 1324 0.367272
+ 1325 0.367272
+ 1326 0.367272
+ 1327 0.368091
+ 1328 0.368091
+ 1329 0.368091
+ 1330 0.368091
+ 1331 0.368091
+ 1332 0.368091
+ 1333 0.00328618
+ 1334 0.00328618
+ 1335 0.00328618
+ 1336 0.00328618
+ 1337 0.00328618
+ 1338 0.00328618
+ 1339 0.36367
+ 1340 0.36367
+ 1341 0.36367
+ 1342 0.36367
+ 1343 0.36367
+ 1344 0.36367
+ 1345 0.365851
+ 1346 0.365851
+ 1347 0.365851
+ 1348 0.365851
+ 1349 0.365851
+ 1350 0.365851
+ 1351 0.00289476
+ 1352 0.00289476
+ 1353 0.00289476
+ 1354 0.00289476
+ 1355 0.00289476
+ 1356 0.00289476
+ 1357 0.366624
+ 1358 0.366624
+ 1359 0.366624
+ 1360 0.366624
+ 1361 0.366624
+ 1362 0.366624
+ 1363 0.362548
+ 1364 0.362548
+ 1365 0.362548
+ 1366 0.362548
+ 1367 0.362548
+ 1368 0.362548
+ 1369 0.00255603
+ 1370 0.00255603
+ 1371 0.00255603
+ 1372 0.00255603
+ 1373 0.00255603
+ 1374 0.00255603
+ 1375 0.375304
+ 1376 0.375304
+ 1377 0.375304
+ 1378 0.375304
+ 1379 0.375304
+ 1380 0.375304
+ 1381 0.37154
+ 1382 0.37154
+ 1383 0.37154
+ 1384 0.37154
+ 1385 0.37154
+ 1386 0.37154
+ 1387 0.00318003
+ 1388 0.00318003
+ 1389 0.00318003
+ 1390 0.00318003
+ 1391 0.00318003
+ 1392 0.374529
+ 1393 0.374529
+ 1394 0.374529
+ 1395 0.374529
+ 1396 0.374529
+ 1397 0.374529
+ 1398 0.377021
+ 1399 0.377021
+ 1400 0.377021
+ 1401 0.377021
+ 1402 0.377021
+ 1403 0.377021
+ 1404 0.00434786
+ 1405 0.00434786
+ 1406 0.00434786
+ 1407 0.00434786
+ 1408 0.00434786
+ 1409 0.00434786
+ 1410 0.378212
+ 1411 0.378212
+ 1412 0.378212
+ 1413 0.378212
+ 1414 0.378212
+ 1415 0.378212
+ 1416 0.376587
+ 1417 0.376587
+ 1418 0.376587
+ 1419 0.376587
+ 1420 0.376587
+ 1421 0.376587
+ 1422 0.00412774
+ 1423 0.00412774
+ 1424 0.00412774
+ 1425 0.00412774
+ 1426 0.00412774
+ 1427 0.00412774
+ }
+ VertexInfluence "Bone_001" 1428 {
+ 0 0.0536852
+ 1 0.0536852
+ 2 0.0536852
+ 3 0.0536852
+ 4 0.663579
+ 5 0.663579
+ 6 0.663579
+ 7 0.663579
+ 8 0.0582281
+ 9 0.0582281
+ 10 0.0582281
+ 11 0.0582281
+ 12 0.68577
+ 13 0.68577
+ 14 0.68577
+ 15 0.68577
+ 16 0.059167
+ 17 0.059167
+ 18 0.059167
+ 19 0.059167
+ 20 0.692283
+ 21 0.692283
+ 22 0.692283
+ 23 0.692283
+ 24 0.0595554
+ 25 0.0595554
+ 26 0.0595554
+ 27 0.0595554
+ 28 0.705973
+ 29 0.705973
+ 30 0.705973
+ 31 0.705973
+ 32 0.0534075
+ 33 0.0534075
+ 34 0.0534075
+ 35 0.0534075
+ 36 0.698447
+ 37 0.698447
+ 38 0.698447
+ 39 0.698447
+ 40 0.0581471
+ 41 0.0581471
+ 42 0.0581471
+ 43 0.0581471
+ 44 0.683882
+ 45 0.683882
+ 46 0.683882
+ 47 0.683882
+ 48 0.0594622
+ 49 0.0594622
+ 50 0.0594622
+ 51 0.0594622
+ 52 0.628315
+ 53 0.628315
+ 54 0.628315
+ 55 0.628315
+ 56 0.0600619
+ 57 0.0600619
+ 58 0.0600619
+ 59 0.0600619
+ 60 0.622147
+ 61 0.622147
+ 62 0.622147
+ 63 0.622147
+ 64 0.0537814
+ 65 0.0537814
+ 66 0.0537814
+ 67 0.0537814
+ 68 0.621214
+ 69 0.621214
+ 70 0.621214
+ 71 0.621214
+ 72 0.0575935
+ 73 0.0575935
+ 74 0.0575935
+ 75 0.0575935
+ 76 0.634884
+ 77 0.634884
+ 78 0.634884
+ 79 0.634884
+ 80 0.058103
+ 81 0.058103
+ 82 0.058103
+ 83 0.058103
+ 84 0.582854
+ 85 0.582854
+ 86 0.582854
+ 87 0.582854
+ 88 0.0591488
+ 89 0.0591488
+ 90 0.0591488
+ 91 0.0591488
+ 92 0.616904
+ 93 0.616904
+ 94 0.616904
+ 95 0.616904
+ 96 0.0547738
+ 97 0.0547738
+ 98 0.0547738
+ 99 0.0547738
+ 100 0.0547738
+ 101 0.0547738
+ 102 0.0547738
+ 103 0.690176
+ 104 0.690176
+ 105 0.690176
+ 106 0.690176
+ 107 0.690176
+ 108 0.690176
+ 109 0.690176
+ 110 0.408744
+ 111 0.408744
+ 112 0.408744
+ 113 0.408744
+ 114 0.408744
+ 115 0.408744
+ 116 0.410372
+ 117 0.410372
+ 118 0.410372
+ 119 0.410372
+ 120 0.410372
+ 121 0.410372
+ 122 0.0606667
+ 123 0.0606667
+ 124 0.0606667
+ 125 0.0606667
+ 126 0.0606667
+ 127 0.0606667
+ 128 0.0606667
+ 129 0.0606667
+ 130 0.0606667
+ 131 0.0606667
+ 132 0.683242
+ 133 0.683242
+ 134 0.683242
+ 135 0.683242
+ 136 0.683242
+ 137 0.683242
+ 138 0.683242
+ 139 0.683242
+ 140 0.683242
+ 141 0.411126
+ 142 0.411126
+ 143 0.411126
+ 144 0.411126
+ 145 0.411126
+ 146 0.411126
+ 147 0.059725
+ 148 0.059725
+ 149 0.059725
+ 150 0.059725
+ 151 0.059725
+ 152 0.059725
+ 153 0.059725
+ 154 0.699151
+ 155 0.699151
+ 156 0.699151
+ 157 0.699151
+ 158 0.699151
+ 159 0.699151
+ 160 0.699151
+ 161 0.699151
+ 162 0.411491
+ 163 0.411491
+ 164 0.411491
+ 165 0.411491
+ 166 0.411491
+ 167 0.411491
+ 168 0.0585465
+ 169 0.0585465
+ 170 0.0585465
+ 171 0.0585465
+ 172 0.0585465
+ 173 0.0585465
+ 174 0.0585465
+ 175 0.0585465
+ 176 0.0585465
+ 177 0.0585465
+ 178 0.692537
+ 179 0.692537
+ 180 0.692537
+ 181 0.692537
+ 182 0.692537
+ 183 0.692537
+ 184 0.692537
+ 185 0.692537
+ 186 0.411145
+ 187 0.411145
+ 188 0.411145
+ 189 0.411145
+ 190 0.411145
+ 191 0.411145
+ 192 0.0545445
+ 193 0.0545445
+ 194 0.0545445
+ 195 0.0545445
+ 196 0.0545445
+ 197 0.0545445
+ 198 0.0545445
+ 199 0.0545445
+ 200 0.704511
+ 201 0.704511
+ 202 0.704511
+ 203 0.704511
+ 204 0.704511
+ 205 0.704511
+ 206 0.704511
+ 207 0.704511
+ 208 0.410016
+ 209 0.410016
+ 210 0.410016
+ 211 0.410016
+ 212 0.410016
+ 213 0.410016
+ 214 0.0607347
+ 215 0.0607347
+ 216 0.0607347
+ 217 0.0607347
+ 218 0.0607347
+ 219 0.0607347
+ 220 0.0607347
+ 221 0.0607347
+ 222 0.633219
+ 223 0.633219
+ 224 0.633219
+ 225 0.633219
+ 226 0.633219
+ 227 0.633219
+ 228 0.633219
+ 229 0.633219
+ 230 0.633219
+ 231 0.633219
+ 232 0.407998
+ 233 0.407998
+ 234 0.407998
+ 235 0.407998
+ 236 0.407998
+ 237 0.407998
+ 238 0.0602369
+ 239 0.0602369
+ 240 0.0602369
+ 241 0.0602369
+ 242 0.0602369
+ 243 0.0602369
+ 244 0.0602369
+ 245 0.0602369
+ 246 0.61358
+ 247 0.61358
+ 248 0.61358
+ 249 0.61358
+ 250 0.61358
+ 251 0.61358
+ 252 0.61358
+ 253 0.406953
+ 254 0.406953
+ 255 0.406953
+ 256 0.406953
+ 257 0.406953
+ 258 0.406953
+ 259 0.0589982
+ 260 0.0589982
+ 261 0.0589982
+ 262 0.0589982
+ 263 0.0589982
+ 264 0.0589982
+ 265 0.0589982
+ 266 0.0589982
+ 267 0.0589982
+ 268 0.602094
+ 269 0.602094
+ 270 0.602094
+ 271 0.602094
+ 272 0.602094
+ 273 0.602094
+ 274 0.602094
+ 275 0.602094
+ 276 0.602094
+ 277 0.406649
+ 278 0.406649
+ 279 0.406649
+ 280 0.406649
+ 281 0.406649
+ 282 0.406649
+ 283 0.0546197
+ 284 0.0546197
+ 285 0.0546197
+ 286 0.0546197
+ 287 0.0546197
+ 288 0.0546197
+ 289 0.0546197
+ 290 0.629987
+ 291 0.629987
+ 292 0.629987
+ 293 0.629987
+ 294 0.629987
+ 295 0.629987
+ 296 0.629987
+ 297 0.406314
+ 298 0.406314
+ 299 0.406314
+ 300 0.406314
+ 301 0.406314
+ 302 0.406314
+ 303 0.0596827
+ 304 0.0596827
+ 305 0.0596827
+ 306 0.0596827
+ 307 0.0596827
+ 308 0.0596827
+ 309 0.0596827
+ 310 0.0596827
+ 311 0.0596827
+ 312 0.0596827
+ 313 0.0596827
+ 314 0.617563
+ 315 0.617563
+ 316 0.617563
+ 317 0.617563
+ 318 0.617563
+ 319 0.617563
+ 320 0.617563
+ 321 0.617563
+ 322 0.405375
+ 323 0.405375
+ 324 0.405375
+ 325 0.405375
+ 326 0.405375
+ 327 0.405375
+ 328 0.0589791
+ 329 0.0589791
+ 330 0.0589791
+ 331 0.0589791
+ 332 0.0589791
+ 333 0.0589791
+ 334 0.0589791
+ 335 0.0589791
+ 336 0.618805
+ 337 0.618805
+ 338 0.618805
+ 339 0.618805
+ 340 0.618805
+ 341 0.618805
+ 342 0.618805
+ 343 0.618805
+ 344 0.618805
+ 345 0.618805
+ 346 0.618805
+ 347 0.618805
+ 348 0.406375
+ 349 0.406375
+ 350 0.406375
+ 351 0.406375
+ 352 0.406375
+ 353 0.406375
+ 354 0.0567919
+ 355 0.0567919
+ 356 0.0567919
+ 357 0.0567919
+ 358 0.0567919
+ 359 0.0567919
+ 360 0.0567919
+ 361 0.0567919
+ 362 0.0567919
+ 363 0.641822
+ 364 0.641822
+ 365 0.641822
+ 366 0.641822
+ 367 0.641822
+ 368 0.641822
+ 369 0.641822
+ 370 0.641822
+ 371 0.410126
+ 372 0.410126
+ 373 0.410126
+ 374 0.410126
+ 375 0.410126
+ 376 0.410126
+ 377 0.411144
+ 378 0.411144
+ 379 0.411144
+ 380 0.411144
+ 381 0.411144
+ 382 0.411144
+ 383 0.411758
+ 384 0.411758
+ 385 0.411758
+ 386 0.411758
+ 387 0.411758
+ 388 0.411758
+ 389 0.411717
+ 390 0.411717
+ 391 0.411717
+ 392 0.411717
+ 393 0.411717
+ 394 0.411717
+ 395 0.411143
+ 396 0.411143
+ 397 0.411143
+ 398 0.411143
+ 399 0.411143
+ 400 0.411143
+ 401 0.409299
+ 402 0.409299
+ 403 0.409299
+ 404 0.409299
+ 405 0.409299
+ 406 0.409299
+ 407 0.407735
+ 408 0.407735
+ 409 0.407735
+ 410 0.407735
+ 411 0.407735
+ 412 0.407735
+ 413 0.407069
+ 414 0.407069
+ 415 0.407069
+ 416 0.407069
+ 417 0.407069
+ 418 0.407069
+ 419 0.407022
+ 420 0.407022
+ 421 0.407022
+ 422 0.407022
+ 423 0.407022
+ 424 0.407022
+ 425 0.4061
+ 426 0.4061
+ 427 0.4061
+ 428 0.4061
+ 429 0.4061
+ 430 0.4061
+ 431 0.407861
+ 432 0.407861
+ 433 0.407861
+ 434 0.407861
+ 435 0.407861
+ 436 0.407861
+ 437 0.406035
+ 438 0.406035
+ 439 0.406035
+ 440 0.406035
+ 441 0.406035
+ 442 0.406035
+ 443 0.0558424
+ 444 0.0558424
+ 445 0.0558424
+ 446 0.0558424
+ 447 0.0558424
+ 448 0.709458
+ 449 0.709458
+ 450 0.709458
+ 451 0.709458
+ 452 0.709458
+ 453 0.709458
+ 454 0.527779
+ 455 0.527779
+ 456 0.527779
+ 457 0.527779
+ 458 0.527779
+ 459 0.527779
+ 460 0.117577
+ 461 0.117577
+ 462 0.117577
+ 463 0.117577
+ 464 0.117577
+ 465 0.117577
+ 466 0.0593598
+ 467 0.0593598
+ 468 0.0593598
+ 469 0.0593598
+ 470 0.0593598
+ 471 0.690194
+ 472 0.690194
+ 473 0.690194
+ 474 0.690194
+ 475 0.690194
+ 476 0.690194
+ 477 0.118115
+ 478 0.118115
+ 479 0.118115
+ 480 0.118115
+ 481 0.118115
+ 482 0.118115
+ 483 0.0590717
+ 484 0.0590717
+ 485 0.0590717
+ 486 0.0590717
+ 487 0.0590717
+ 488 0.699241
+ 489 0.699241
+ 490 0.699241
+ 491 0.699241
+ 492 0.699241
+ 493 0.699241
+ 494 0.118085
+ 495 0.118085
+ 496 0.118085
+ 497 0.118085
+ 498 0.118085
+ 499 0.118085
+ 500 0.0552082
+ 501 0.0552082
+ 502 0.0552082
+ 503 0.0552082
+ 504 0.0552082
+ 505 0.0552082
+ 506 0.702913
+ 507 0.702913
+ 508 0.702913
+ 509 0.702913
+ 510 0.702913
+ 511 0.702913
+ 512 0.11715
+ 513 0.11715
+ 514 0.11715
+ 515 0.11715
+ 516 0.11715
+ 517 0.11715
+ 518 0.0556777
+ 519 0.0556777
+ 520 0.0556777
+ 521 0.0556777
+ 522 0.0556777
+ 523 0.705151
+ 524 0.705151
+ 525 0.705151
+ 526 0.705151
+ 527 0.705151
+ 528 0.117498
+ 529 0.117498
+ 530 0.117498
+ 531 0.117498
+ 532 0.117498
+ 533 0.117498
+ 534 0.0595562
+ 535 0.0595562
+ 536 0.0595562
+ 537 0.0595562
+ 538 0.0595562
+ 539 0.0595562
+ 540 0.675713
+ 541 0.675713
+ 542 0.675713
+ 543 0.675713
+ 544 0.675713
+ 545 0.675713
+ 546 0.117429
+ 547 0.117429
+ 548 0.117429
+ 549 0.117429
+ 550 0.117429
+ 551 0.117429
+ 552 0.0596137
+ 553 0.0596137
+ 554 0.0596137
+ 555 0.0596137
+ 556 0.0596137
+ 557 0.636998
+ 558 0.636998
+ 559 0.636998
+ 560 0.636998
+ 561 0.636998
+ 562 0.117068
+ 563 0.117068
+ 564 0.117068
+ 565 0.117068
+ 566 0.117068
+ 567 0.117068
+ 568 0.0557424
+ 569 0.0557424
+ 570 0.0557424
+ 571 0.0557424
+ 572 0.0557424
+ 573 0.0557424
+ 574 0.627912
+ 575 0.627912
+ 576 0.627912
+ 577 0.627912
+ 578 0.627912
+ 579 0.116074
+ 580 0.116074
+ 581 0.116074
+ 582 0.116074
+ 583 0.116074
+ 584 0.116074
+ 585 0.0554518
+ 586 0.0554518
+ 587 0.0554518
+ 588 0.0554518
+ 589 0.0554518
+ 590 0.0554518
+ 591 0.641835
+ 592 0.641835
+ 593 0.641835
+ 594 0.641835
+ 595 0.641835
+ 596 0.116331
+ 597 0.116331
+ 598 0.116331
+ 599 0.116331
+ 600 0.116331
+ 601 0.116331
+ 602 0.0582382
+ 603 0.0582382
+ 604 0.0582382
+ 605 0.0582382
+ 606 0.0582382
+ 607 0.0582382
+ 608 0.600511
+ 609 0.600511
+ 610 0.600511
+ 611 0.600511
+ 612 0.600511
+ 613 0.600511
+ 614 0.116458
+ 615 0.116458
+ 616 0.116458
+ 617 0.116458
+ 618 0.116458
+ 619 0.116458
+ 620 0.0584118
+ 621 0.0584118
+ 622 0.0584118
+ 623 0.0584118
+ 624 0.0584118
+ 625 0.577143
+ 626 0.577143
+ 627 0.577143
+ 628 0.577143
+ 629 0.577143
+ 630 0.116709
+ 631 0.116709
+ 632 0.116709
+ 633 0.116709
+ 634 0.116709
+ 635 0.116709
+ 636 0.055461
+ 637 0.055461
+ 638 0.055461
+ 639 0.055461
+ 640 0.055461
+ 641 0.603492
+ 642 0.603492
+ 643 0.603492
+ 644 0.603492
+ 645 0.603492
+ 646 0.603492
+ 647 0.0485551
+ 648 0.0485551
+ 649 0.0485551
+ 650 0.0485551
+ 651 0.0485551
+ 652 0.0485551
+ 653 0.691429
+ 654 0.691429
+ 655 0.691429
+ 656 0.691429
+ 657 0.691429
+ 658 0.116582
+ 659 0.116582
+ 660 0.116582
+ 661 0.116582
+ 662 0.116582
+ 663 0.116582
+ 664 0.533249
+ 665 0.533249
+ 666 0.533249
+ 667 0.533249
+ 668 0.533249
+ 669 0.533249
+ 670 0.0586392
+ 671 0.0586392
+ 672 0.0586392
+ 673 0.0586392
+ 674 0.0586392
+ 675 0.0586392
+ 676 0.695704
+ 677 0.695704
+ 678 0.695704
+ 679 0.695704
+ 680 0.695704
+ 681 0.535191
+ 682 0.535191
+ 683 0.535191
+ 684 0.535191
+ 685 0.535191
+ 686 0.535191
+ 687 0.0570583
+ 688 0.0570583
+ 689 0.0570583
+ 690 0.0570583
+ 691 0.0570583
+ 692 0.0570583
+ 693 0.720179
+ 694 0.720179
+ 695 0.720179
+ 696 0.720179
+ 697 0.720179
+ 698 0.537441
+ 699 0.537441
+ 700 0.537441
+ 701 0.537441
+ 702 0.537441
+ 703 0.537441
+ 704 0.0581637
+ 705 0.0581637
+ 706 0.0581637
+ 707 0.0581637
+ 708 0.0581637
+ 709 0.0581637
+ 710 0.702459
+ 711 0.702459
+ 712 0.702459
+ 713 0.702459
+ 714 0.702459
+ 715 0.702459
+ 716 0.536746
+ 717 0.536746
+ 718 0.536746
+ 719 0.536746
+ 720 0.536746
+ 721 0.536746
+ 722 0.0478414
+ 723 0.0478414
+ 724 0.0478414
+ 725 0.0478414
+ 726 0.0478414
+ 727 0.72579
+ 728 0.72579
+ 729 0.72579
+ 730 0.72579
+ 731 0.72579
+ 732 0.532719
+ 733 0.532719
+ 734 0.532719
+ 735 0.532719
+ 736 0.532719
+ 737 0.532719
+ 738 0.0586395
+ 739 0.0586395
+ 740 0.0586395
+ 741 0.0586395
+ 742 0.0586395
+ 743 0.0586395
+ 744 0.648489
+ 745 0.648489
+ 746 0.648489
+ 747 0.648489
+ 748 0.648489
+ 749 0.648489
+ 750 0.521925
+ 751 0.521925
+ 752 0.521925
+ 753 0.521925
+ 754 0.521925
+ 755 0.521925
+ 756 0.0575213
+ 757 0.0575213
+ 758 0.0575213
+ 759 0.0575213
+ 760 0.0575213
+ 761 0.64483
+ 762 0.64483
+ 763 0.64483
+ 764 0.64483
+ 765 0.64483
+ 766 0.64483
+ 767 0.518645
+ 768 0.518645
+ 769 0.518645
+ 770 0.518645
+ 771 0.518645
+ 772 0.518645
+ 773 0.0587439
+ 774 0.0587439
+ 775 0.0587439
+ 776 0.0587439
+ 777 0.0587439
+ 778 0.0587439
+ 779 0.628935
+ 780 0.628935
+ 781 0.628935
+ 782 0.628935
+ 783 0.628935
+ 784 0.628935
+ 785 0.518782
+ 786 0.518782
+ 787 0.518782
+ 788 0.518782
+ 789 0.518782
+ 790 0.518782
+ 791 0.0484385
+ 792 0.0484385
+ 793 0.0484385
+ 794 0.0484385
+ 795 0.0484385
+ 796 0.6766
+ 797 0.6766
+ 798 0.6766
+ 799 0.6766
+ 800 0.6766
+ 801 0.6766
+ 802 0.518391
+ 803 0.518391
+ 804 0.518391
+ 805 0.518391
+ 806 0.518391
+ 807 0.518391
+ 808 0.0577665
+ 809 0.0577665
+ 810 0.0577665
+ 811 0.0577665
+ 812 0.0577665
+ 813 0.576605
+ 814 0.576605
+ 815 0.576605
+ 816 0.576605
+ 817 0.576605
+ 818 0.576605
+ 819 0.510493
+ 820 0.510493
+ 821 0.510493
+ 822 0.510493
+ 823 0.510493
+ 824 0.510493
+ 825 0.0560403
+ 826 0.0560403
+ 827 0.0560403
+ 828 0.0560403
+ 829 0.0560403
+ 830 0.592783
+ 831 0.592783
+ 832 0.592783
+ 833 0.592783
+ 834 0.592783
+ 835 0.515418
+ 836 0.515418
+ 837 0.515418
+ 838 0.515418
+ 839 0.515418
+ 840 0.515418
+ 841 0.058051
+ 842 0.058051
+ 843 0.058051
+ 844 0.058051
+ 845 0.058051
+ 846 0.058051
+ 847 0.649668
+ 848 0.649668
+ 849 0.649668
+ 850 0.649668
+ 851 0.649668
+ 852 0.410309
+ 853 0.410309
+ 854 0.410309
+ 855 0.410309
+ 856 0.410309
+ 857 0.410309
+ 858 0.409519
+ 859 0.409519
+ 860 0.409519
+ 861 0.409519
+ 862 0.409519
+ 863 0.409519
+ 864 0.116426
+ 865 0.116426
+ 866 0.116426
+ 867 0.116426
+ 868 0.116426
+ 869 0.116426
+ 870 0.532346
+ 871 0.532346
+ 872 0.532346
+ 873 0.532346
+ 874 0.532346
+ 875 0.532346
+ 876 0.411165
+ 877 0.411165
+ 878 0.411165
+ 879 0.411165
+ 880 0.411165
+ 881 0.411165
+ 882 0.410794
+ 883 0.410794
+ 884 0.410794
+ 885 0.410794
+ 886 0.410794
+ 887 0.410794
+ 888 0.117535
+ 889 0.117535
+ 890 0.117535
+ 891 0.117535
+ 892 0.117535
+ 893 0.117535
+ 894 0.533844
+ 895 0.533844
+ 896 0.533844
+ 897 0.533844
+ 898 0.533844
+ 899 0.533844
+ 900 0.411683
+ 901 0.411683
+ 902 0.411683
+ 903 0.411683
+ 904 0.411683
+ 905 0.411683
+ 906 0.411475
+ 907 0.411475
+ 908 0.411475
+ 909 0.411475
+ 910 0.411475
+ 911 0.411475
+ 912 0.117656
+ 913 0.117656
+ 914 0.117656
+ 915 0.117656
+ 916 0.117656
+ 917 0.117656
+ 918 0.536717
+ 919 0.536717
+ 920 0.536717
+ 921 0.536717
+ 922 0.536717
+ 923 0.536717
+ 924 0.411463
+ 925 0.411463
+ 926 0.411463
+ 927 0.411463
+ 928 0.411463
+ 929 0.411463
+ 930 0.411641
+ 931 0.411641
+ 932 0.411641
+ 933 0.411641
+ 934 0.411641
+ 935 0.411641
+ 936 0.117234
+ 937 0.117234
+ 938 0.117234
+ 939 0.117234
+ 940 0.117234
+ 941 0.117234
+ 942 0.536611
+ 943 0.536611
+ 944 0.536611
+ 945 0.536611
+ 946 0.536611
+ 947 0.536611
+ 948 0.410672
+ 949 0.410672
+ 950 0.410672
+ 951 0.410672
+ 952 0.410672
+ 953 0.410672
+ 954 0.411195
+ 955 0.411195
+ 956 0.411195
+ 957 0.411195
+ 958 0.411195
+ 959 0.411195
+ 960 0.116627
+ 961 0.116627
+ 962 0.116627
+ 963 0.116627
+ 964 0.116627
+ 965 0.116627
+ 966 0.536487
+ 967 0.536487
+ 968 0.536487
+ 969 0.536487
+ 970 0.536487
+ 971 0.536487
+ 972 0.40864
+ 973 0.40864
+ 974 0.40864
+ 975 0.40864
+ 976 0.40864
+ 977 0.40864
+ 978 0.409689
+ 979 0.409689
+ 980 0.409689
+ 981 0.409689
+ 982 0.409689
+ 983 0.409689
+ 984 0.117148
+ 985 0.117148
+ 986 0.117148
+ 987 0.117148
+ 988 0.117148
+ 989 0.117148
+ 990 0.525977
+ 991 0.525977
+ 992 0.525977
+ 993 0.525977
+ 994 0.525977
+ 995 0.525977
+ 996 0.407355
+ 997 0.407355
+ 998 0.407355
+ 999 0.407355
+ 1000 0.407355
+ 1001 0.407355
+ 1002 0.407854
+ 1003 0.407854
+ 1004 0.407854
+ 1005 0.407854
+ 1006 0.407854
+ 1007 0.407854
+ 1008 0.116803
+ 1009 0.116803
+ 1010 0.116803
+ 1011 0.116803
+ 1012 0.116803
+ 1013 0.116803
+ 1014 0.519599
+ 1015 0.519599
+ 1016 0.519599
+ 1017 0.519599
+ 1018 0.519599
+ 1019 0.519599
+ 1020 0.406864
+ 1021 0.406864
+ 1022 0.406864
+ 1023 0.406864
+ 1024 0.406864
+ 1025 0.406864
+ 1026 0.407012
+ 1027 0.407012
+ 1028 0.407012
+ 1029 0.407012
+ 1030 0.407012
+ 1031 0.407012
+ 1032 0.116194
+ 1033 0.116194
+ 1034 0.116194
+ 1035 0.116194
+ 1036 0.116194
+ 1037 0.116194
+ 1038 0.517497
+ 1039 0.517497
+ 1040 0.517497
+ 1041 0.517497
+ 1042 0.517497
+ 1043 0.517497
+ 1044 0.406762
+ 1045 0.406762
+ 1046 0.406762
+ 1047 0.406762
+ 1048 0.406762
+ 1049 0.406762
+ 1050 0.406875
+ 1051 0.406875
+ 1052 0.406875
+ 1053 0.406875
+ 1054 0.406875
+ 1055 0.406875
+ 1056 0.115553
+ 1057 0.115553
+ 1058 0.115553
+ 1059 0.115553
+ 1060 0.115553
+ 1061 0.115553
+ 1062 0.520202
+ 1063 0.520202
+ 1064 0.520202
+ 1065 0.520202
+ 1066 0.520202
+ 1067 0.520202
+ 1068 0.405691
+ 1069 0.405691
+ 1070 0.405691
+ 1071 0.405691
+ 1072 0.405691
+ 1073 0.405691
+ 1074 0.40622
+ 1075 0.40622
+ 1076 0.40622
+ 1077 0.40622
+ 1078 0.40622
+ 1079 0.40622
+ 1080 0.116043
+ 1081 0.116043
+ 1082 0.116043
+ 1083 0.116043
+ 1084 0.116043
+ 1085 0.116043
+ 1086 0.513728
+ 1087 0.513728
+ 1088 0.513728
+ 1089 0.513728
+ 1090 0.513728
+ 1091 0.513728
+ 1092 0.513728
+ 1093 0.408328
+ 1094 0.408328
+ 1095 0.408328
+ 1096 0.408328
+ 1097 0.408328
+ 1098 0.408328
+ 1099 0.407093
+ 1100 0.407093
+ 1101 0.407093
+ 1102 0.407093
+ 1103 0.407093
+ 1104 0.407093
+ 1105 0.116205
+ 1106 0.116205
+ 1107 0.116205
+ 1108 0.116205
+ 1109 0.116205
+ 1110 0.116205
+ 1111 0.521077
+ 1112 0.521077
+ 1113 0.521077
+ 1114 0.521077
+ 1115 0.521077
+ 1116 0.521077
+ 1117 0.406172
+ 1118 0.406172
+ 1119 0.406172
+ 1120 0.406172
+ 1121 0.406172
+ 1122 0.406172
+ 1123 0.405649
+ 1124 0.405649
+ 1125 0.405649
+ 1126 0.405649
+ 1127 0.405649
+ 1128 0.405649
+ 1129 0.116095
+ 1130 0.116095
+ 1131 0.116095
+ 1132 0.116095
+ 1133 0.116095
+ 1134 0.116095
+ 1135 0.512592
+ 1136 0.512592
+ 1137 0.512592
+ 1138 0.512592
+ 1139 0.512592
+ 1140 0.512592
+ 1141 0.532998
+ 1142 0.532998
+ 1143 0.532998
+ 1144 0.532998
+ 1145 0.532998
+ 1146 0.532998
+ 1147 0.534609
+ 1148 0.534609
+ 1149 0.534609
+ 1150 0.534609
+ 1151 0.534609
+ 1152 0.534609
+ 1153 0.537637
+ 1154 0.537637
+ 1155 0.537637
+ 1156 0.537637
+ 1157 0.537637
+ 1158 0.537637
+ 1159 0.536753
+ 1160 0.536753
+ 1161 0.536753
+ 1162 0.536753
+ 1163 0.536753
+ 1164 0.536753
+ 1165 0.535728
+ 1166 0.535728
+ 1167 0.535728
+ 1168 0.535728
+ 1169 0.535728
+ 1170 0.535728
+ 1171 0.523952
+ 1172 0.523952
+ 1173 0.523952
+ 1174 0.523952
+ 1175 0.523952
+ 1176 0.523952
+ 1177 0.51959
+ 1178 0.51959
+ 1179 0.51959
+ 1180 0.51959
+ 1181 0.51959
+ 1182 0.51959
+ 1183 0.518243
+ 1184 0.518243
+ 1185 0.518243
+ 1186 0.518243
+ 1187 0.518243
+ 1188 0.518243
+ 1189 0.520723
+ 1190 0.520723
+ 1191 0.520723
+ 1192 0.520723
+ 1193 0.520723
+ 1194 0.520723
+ 1195 0.511297
+ 1196 0.511297
+ 1197 0.511297
+ 1198 0.511297
+ 1199 0.511297
+ 1200 0.511297
+ 1201 0.524385
+ 1202 0.524385
+ 1203 0.524385
+ 1204 0.524385
+ 1205 0.524385
+ 1206 0.524385
+ 1207 0.513435
+ 1208 0.513435
+ 1209 0.513435
+ 1210 0.513435
+ 1211 0.513435
+ 1212 0.513435
+ 1213 0.116967
+ 1214 0.116967
+ 1215 0.116967
+ 1216 0.116967
+ 1217 0.116967
+ 1218 0.116967
+ 1219 0.116342
+ 1220 0.116342
+ 1221 0.116342
+ 1222 0.116342
+ 1223 0.116342
+ 1224 0.116342
+ 1225 0.531194
+ 1226 0.531194
+ 1227 0.531194
+ 1228 0.531194
+ 1229 0.531194
+ 1230 0.531194
+ 1231 0.117819
+ 1232 0.117819
+ 1233 0.117819
+ 1234 0.117819
+ 1235 0.117819
+ 1236 0.117819
+ 1237 0.117551
+ 1238 0.117551
+ 1239 0.117551
+ 1240 0.117551
+ 1241 0.117551
+ 1242 0.117551
+ 1243 0.533594
+ 1244 0.533594
+ 1245 0.533594
+ 1246 0.533594
+ 1247 0.533594
+ 1248 0.533594
+ 1249 0.117864
+ 1250 0.117864
+ 1251 0.117864
+ 1252 0.117864
+ 1253 0.117864
+ 1254 0.117864
+ 1255 0.117811
+ 1256 0.117811
+ 1257 0.117811
+ 1258 0.117811
+ 1259 0.117811
+ 1260 0.117811
+ 1261 0.535981
+ 1262 0.535981
+ 1263 0.535981
+ 1264 0.535981
+ 1265 0.535981
+ 1266 0.535981
+ 1267 0.117146
+ 1268 0.117146
+ 1269 0.117146
+ 1270 0.117146
+ 1271 0.117146
+ 1272 0.117146
+ 1273 0.11765
+ 1274 0.11765
+ 1275 0.11765
+ 1276 0.11765
+ 1277 0.11765
+ 1278 0.11765
+ 1279 0.537053
+ 1280 0.537053
+ 1281 0.537053
+ 1282 0.537053
+ 1283 0.537053
+ 1284 0.537053
+ 1285 0.117029
+ 1286 0.117029
+ 1287 0.117029
+ 1288 0.117029
+ 1289 0.117029
+ 1290 0.117029
+ 1291 0.11669
+ 1292 0.11669
+ 1293 0.11669
+ 1294 0.11669
+ 1295 0.11669
+ 1296 0.11669
+ 1297 0.536812
+ 1298 0.536812
+ 1299 0.536812
+ 1300 0.536812
+ 1301 0.536812
+ 1302 0.536812
+ 1303 0.117275
+ 1304 0.117275
+ 1305 0.117275
+ 1306 0.117275
+ 1307 0.117275
+ 1308 0.117275
+ 1309 0.117316
+ 1310 0.117316
+ 1311 0.117316
+ 1312 0.117316
+ 1313 0.117316
+ 1314 0.117316
+ 1315 0.52955
+ 1316 0.52955
+ 1317 0.52955
+ 1318 0.52955
+ 1319 0.52955
+ 1320 0.52955
+ 1321 0.116922
+ 1322 0.116922
+ 1323 0.116922
+ 1324 0.116922
+ 1325 0.116922
+ 1326 0.116922
+ 1327 0.117038
+ 1328 0.117038
+ 1329 0.117038
+ 1330 0.117038
+ 1331 0.117038
+ 1332 0.117038
+ 1333 0.520788
+ 1334 0.520788
+ 1335 0.520788
+ 1336 0.520788
+ 1337 0.520788
+ 1338 0.520788
+ 1339 0.11609
+ 1340 0.11609
+ 1341 0.11609
+ 1342 0.11609
+ 1343 0.11609
+ 1344 0.11609
+ 1345 0.116618
+ 1346 0.116618
+ 1347 0.116618
+ 1348 0.116618
+ 1349 0.116618
+ 1350 0.116618
+ 1351 0.518144
+ 1352 0.518144
+ 1353 0.518144
+ 1354 0.518144
+ 1355 0.518144
+ 1356 0.518144
+ 1357 0.115908
+ 1358 0.115908
+ 1359 0.115908
+ 1360 0.115908
+ 1361 0.115908
+ 1362 0.115908
+ 1363 0.115628
+ 1364 0.115628
+ 1365 0.115628
+ 1366 0.115628
+ 1367 0.115628
+ 1368 0.115628
+ 1369 0.51984
+ 1370 0.51984
+ 1371 0.51984
+ 1372 0.51984
+ 1373 0.51984
+ 1374 0.51984
+ 1375 0.116223
+ 1376 0.116223
+ 1377 0.116223
+ 1378 0.116223
+ 1379 0.116223
+ 1380 0.116223
+ 1381 0.116171
+ 1382 0.116171
+ 1383 0.116171
+ 1384 0.116171
+ 1385 0.116171
+ 1386 0.116171
+ 1387 0.515533
+ 1388 0.515533
+ 1389 0.515533
+ 1390 0.515533
+ 1391 0.515533
+ 1392 0.116361
+ 1393 0.116361
+ 1394 0.116361
+ 1395 0.116361
+ 1396 0.116361
+ 1397 0.116361
+ 1398 0.116448
+ 1399 0.116448
+ 1400 0.116448
+ 1401 0.116448
+ 1402 0.116448
+ 1403 0.116448
+ 1404 0.517509
+ 1405 0.517509
+ 1406 0.517509
+ 1407 0.517509
+ 1408 0.517509
+ 1409 0.517509
+ 1410 0.116379
+ 1411 0.116379
+ 1412 0.116379
+ 1413 0.116379
+ 1414 0.116379
+ 1415 0.116379
+ 1416 0.116183
+ 1417 0.116183
+ 1418 0.116183
+ 1419 0.116183
+ 1420 0.116183
+ 1421 0.116183
+ 1422 0.510798
+ 1423 0.510798
+ 1424 0.510798
+ 1425 0.510798
+ 1426 0.510798
+ 1427 0.510798
+ }
+ VertexInfluence "Bone_002" 1428 {
+ 0 0.598155
+ 1 0.598155
+ 2 0.598155
+ 3 0.598155
+ 4 0.0582628
+ 5 0.0582628
+ 6 0.0582628
+ 7 0.0582628
+ 8 0.574353
+ 9 0.574353
+ 10 0.574353
+ 11 0.574353
+ 12 0.0690805
+ 13 0.0690805
+ 14 0.0690805
+ 15 0.0690805
+ 16 0.576778
+ 17 0.576778
+ 18 0.576778
+ 19 0.576778
+ 20 0.0726471
+ 21 0.0726471
+ 22 0.0726471
+ 23 0.0726471
+ 24 0.56066
+ 25 0.56066
+ 26 0.56066
+ 27 0.56066
+ 28 0.0699223
+ 29 0.0699223
+ 30 0.0699223
+ 31 0.0699223
+ 32 0.604528
+ 33 0.604528
+ 34 0.604528
+ 35 0.604528
+ 36 0.0679579
+ 37 0.0679579
+ 38 0.0679579
+ 39 0.0679579
+ 40 0.586099
+ 41 0.586099
+ 42 0.586099
+ 43 0.586099
+ 44 0.0579427
+ 45 0.0579427
+ 46 0.0579427
+ 47 0.0579427
+ 48 0.59979
+ 49 0.59979
+ 50 0.59979
+ 51 0.59979
+ 52 0.0618867
+ 53 0.0618867
+ 54 0.0618867
+ 55 0.0618867
+ 56 0.602846
+ 57 0.602846
+ 58 0.602846
+ 59 0.602846
+ 60 0.0601907
+ 61 0.0601907
+ 62 0.0601907
+ 63 0.0601907
+ 64 0.640274
+ 65 0.640274
+ 66 0.640274
+ 67 0.640274
+ 68 0.0585797
+ 69 0.0585797
+ 70 0.0585797
+ 71 0.0585797
+ 72 0.597926
+ 73 0.597926
+ 74 0.597926
+ 75 0.597926
+ 76 0.0539
+ 77 0.0539
+ 78 0.0539
+ 79 0.0539
+ 80 0.56915
+ 81 0.56915
+ 82 0.56915
+ 83 0.56915
+ 84 0.0600049
+ 85 0.0600049
+ 86 0.0600049
+ 87 0.0600049
+ 88 0.554443
+ 89 0.554443
+ 90 0.554443
+ 91 0.554443
+ 92 0.0616531
+ 93 0.0616531
+ 94 0.0616531
+ 95 0.0616531
+ 96 0.587102
+ 97 0.587102
+ 98 0.587102
+ 99 0.587102
+ 100 0.587102
+ 101 0.587102
+ 102 0.587102
+ 103 0.0637755
+ 104 0.0637755
+ 105 0.0637755
+ 106 0.0637755
+ 107 0.0637755
+ 108 0.0637755
+ 109 0.0637755
+ 110 0.404264
+ 111 0.404264
+ 112 0.404264
+ 113 0.404264
+ 114 0.404264
+ 115 0.404264
+ 116 0.40436
+ 117 0.40436
+ 118 0.40436
+ 119 0.40436
+ 120 0.40436
+ 121 0.40436
+ 122 0.560407
+ 123 0.560407
+ 124 0.560407
+ 125 0.560407
+ 126 0.560407
+ 127 0.560407
+ 128 0.560407
+ 129 0.560407
+ 130 0.560407
+ 131 0.560407
+ 132 0.0732167
+ 133 0.0732167
+ 134 0.0732167
+ 135 0.0732167
+ 136 0.0732167
+ 137 0.0732167
+ 138 0.0732167
+ 139 0.0732167
+ 140 0.0732167
+ 141 0.404198
+ 142 0.404198
+ 143 0.404198
+ 144 0.404198
+ 145 0.404198
+ 146 0.404198
+ 147 0.557395
+ 148 0.557395
+ 149 0.557395
+ 150 0.557395
+ 151 0.557395
+ 152 0.557395
+ 153 0.557395
+ 154 0.0720451
+ 155 0.0720451
+ 156 0.0720451
+ 157 0.0720451
+ 158 0.0720451
+ 159 0.0720451
+ 160 0.0720451
+ 161 0.0720451
+ 162 0.404162
+ 163 0.404162
+ 164 0.404162
+ 165 0.404162
+ 166 0.404162
+ 167 0.404162
+ 168 0.56108
+ 169 0.56108
+ 170 0.56108
+ 171 0.56108
+ 172 0.56108
+ 173 0.56108
+ 174 0.56108
+ 175 0.56108
+ 176 0.56108
+ 177 0.56108
+ 178 0.0721328
+ 179 0.0721328
+ 180 0.0721328
+ 181 0.0721328
+ 182 0.0721328
+ 183 0.0721328
+ 184 0.0721328
+ 185 0.0721328
+ 186 0.404965
+ 187 0.404965
+ 188 0.404965
+ 189 0.404965
+ 190 0.404965
+ 191 0.404965
+ 192 0.595569
+ 193 0.595569
+ 194 0.595569
+ 195 0.595569
+ 196 0.595569
+ 197 0.595569
+ 198 0.595569
+ 199 0.595569
+ 200 0.0630788
+ 201 0.0630788
+ 202 0.0630788
+ 203 0.0630788
+ 204 0.0630788
+ 205 0.0630788
+ 206 0.0630788
+ 207 0.0630788
+ 208 0.404967
+ 209 0.404967
+ 210 0.404967
+ 211 0.404967
+ 212 0.404967
+ 213 0.404967
+ 214 0.576136
+ 215 0.576136
+ 216 0.576136
+ 217 0.576136
+ 218 0.576136
+ 219 0.576136
+ 220 0.576136
+ 221 0.576136
+ 222 0.0616774
+ 223 0.0616774
+ 224 0.0616774
+ 225 0.0616774
+ 226 0.0616774
+ 227 0.0616774
+ 228 0.0616774
+ 229 0.0616774
+ 230 0.0616774
+ 231 0.0616774
+ 232 0.405261
+ 233 0.405261
+ 234 0.405261
+ 235 0.405261
+ 236 0.405261
+ 237 0.405261
+ 238 0.592427
+ 239 0.592427
+ 240 0.592427
+ 241 0.592427
+ 242 0.592427
+ 243 0.592427
+ 244 0.592427
+ 245 0.592427
+ 246 0.0613356
+ 247 0.0613356
+ 248 0.0613356
+ 249 0.0613356
+ 250 0.0613356
+ 251 0.0613356
+ 252 0.0613356
+ 253 0.405806
+ 254 0.405806
+ 255 0.405806
+ 256 0.405806
+ 257 0.405806
+ 258 0.405806
+ 259 0.601937
+ 260 0.601937
+ 261 0.601937
+ 262 0.601937
+ 263 0.601937
+ 264 0.601937
+ 265 0.601937
+ 266 0.601937
+ 267 0.601937
+ 268 0.0612082
+ 269 0.0612082
+ 270 0.0612082
+ 271 0.0612082
+ 272 0.0612082
+ 273 0.0612082
+ 274 0.0612082
+ 275 0.0612082
+ 276 0.0612082
+ 277 0.406295
+ 278 0.406295
+ 279 0.406295
+ 280 0.406295
+ 281 0.406295
+ 282 0.406295
+ 283 0.621235
+ 284 0.621235
+ 285 0.621235
+ 286 0.621235
+ 287 0.621235
+ 288 0.621235
+ 289 0.621235
+ 290 0.0550011
+ 291 0.0550011
+ 292 0.0550011
+ 293 0.0550011
+ 294 0.0550011
+ 295 0.0550011
+ 296 0.0550011
+ 297 0.405013
+ 298 0.405013
+ 299 0.405013
+ 300 0.405013
+ 301 0.405013
+ 302 0.405013
+ 303 0.561939
+ 304 0.561939
+ 305 0.561939
+ 306 0.561939
+ 307 0.561939
+ 308 0.561939
+ 309 0.561939
+ 310 0.561939
+ 311 0.561939
+ 312 0.561939
+ 313 0.561939
+ 314 0.057139
+ 315 0.057139
+ 316 0.057139
+ 317 0.057139
+ 318 0.057139
+ 319 0.057139
+ 320 0.057139
+ 321 0.057139
+ 322 0.403626
+ 323 0.403626
+ 324 0.403626
+ 325 0.403626
+ 326 0.403626
+ 327 0.403626
+ 328 0.552706
+ 329 0.552706
+ 330 0.552706
+ 331 0.552706
+ 332 0.552706
+ 333 0.552706
+ 334 0.552706
+ 335 0.552706
+ 336 0.0588137
+ 337 0.0588137
+ 338 0.0588137
+ 339 0.0588137
+ 340 0.0588137
+ 341 0.0588137
+ 342 0.0588137
+ 343 0.0588137
+ 344 0.0588137
+ 345 0.0588137
+ 346 0.0588137
+ 347 0.0588137
+ 348 0.403396
+ 349 0.403396
+ 350 0.403396
+ 351 0.403396
+ 352 0.403396
+ 353 0.403396
+ 354 0.577441
+ 355 0.577441
+ 356 0.577441
+ 357 0.577441
+ 358 0.577441
+ 359 0.577441
+ 360 0.577441
+ 361 0.577441
+ 362 0.577441
+ 363 0.0601323
+ 364 0.0601323
+ 365 0.0601323
+ 366 0.0601323
+ 367 0.0601323
+ 368 0.0601323
+ 369 0.0601323
+ 370 0.0601323
+ 371 0.404841
+ 372 0.404841
+ 373 0.404841
+ 374 0.404841
+ 375 0.404841
+ 376 0.404841
+ 377 0.404642
+ 378 0.404642
+ 379 0.404642
+ 380 0.404642
+ 381 0.404642
+ 382 0.404642
+ 383 0.404494
+ 384 0.404494
+ 385 0.404494
+ 386 0.404494
+ 387 0.404494
+ 388 0.404494
+ 389 0.404896
+ 390 0.404896
+ 391 0.404896
+ 392 0.404896
+ 393 0.404896
+ 394 0.404896
+ 395 0.405469
+ 396 0.405469
+ 397 0.405469
+ 398 0.405469
+ 399 0.405469
+ 400 0.405469
+ 401 0.405397
+ 402 0.405397
+ 403 0.405397
+ 404 0.405397
+ 405 0.405397
+ 406 0.405397
+ 407 0.405906
+ 408 0.405906
+ 409 0.405906
+ 410 0.405906
+ 411 0.405906
+ 412 0.405906
+ 413 0.406506
+ 414 0.406506
+ 415 0.406506
+ 416 0.406506
+ 417 0.406506
+ 418 0.406506
+ 419 0.40626
+ 420 0.40626
+ 421 0.40626
+ 422 0.40626
+ 423 0.40626
+ 424 0.40626
+ 425 0.404552
+ 426 0.404552
+ 427 0.404552
+ 428 0.404552
+ 429 0.404552
+ 430 0.404552
+ 431 0.404184
+ 432 0.404184
+ 433 0.404184
+ 434 0.404184
+ 435 0.404184
+ 436 0.404184
+ 437 0.403743
+ 438 0.403743
+ 439 0.403743
+ 440 0.403743
+ 441 0.403743
+ 442 0.403743
+ 443 0.589523
+ 444 0.589523
+ 445 0.589523
+ 446 0.589523
+ 447 0.589523
+ 448 0.0552031
+ 449 0.0552031
+ 450 0.0552031
+ 451 0.0552031
+ 452 0.0552031
+ 453 0.0552031
+ 454 0.116564
+ 455 0.116564
+ 456 0.116564
+ 457 0.116564
+ 458 0.116564
+ 459 0.116564
+ 460 0.507737
+ 461 0.507737
+ 462 0.507737
+ 463 0.507737
+ 464 0.507737
+ 465 0.507737
+ 466 0.583154
+ 467 0.583154
+ 468 0.583154
+ 469 0.583154
+ 470 0.583154
+ 471 0.0695297
+ 472 0.0695297
+ 473 0.0695297
+ 474 0.0695297
+ 475 0.0695297
+ 476 0.0695297
+ 477 0.506298
+ 478 0.506298
+ 479 0.506298
+ 480 0.506298
+ 481 0.506298
+ 482 0.506298
+ 483 0.567147
+ 484 0.567147
+ 485 0.567147
+ 486 0.567147
+ 487 0.567147
+ 488 0.0712864
+ 489 0.0712864
+ 490 0.0712864
+ 491 0.0712864
+ 492 0.0712864
+ 493 0.0712864
+ 494 0.505047
+ 495 0.505047
+ 496 0.505047
+ 497 0.505047
+ 498 0.505047
+ 499 0.505047
+ 500 0.590716
+ 501 0.590716
+ 502 0.590716
+ 503 0.590716
+ 504 0.590716
+ 505 0.590716
+ 506 0.0699366
+ 507 0.0699366
+ 508 0.0699366
+ 509 0.0699366
+ 510 0.0699366
+ 511 0.0699366
+ 512 0.511597
+ 513 0.511597
+ 514 0.511597
+ 515 0.511597
+ 516 0.511597
+ 517 0.511597
+ 518 0.599921
+ 519 0.599921
+ 520 0.599921
+ 521 0.599921
+ 522 0.599921
+ 523 0.0643527
+ 524 0.0643527
+ 525 0.0643527
+ 526 0.0643527
+ 527 0.0643527
+ 528 0.511186
+ 529 0.511186
+ 530 0.511186
+ 531 0.511186
+ 532 0.511186
+ 533 0.511186
+ 534 0.603076
+ 535 0.603076
+ 536 0.603076
+ 537 0.603076
+ 538 0.603076
+ 539 0.603076
+ 540 0.059269
+ 541 0.059269
+ 542 0.059269
+ 543 0.059269
+ 544 0.059269
+ 545 0.059269
+ 546 0.512515
+ 547 0.512515
+ 548 0.512515
+ 549 0.512515
+ 550 0.512515
+ 551 0.512515
+ 552 0.613036
+ 553 0.613036
+ 554 0.613036
+ 555 0.613036
+ 556 0.613036
+ 557 0.0611269
+ 558 0.0611269
+ 559 0.0611269
+ 560 0.0611269
+ 561 0.0611269
+ 562 0.514589
+ 563 0.514589
+ 564 0.514589
+ 565 0.514589
+ 566 0.514589
+ 567 0.514589
+ 568 0.637212
+ 569 0.637212
+ 570 0.637212
+ 571 0.637212
+ 572 0.637212
+ 573 0.637212
+ 574 0.0601891
+ 575 0.0601891
+ 576 0.0601891
+ 577 0.0601891
+ 578 0.0601891
+ 579 0.519667
+ 580 0.519667
+ 581 0.519667
+ 582 0.519667
+ 583 0.519667
+ 584 0.519667
+ 585 0.625354
+ 586 0.625354
+ 587 0.625354
+ 588 0.625354
+ 589 0.625354
+ 590 0.625354
+ 591 0.0561587
+ 592 0.0561587
+ 593 0.0561587
+ 594 0.0561587
+ 595 0.0561587
+ 596 0.512534
+ 597 0.512534
+ 598 0.512534
+ 599 0.512534
+ 600 0.512534
+ 601 0.512534
+ 602 0.572912
+ 603 0.572912
+ 604 0.572912
+ 605 0.572912
+ 606 0.572912
+ 607 0.572912
+ 608 0.0574506
+ 609 0.0574506
+ 610 0.0574506
+ 611 0.0574506
+ 612 0.0574506
+ 613 0.0574506
+ 614 0.505162
+ 615 0.505162
+ 616 0.505162
+ 617 0.505162
+ 618 0.505162
+ 619 0.505162
+ 620 0.560628
+ 621 0.560628
+ 622 0.560628
+ 623 0.560628
+ 624 0.560628
+ 625 0.0610238
+ 626 0.0610238
+ 627 0.0610238
+ 628 0.0610238
+ 629 0.0610238
+ 630 0.503603
+ 631 0.503603
+ 632 0.503603
+ 633 0.503603
+ 634 0.503603
+ 635 0.503603
+ 636 0.585366
+ 637 0.585366
+ 638 0.585366
+ 639 0.585366
+ 640 0.585366
+ 641 0.0623888
+ 642 0.0623888
+ 643 0.0623888
+ 644 0.0623888
+ 645 0.0623888
+ 646 0.0623888
+ 647 0.638203
+ 648 0.638203
+ 649 0.638203
+ 650 0.638203
+ 651 0.638203
+ 652 0.638203
+ 653 0.065283
+ 654 0.065283
+ 655 0.065283
+ 656 0.065283
+ 657 0.065283
+ 658 0.510159
+ 659 0.510159
+ 660 0.510159
+ 661 0.510159
+ 662 0.510159
+ 663 0.510159
+ 664 0.118231
+ 665 0.118231
+ 666 0.118231
+ 667 0.118231
+ 668 0.118231
+ 669 0.118231
+ 670 0.583584
+ 671 0.583584
+ 672 0.583584
+ 673 0.583584
+ 674 0.583584
+ 675 0.583584
+ 676 0.0708357
+ 677 0.0708357
+ 678 0.0708357
+ 679 0.0708357
+ 680 0.0708357
+ 681 0.119047
+ 682 0.119047
+ 683 0.119047
+ 684 0.119047
+ 685 0.119047
+ 686 0.119047
+ 687 0.595147
+ 688 0.595147
+ 689 0.595147
+ 690 0.595147
+ 691 0.595147
+ 692 0.595147
+ 693 0.0665772
+ 694 0.0665772
+ 695 0.0665772
+ 696 0.0665772
+ 697 0.0665772
+ 698 0.118746
+ 699 0.118746
+ 700 0.118746
+ 701 0.118746
+ 702 0.118746
+ 703 0.118746
+ 704 0.577364
+ 705 0.577364
+ 706 0.577364
+ 707 0.577364
+ 708 0.577364
+ 709 0.577364
+ 710 0.0681478
+ 711 0.0681478
+ 712 0.0681478
+ 713 0.0681478
+ 714 0.0681478
+ 715 0.0681478
+ 716 0.118146
+ 717 0.118146
+ 718 0.118146
+ 719 0.118146
+ 720 0.118146
+ 721 0.118146
+ 722 0.646413
+ 723 0.646413
+ 724 0.646413
+ 725 0.646413
+ 726 0.646413
+ 727 0.0546473
+ 728 0.0546473
+ 729 0.0546473
+ 730 0.0546473
+ 731 0.0546473
+ 732 0.116752
+ 733 0.116752
+ 734 0.116752
+ 735 0.116752
+ 736 0.116752
+ 737 0.116752
+ 738 0.597644
+ 739 0.597644
+ 740 0.597644
+ 741 0.597644
+ 742 0.597644
+ 743 0.597644
+ 744 0.0610572
+ 745 0.0610572
+ 746 0.0610572
+ 747 0.0610572
+ 748 0.0610572
+ 749 0.0610572
+ 750 0.117041
+ 751 0.117041
+ 752 0.117041
+ 753 0.117041
+ 754 0.117041
+ 755 0.117041
+ 756 0.623138
+ 757 0.623138
+ 758 0.623138
+ 759 0.623138
+ 760 0.623138
+ 761 0.058441
+ 762 0.058441
+ 763 0.058441
+ 764 0.058441
+ 765 0.058441
+ 766 0.058441
+ 767 0.116932
+ 768 0.116932
+ 769 0.116932
+ 770 0.116932
+ 771 0.116932
+ 772 0.116932
+ 773 0.619449
+ 774 0.619449
+ 775 0.619449
+ 776 0.619449
+ 777 0.619449
+ 778 0.619449
+ 779 0.0591191
+ 780 0.0591191
+ 781 0.0591191
+ 782 0.0591191
+ 783 0.0591191
+ 784 0.0591191
+ 785 0.116482
+ 786 0.116482
+ 787 0.116482
+ 788 0.116482
+ 789 0.116482
+ 790 0.116482
+ 791 0.674294
+ 792 0.674294
+ 793 0.674294
+ 794 0.674294
+ 795 0.674294
+ 796 0.0487278
+ 797 0.0487278
+ 798 0.0487278
+ 799 0.0487278
+ 800 0.0487278
+ 801 0.0487278
+ 802 0.115685
+ 803 0.115685
+ 804 0.115685
+ 805 0.115685
+ 806 0.115685
+ 807 0.115685
+ 808 0.5969
+ 809 0.5969
+ 810 0.5969
+ 811 0.5969
+ 812 0.5969
+ 813 0.0602494
+ 814 0.0602494
+ 815 0.0602494
+ 816 0.0602494
+ 817 0.0602494
+ 818 0.0602494
+ 819 0.116266
+ 820 0.116266
+ 821 0.116266
+ 822 0.116266
+ 823 0.116266
+ 824 0.116266
+ 825 0.589349
+ 826 0.589349
+ 827 0.589349
+ 828 0.589349
+ 829 0.589349
+ 830 0.0617826
+ 831 0.0617826
+ 832 0.0617826
+ 833 0.0617826
+ 834 0.0617826
+ 835 0.116548
+ 836 0.116548
+ 837 0.116548
+ 838 0.116548
+ 839 0.116548
+ 840 0.116548
+ 841 0.570853
+ 842 0.570853
+ 843 0.570853
+ 844 0.570853
+ 845 0.570853
+ 846 0.570853
+ 847 0.0593951
+ 848 0.0593951
+ 849 0.0593951
+ 850 0.0593951
+ 851 0.0593951
+ 852 0.404638
+ 853 0.404638
+ 854 0.404638
+ 855 0.404638
+ 856 0.404638
+ 857 0.404638
+ 858 0.404639
+ 859 0.404639
+ 860 0.404639
+ 861 0.404639
+ 862 0.404639
+ 863 0.404639
+ 864 0.5101
+ 865 0.5101
+ 866 0.5101
+ 867 0.5101
+ 868 0.5101
+ 869 0.5101
+ 870 0.116803
+ 871 0.116803
+ 872 0.116803
+ 873 0.116803
+ 874 0.116803
+ 875 0.116803
+ 876 0.404444
+ 877 0.404444
+ 878 0.404444
+ 879 0.404444
+ 880 0.404444
+ 881 0.404444
+ 882 0.404521
+ 883 0.404521
+ 884 0.404521
+ 885 0.404521
+ 886 0.404521
+ 887 0.404521
+ 888 0.506148
+ 889 0.506148
+ 890 0.506148
+ 891 0.506148
+ 892 0.506148
+ 893 0.506148
+ 894 0.118404
+ 895 0.118404
+ 896 0.118404
+ 897 0.118404
+ 898 0.118404
+ 899 0.118404
+ 900 0.404315
+ 901 0.404315
+ 902 0.404315
+ 903 0.404315
+ 904 0.404315
+ 905 0.404315
+ 906 0.404378
+ 907 0.404378
+ 908 0.404378
+ 909 0.404378
+ 910 0.404378
+ 911 0.404378
+ 912 0.505019
+ 913 0.505019
+ 914 0.505019
+ 915 0.505019
+ 916 0.505019
+ 917 0.505019
+ 918 0.118446
+ 919 0.118446
+ 920 0.118446
+ 921 0.118446
+ 922 0.118446
+ 923 0.118446
+ 924 0.404965
+ 925 0.404965
+ 926 0.404965
+ 927 0.404965
+ 928 0.404965
+ 929 0.404965
+ 930 0.404526
+ 931 0.404526
+ 932 0.404526
+ 933 0.404526
+ 934 0.404526
+ 935 0.404526
+ 936 0.507029
+ 937 0.507029
+ 938 0.507029
+ 939 0.507029
+ 940 0.507029
+ 941 0.507029
+ 942 0.118214
+ 943 0.118214
+ 944 0.118214
+ 945 0.118214
+ 946 0.118214
+ 947 0.118214
+ 948 0.405236
+ 949 0.405236
+ 950 0.405236
+ 951 0.405236
+ 952 0.405236
+ 953 0.405236
+ 954 0.405311
+ 955 0.405311
+ 956 0.405311
+ 957 0.405311
+ 958 0.405311
+ 959 0.405311
+ 960 0.512392
+ 961 0.512392
+ 962 0.512392
+ 963 0.512392
+ 964 0.512392
+ 965 0.512392
+ 966 0.116842
+ 967 0.116842
+ 968 0.116842
+ 969 0.116842
+ 970 0.116842
+ 971 0.116842
+ 972 0.40534
+ 973 0.40534
+ 974 0.40534
+ 975 0.40534
+ 976 0.40534
+ 977 0.40534
+ 978 0.40518
+ 979 0.40518
+ 980 0.40518
+ 981 0.40518
+ 982 0.40518
+ 983 0.40518
+ 984 0.510807
+ 985 0.510807
+ 986 0.510807
+ 987 0.510807
+ 988 0.510807
+ 989 0.510807
+ 990 0.116457
+ 991 0.116457
+ 992 0.116457
+ 993 0.116457
+ 994 0.116457
+ 995 0.116457
+ 996 0.405873
+ 997 0.405873
+ 998 0.405873
+ 999 0.405873
+ 1000 0.405873
+ 1001 0.405873
+ 1002 0.405617
+ 1003 0.405617
+ 1004 0.405617
+ 1005 0.405617
+ 1006 0.405617
+ 1007 0.405617
+ 1008 0.513382
+ 1009 0.513382
+ 1010 0.513382
+ 1011 0.513382
+ 1012 0.513382
+ 1013 0.513382
+ 1014 0.116536
+ 1015 0.116536
+ 1016 0.116536
+ 1017 0.116536
+ 1018 0.116536
+ 1019 0.116536
+ 1020 0.406472
+ 1021 0.406472
+ 1022 0.406472
+ 1023 0.406472
+ 1024 0.406472
+ 1025 0.406472
+ 1026 0.406184
+ 1027 0.406184
+ 1028 0.406184
+ 1029 0.406184
+ 1030 0.406184
+ 1031 0.406184
+ 1032 0.516566
+ 1033 0.516566
+ 1034 0.516566
+ 1035 0.516566
+ 1036 0.516566
+ 1037 0.516566
+ 1038 0.11641
+ 1039 0.11641
+ 1040 0.11641
+ 1041 0.11641
+ 1042 0.11641
+ 1043 0.11641
+ 1044 0.405684
+ 1045 0.405684
+ 1046 0.405684
+ 1047 0.405684
+ 1048 0.405684
+ 1049 0.405684
+ 1050 0.406396
+ 1051 0.406396
+ 1052 0.406396
+ 1053 0.406396
+ 1054 0.406396
+ 1055 0.406396
+ 1056 0.517821
+ 1057 0.517821
+ 1058 0.517821
+ 1059 0.517821
+ 1060 0.517821
+ 1061 0.517821
+ 1062 0.115424
+ 1063 0.115424
+ 1064 0.115424
+ 1065 0.115424
+ 1066 0.115424
+ 1067 0.115424
+ 1068 0.404072
+ 1069 0.404072
+ 1070 0.404072
+ 1071 0.404072
+ 1072 0.404072
+ 1073 0.404072
+ 1074 0.404785
+ 1075 0.404785
+ 1076 0.404785
+ 1077 0.404785
+ 1078 0.404785
+ 1079 0.404785
+ 1080 0.507313
+ 1081 0.507313
+ 1082 0.507313
+ 1083 0.507313
+ 1084 0.507313
+ 1085 0.507313
+ 1086 0.115527
+ 1087 0.115527
+ 1088 0.115527
+ 1089 0.115527
+ 1090 0.115527
+ 1091 0.115527
+ 1092 0.115527
+ 1093 0.404256
+ 1094 0.404256
+ 1095 0.404256
+ 1096 0.404256
+ 1097 0.404256
+ 1098 0.404256
+ 1099 0.403785
+ 1100 0.403785
+ 1101 0.403785
+ 1102 0.403785
+ 1103 0.403785
+ 1104 0.403785
+ 1105 0.50667
+ 1106 0.50667
+ 1107 0.50667
+ 1108 0.50667
+ 1109 0.50667
+ 1110 0.50667
+ 1111 0.116069
+ 1112 0.116069
+ 1113 0.116069
+ 1114 0.116069
+ 1115 0.116069
+ 1116 0.116069
+ 1117 0.403544
+ 1118 0.403544
+ 1119 0.403544
+ 1120 0.403544
+ 1121 0.403544
+ 1122 0.403544
+ 1123 0.403687
+ 1124 0.403687
+ 1125 0.403687
+ 1126 0.403687
+ 1127 0.403687
+ 1128 0.403687
+ 1129 0.503588
+ 1130 0.503588
+ 1131 0.503588
+ 1132 0.503588
+ 1133 0.503588
+ 1134 0.503588
+ 1135 0.115894
+ 1136 0.115894
+ 1137 0.115894
+ 1138 0.115894
+ 1139 0.115894
+ 1140 0.115894
+ 1141 0.117496
+ 1142 0.117496
+ 1143 0.117496
+ 1144 0.117496
+ 1145 0.117496
+ 1146 0.117496
+ 1147 0.118709
+ 1148 0.118709
+ 1149 0.118709
+ 1150 0.118709
+ 1151 0.118709
+ 1152 0.118709
+ 1153 0.11847
+ 1154 0.11847
+ 1155 0.11847
+ 1156 0.11847
+ 1157 0.11847
+ 1158 0.11847
+ 1159 0.118171
+ 1160 0.118171
+ 1161 0.118171
+ 1162 0.118171
+ 1163 0.118171
+ 1164 0.118171
+ 1165 0.116581
+ 1166 0.116581
+ 1167 0.116581
+ 1168 0.116581
+ 1169 0.116581
+ 1170 0.116581
+ 1171 0.116727
+ 1172 0.116727
+ 1173 0.116727
+ 1174 0.116727
+ 1175 0.116727
+ 1176 0.116727
+ 1177 0.116657
+ 1178 0.116657
+ 1179 0.116657
+ 1180 0.116657
+ 1181 0.116657
+ 1182 0.116657
+ 1183 0.116443
+ 1184 0.116443
+ 1185 0.116443
+ 1186 0.116443
+ 1187 0.116443
+ 1188 0.116443
+ 1189 0.115361
+ 1190 0.115361
+ 1191 0.115361
+ 1192 0.115361
+ 1193 0.115361
+ 1194 0.115361
+ 1195 0.115911
+ 1196 0.115911
+ 1197 0.115911
+ 1198 0.115911
+ 1199 0.115911
+ 1200 0.115911
+ 1201 0.11626
+ 1202 0.11626
+ 1203 0.11626
+ 1204 0.11626
+ 1205 0.11626
+ 1206 0.11626
+ 1207 0.116221
+ 1208 0.116221
+ 1209 0.116221
+ 1210 0.116221
+ 1211 0.116221
+ 1212 0.116221
+ 1213 0.508977
+ 1214 0.508977
+ 1215 0.508977
+ 1216 0.508977
+ 1217 0.508977
+ 1218 0.508977
+ 1219 0.511594
+ 1220 0.511594
+ 1221 0.511594
+ 1222 0.511594
+ 1223 0.511594
+ 1224 0.511594
+ 1225 0.116476
+ 1226 0.116476
+ 1227 0.116476
+ 1228 0.116476
+ 1229 0.116476
+ 1230 0.116476
+ 1231 0.506393
+ 1232 0.506393
+ 1233 0.506393
+ 1234 0.506393
+ 1235 0.506393
+ 1236 0.506393
+ 1237 0.507048
+ 1238 0.507048
+ 1239 0.507048
+ 1240 0.507048
+ 1241 0.507048
+ 1242 0.507048
+ 1243 0.118316
+ 1244 0.118316
+ 1245 0.118316
+ 1246 0.118316
+ 1247 0.118316
+ 1248 0.118316
+ 1249 0.504857
+ 1250 0.504857
+ 1251 0.504857
+ 1252 0.504857
+ 1253 0.504857
+ 1254 0.504857
+ 1255 0.506248
+ 1256 0.506248
+ 1257 0.506248
+ 1258 0.506248
+ 1259 0.506248
+ 1260 0.506248
+ 1261 0.118738
+ 1262 0.118738
+ 1263 0.118738
+ 1264 0.118738
+ 1265 0.118738
+ 1266 0.118738
+ 1267 0.50935
+ 1268 0.50935
+ 1269 0.50935
+ 1270 0.50935
+ 1271 0.50935
+ 1272 0.50935
+ 1273 0.506054
+ 1274 0.506054
+ 1275 0.506054
+ 1276 0.506054
+ 1277 0.506054
+ 1278 0.506054
+ 1279 0.11847
+ 1280 0.11847
+ 1281 0.11847
+ 1282 0.11847
+ 1283 0.11847
+ 1284 0.11847
+ 1285 0.511842
+ 1286 0.511842
+ 1287 0.511842
+ 1288 0.511842
+ 1289 0.511842
+ 1290 0.511842
+ 1291 0.513364
+ 1292 0.513364
+ 1293 0.513364
+ 1294 0.513364
+ 1295 0.513364
+ 1296 0.513364
+ 1297 0.117473
+ 1298 0.117473
+ 1299 0.117473
+ 1300 0.117473
+ 1301 0.117473
+ 1302 0.117473
+ 1303 0.511786
+ 1304 0.511786
+ 1305 0.511786
+ 1306 0.511786
+ 1307 0.511786
+ 1308 0.511786
+ 1309 0.511063
+ 1310 0.511063
+ 1311 0.511063
+ 1312 0.511063
+ 1313 0.511063
+ 1314 0.511063
+ 1315 0.116543
+ 1316 0.116543
+ 1317 0.116543
+ 1318 0.116543
+ 1319 0.116543
+ 1320 0.116543
+ 1321 0.514116
+ 1322 0.514116
+ 1323 0.514116
+ 1324 0.514116
+ 1325 0.514116
+ 1326 0.514116
+ 1327 0.513524
+ 1328 0.513524
+ 1329 0.513524
+ 1330 0.513524
+ 1331 0.513524
+ 1332 0.513524
+ 1333 0.116773
+ 1334 0.116773
+ 1335 0.116773
+ 1336 0.116773
+ 1337 0.116773
+ 1338 0.116773
+ 1339 0.518506
+ 1340 0.518506
+ 1341 0.518506
+ 1342 0.518506
+ 1343 0.518506
+ 1344 0.518506
+ 1345 0.515716
+ 1346 0.515716
+ 1347 0.515716
+ 1348 0.515716
+ 1349 0.515716
+ 1350 0.515716
+ 1351 0.116662
+ 1352 0.116662
+ 1353 0.116662
+ 1354 0.116662
+ 1355 0.116662
+ 1356 0.116662
+ 1357 0.515539
+ 1358 0.515539
+ 1359 0.515539
+ 1360 0.515539
+ 1361 0.515539
+ 1362 0.515539
+ 1363 0.520186
+ 1364 0.520186
+ 1365 0.520186
+ 1366 0.520186
+ 1367 0.520186
+ 1368 0.520186
+ 1369 0.115923
+ 1370 0.115923
+ 1371 0.115923
+ 1372 0.115923
+ 1373 0.115923
+ 1374 0.115923
+ 1375 0.50603
+ 1376 0.50603
+ 1377 0.50603
+ 1378 0.50603
+ 1379 0.50603
+ 1380 0.50603
+ 1381 0.510017
+ 1382 0.510017
+ 1383 0.510017
+ 1384 0.510017
+ 1385 0.510017
+ 1386 0.510017
+ 1387 0.115599
+ 1388 0.115599
+ 1389 0.115599
+ 1390 0.115599
+ 1391 0.115599
+ 1392 0.508396
+ 1393 0.508396
+ 1394 0.508396
+ 1395 0.508396
+ 1396 0.508396
+ 1397 0.508396
+ 1398 0.505024
+ 1399 0.505024
+ 1400 0.505024
+ 1401 0.505024
+ 1402 0.505024
+ 1403 0.505024
+ 1404 0.116323
+ 1405 0.116323
+ 1406 0.116323
+ 1407 0.116323
+ 1408 0.116323
+ 1409 0.116323
+ 1410 0.503336
+ 1411 0.503336
+ 1412 0.503336
+ 1413 0.503336
+ 1414 0.503336
+ 1415 0.503336
+ 1416 0.504887
+ 1417 0.504887
+ 1418 0.504887
+ 1419 0.504887
+ 1420 0.504887
+ 1421 0.504887
+ 1422 0.116081
+ 1423 0.116081
+ 1424 0.116081
+ 1425 0.116081
+ 1426 0.116081
+ 1427 0.116081
+ }
+ VertexInfluence "Bone_003" 1032 {
+ 4 0.278159
+ 5 0.278159
+ 6 0.278159
+ 7 0.278159
+ 12 0.24515
+ 13 0.24515
+ 14 0.24515
+ 15 0.24515
+ 20 0.23507
+ 21 0.23507
+ 22 0.23507
+ 23 0.23507
+ 28 0.224104
+ 29 0.224104
+ 30 0.224104
+ 31 0.224104
+ 36 0.233595
+ 37 0.233595
+ 38 0.233595
+ 39 0.233595
+ 44 0.258175
+ 45 0.258175
+ 46 0.258175
+ 47 0.258175
+ 52 0.309799
+ 53 0.309799
+ 54 0.309799
+ 55 0.309799
+ 60 0.317662
+ 61 0.317662
+ 62 0.317662
+ 63 0.317662
+ 68 0.320206
+ 69 0.320206
+ 70 0.320206
+ 71 0.320206
+ 76 0.311216
+ 77 0.311216
+ 78 0.311216
+ 79 0.311216
+ 84 0.357141
+ 85 0.357141
+ 86 0.357141
+ 87 0.357141
+ 92 0.321442
+ 93 0.321442
+ 94 0.321442
+ 95 0.321442
+ 103 0.246048
+ 104 0.246048
+ 105 0.246048
+ 106 0.246048
+ 107 0.246048
+ 108 0.246048
+ 109 0.246048
+ 110 0.090729
+ 111 0.090729
+ 112 0.090729
+ 113 0.090729
+ 114 0.090729
+ 115 0.090729
+ 116 0.0887967
+ 117 0.0887967
+ 118 0.0887967
+ 119 0.0887967
+ 120 0.0887967
+ 121 0.0887967
+ 132 0.243541
+ 133 0.243541
+ 134 0.243541
+ 135 0.243541
+ 136 0.243541
+ 137 0.243541
+ 138 0.243541
+ 139 0.243541
+ 140 0.243541
+ 141 0.0878763
+ 142 0.0878763
+ 143 0.0878763
+ 144 0.0878763
+ 145 0.0878763
+ 146 0.0878763
+ 154 0.228804
+ 155 0.228804
+ 156 0.228804
+ 157 0.228804
+ 158 0.228804
+ 159 0.228804
+ 160 0.228804
+ 161 0.228804
+ 162 0.0875306
+ 163 0.0875306
+ 164 0.0875306
+ 165 0.0875306
+ 166 0.0875306
+ 167 0.0875306
+ 178 0.23533
+ 179 0.23533
+ 180 0.23533
+ 181 0.23533
+ 182 0.23533
+ 183 0.23533
+ 184 0.23533
+ 185 0.23533
+ 186 0.0880036
+ 187 0.0880036
+ 188 0.0880036
+ 189 0.0880036
+ 190 0.0880036
+ 191 0.0880036
+ 200 0.23241
+ 201 0.23241
+ 202 0.23241
+ 203 0.23241
+ 204 0.23241
+ 205 0.23241
+ 206 0.23241
+ 207 0.23241
+ 208 0.0895814
+ 209 0.0895814
+ 210 0.0895814
+ 211 0.0895814
+ 212 0.0895814
+ 213 0.0895814
+ 222 0.305104
+ 223 0.305104
+ 224 0.305104
+ 225 0.305104
+ 226 0.305104
+ 227 0.305104
+ 228 0.305104
+ 229 0.305104
+ 230 0.305104
+ 231 0.305104
+ 232 0.0917889
+ 233 0.0917889
+ 234 0.0917889
+ 235 0.0917889
+ 236 0.0917889
+ 237 0.0917889
+ 246 0.325085
+ 247 0.325085
+ 248 0.325085
+ 249 0.325085
+ 250 0.325085
+ 251 0.325085
+ 252 0.325085
+ 253 0.0928968
+ 254 0.0928968
+ 255 0.0928968
+ 256 0.0928968
+ 257 0.0928968
+ 258 0.0928968
+ 268 0.336697
+ 269 0.336697
+ 270 0.336697
+ 271 0.336697
+ 272 0.336697
+ 273 0.336697
+ 274 0.336697
+ 275 0.336697
+ 276 0.336697
+ 277 0.0932003
+ 278 0.0932003
+ 279 0.0932003
+ 280 0.0932003
+ 281 0.0932003
+ 282 0.0932003
+ 290 0.315012
+ 291 0.315012
+ 292 0.315012
+ 293 0.315012
+ 294 0.315012
+ 295 0.315012
+ 296 0.315012
+ 297 0.0936636
+ 298 0.0936636
+ 299 0.0936636
+ 300 0.0936636
+ 301 0.0936636
+ 302 0.0936636
+ 314 0.325298
+ 315 0.325298
+ 316 0.325298
+ 317 0.325298
+ 318 0.325298
+ 319 0.325298
+ 320 0.325298
+ 321 0.325298
+ 322 0.0944848
+ 323 0.0944848
+ 324 0.0944848
+ 325 0.0944848
+ 326 0.0944848
+ 327 0.0944848
+ 336 0.322382
+ 337 0.322382
+ 338 0.322382
+ 339 0.322382
+ 340 0.322382
+ 341 0.322382
+ 342 0.322382
+ 343 0.322382
+ 344 0.322382
+ 345 0.322382
+ 346 0.322382
+ 347 0.322382
+ 348 0.0933232
+ 349 0.0933232
+ 350 0.0933232
+ 351 0.0933232
+ 352 0.0933232
+ 353 0.0933232
+ 363 0.298046
+ 364 0.298046
+ 365 0.298046
+ 366 0.298046
+ 367 0.298046
+ 368 0.298046
+ 369 0.298046
+ 370 0.298046
+ 371 0.0891931
+ 372 0.0891931
+ 373 0.0891931
+ 374 0.0891931
+ 375 0.0891931
+ 376 0.0891931
+ 377 0.0879207
+ 378 0.0879207
+ 379 0.0879207
+ 380 0.0879207
+ 381 0.0879207
+ 382 0.0879207
+ 383 0.0872476
+ 384 0.0872476
+ 385 0.0872476
+ 386 0.0872476
+ 387 0.0872476
+ 388 0.0872476
+ 389 0.0873352
+ 390 0.0873352
+ 391 0.0873352
+ 392 0.0873352
+ 393 0.0873352
+ 394 0.0873352
+ 395 0.0882251
+ 396 0.0882251
+ 397 0.0882251
+ 398 0.0882251
+ 399 0.0882251
+ 400 0.0882251
+ 401 0.0904285
+ 402 0.0904285
+ 403 0.0904285
+ 404 0.0904285
+ 405 0.0904285
+ 406 0.0904285
+ 407 0.092096
+ 408 0.092096
+ 409 0.092096
+ 410 0.092096
+ 411 0.092096
+ 412 0.092096
+ 413 0.0927711
+ 414 0.0927711
+ 415 0.0927711
+ 416 0.0927711
+ 417 0.0927711
+ 418 0.0927711
+ 419 0.0929004
+ 420 0.0929004
+ 421 0.0929004
+ 422 0.0929004
+ 423 0.0929004
+ 424 0.0929004
+ 425 0.0938426
+ 426 0.0938426
+ 427 0.0938426
+ 428 0.0938426
+ 429 0.0938426
+ 430 0.0938426
+ 431 0.0917399
+ 432 0.0917399
+ 433 0.0917399
+ 434 0.0917399
+ 435 0.0917399
+ 436 0.0917399
+ 437 0.0937478
+ 438 0.0937478
+ 439 0.0937478
+ 440 0.0937478
+ 441 0.0937478
+ 442 0.0937478
+ 448 0.235339
+ 449 0.235339
+ 450 0.235339
+ 451 0.235339
+ 452 0.235339
+ 453 0.235339
+ 454 0.351355
+ 455 0.351355
+ 456 0.351355
+ 457 0.351355
+ 458 0.351355
+ 459 0.351355
+ 471 0.240276
+ 472 0.240276
+ 473 0.240276
+ 474 0.240276
+ 475 0.240276
+ 476 0.240276
+ 488 0.229473
+ 489 0.229473
+ 490 0.229473
+ 491 0.229473
+ 492 0.229473
+ 493 0.229473
+ 506 0.22715
+ 507 0.22715
+ 508 0.22715
+ 509 0.22715
+ 510 0.22715
+ 511 0.22715
+ 523 0.230496
+ 524 0.230496
+ 525 0.230496
+ 526 0.230496
+ 527 0.230496
+ 528 6.19344e-005
+ 529 6.19344e-005
+ 530 6.19344e-005
+ 531 6.19344e-005
+ 532 6.19344e-005
+ 533 6.19344e-005
+ 540 0.265018
+ 541 0.265018
+ 542 0.265018
+ 543 0.265018
+ 544 0.265018
+ 545 0.265018
+ 546 0.0012902
+ 547 0.0012902
+ 548 0.0012902
+ 549 0.0012902
+ 550 0.0012902
+ 551 0.0012902
+ 557 0.301875
+ 558 0.301875
+ 559 0.301875
+ 560 0.301875
+ 561 0.301875
+ 562 0.00192118
+ 563 0.00192118
+ 564 0.00192118
+ 565 0.00192118
+ 566 0.00192118
+ 567 0.00192118
+ 574 0.311899
+ 575 0.311899
+ 576 0.311899
+ 577 0.311899
+ 578 0.311899
+ 579 0.00180375
+ 580 0.00180375
+ 581 0.00180375
+ 582 0.00180375
+ 583 0.00180375
+ 584 0.00180375
+ 591 0.302006
+ 592 0.302006
+ 593 0.302006
+ 594 0.302006
+ 595 0.302006
+ 596 0.00225085
+ 597 0.00225085
+ 598 0.00225085
+ 599 0.00225085
+ 600 0.00225085
+ 601 0.00225085
+ 608 0.342038
+ 609 0.342038
+ 610 0.342038
+ 611 0.342038
+ 612 0.342038
+ 613 0.342038
+ 614 0.0025655
+ 615 0.0025655
+ 616 0.0025655
+ 617 0.0025655
+ 618 0.0025655
+ 619 0.0025655
+ 625 0.361833
+ 626 0.361833
+ 627 0.361833
+ 628 0.361833
+ 629 0.361833
+ 630 0.00196952
+ 631 0.00196952
+ 632 0.00196952
+ 633 0.00196952
+ 634 0.00196952
+ 635 0.00196952
+ 641 0.334119
+ 642 0.334119
+ 643 0.334119
+ 644 0.334119
+ 645 0.334119
+ 646 0.334119
+ 653 0.243288
+ 654 0.243288
+ 655 0.243288
+ 656 0.243288
+ 657 0.243288
+ 658 0.000443954
+ 659 0.000443954
+ 660 0.000443954
+ 661 0.000443954
+ 662 0.000443954
+ 663 0.000443954
+ 664 0.343379
+ 665 0.343379
+ 666 0.343379
+ 667 0.343379
+ 668 0.343379
+ 669 0.343379
+ 676 0.23346
+ 677 0.23346
+ 678 0.23346
+ 679 0.23346
+ 680 0.23346
+ 681 0.340088
+ 682 0.340088
+ 683 0.340088
+ 684 0.340088
+ 685 0.340088
+ 686 0.340088
+ 693 0.213244
+ 694 0.213244
+ 695 0.213244
+ 696 0.213244
+ 697 0.213244
+ 698 0.338361
+ 699 0.338361
+ 700 0.338361
+ 701 0.338361
+ 702 0.338361
+ 703 0.338361
+ 710 0.229394
+ 711 0.229394
+ 712 0.229394
+ 713 0.229394
+ 714 0.229394
+ 715 0.229394
+ 716 0.340395
+ 717 0.340395
+ 718 0.340395
+ 719 0.340395
+ 720 0.340395
+ 721 0.340395
+ 727 0.219563
+ 728 0.219563
+ 729 0.219563
+ 730 0.219563
+ 731 0.219563
+ 732 0.346827
+ 733 0.346827
+ 734 0.346827
+ 735 0.346827
+ 736 0.346827
+ 737 0.346827
+ 744 0.290454
+ 745 0.290454
+ 746 0.290454
+ 747 0.290454
+ 748 0.290454
+ 749 0.290454
+ 750 0.35751
+ 751 0.35751
+ 752 0.35751
+ 753 0.35751
+ 754 0.35751
+ 755 0.35751
+ 761 0.296729
+ 762 0.296729
+ 763 0.296729
+ 764 0.296729
+ 765 0.296729
+ 766 0.296729
+ 767 0.36131
+ 768 0.36131
+ 769 0.36131
+ 770 0.36131
+ 771 0.36131
+ 772 0.36131
+ 779 0.311946
+ 780 0.311946
+ 781 0.311946
+ 782 0.311946
+ 783 0.311946
+ 784 0.311946
+ 785 0.361974
+ 786 0.361974
+ 787 0.361974
+ 788 0.361974
+ 789 0.361974
+ 790 0.361974
+ 796 0.274672
+ 797 0.274672
+ 798 0.274672
+ 799 0.274672
+ 800 0.274672
+ 801 0.274672
+ 802 0.362919
+ 803 0.362919
+ 804 0.362919
+ 805 0.362919
+ 806 0.362919
+ 807 0.362919
+ 813 0.363146
+ 814 0.363146
+ 815 0.363146
+ 816 0.363146
+ 817 0.363146
+ 818 0.363146
+ 819 0.369134
+ 820 0.369134
+ 821 0.369134
+ 822 0.369134
+ 823 0.369134
+ 824 0.369134
+ 830 0.345435
+ 831 0.345435
+ 832 0.345435
+ 833 0.345435
+ 834 0.345435
+ 835 0.363526
+ 836 0.363526
+ 837 0.363526
+ 838 0.363526
+ 839 0.363526
+ 840 0.363526
+ 847 0.290937
+ 848 0.290937
+ 849 0.290937
+ 850 0.290937
+ 851 0.290937
+ 852 0.0889319
+ 853 0.0889319
+ 854 0.0889319
+ 855 0.0889319
+ 856 0.0889319
+ 857 0.0889319
+ 858 0.0898799
+ 859 0.0898799
+ 860 0.0898799
+ 861 0.0898799
+ 862 0.0898799
+ 863 0.0898799
+ 870 0.346496
+ 871 0.346496
+ 872 0.346496
+ 873 0.346496
+ 874 0.346496
+ 875 0.346496
+ 876 0.0878627
+ 877 0.0878627
+ 878 0.0878627
+ 879 0.0878627
+ 880 0.0878627
+ 881 0.0878627
+ 882 0.0883173
+ 883 0.0883173
+ 884 0.0883173
+ 885 0.0883173
+ 886 0.0883173
+ 887 0.0883173
+ 894 0.342477
+ 895 0.342477
+ 896 0.342477
+ 897 0.342477
+ 898 0.342477
+ 899 0.342477
+ 900 0.0873327
+ 901 0.0873327
+ 902 0.0873327
+ 903 0.0873327
+ 904 0.0873327
+ 905 0.0873327
+ 906 0.0875221
+ 907 0.0875221
+ 908 0.0875221
+ 909 0.0875221
+ 910 0.0875221
+ 911 0.0875221
+ 918 0.33948
+ 919 0.33948
+ 920 0.33948
+ 921 0.33948
+ 922 0.33948
+ 923 0.33948
+ 924 0.0876242
+ 925 0.0876242
+ 926 0.0876242
+ 927 0.0876242
+ 928 0.0876242
+ 929 0.0876242
+ 930 0.0873905
+ 931 0.0873905
+ 932 0.0873905
+ 933 0.0873905
+ 934 0.0873905
+ 935 0.0873905
+ 942 0.340208
+ 943 0.340208
+ 944 0.340208
+ 945 0.340208
+ 946 0.340208
+ 947 0.340208
+ 948 0.0888228
+ 949 0.0888228
+ 950 0.0888228
+ 951 0.0888228
+ 952 0.0888228
+ 953 0.0888228
+ 954 0.0880492
+ 955 0.0880492
+ 956 0.0880492
+ 957 0.0880492
+ 958 0.0880492
+ 959 0.0880492
+ 966 0.342818
+ 967 0.342818
+ 968 0.342818
+ 969 0.342818
+ 970 0.342818
+ 971 0.342818
+ 972 0.091122
+ 973 0.091122
+ 974 0.091122
+ 975 0.091122
+ 976 0.091122
+ 977 0.091122
+ 978 0.0899847
+ 979 0.0899847
+ 980 0.0899847
+ 981 0.0899847
+ 982 0.0899847
+ 983 0.0899847
+ 984 0.000533675
+ 985 0.000533675
+ 986 0.000533675
+ 987 0.000533675
+ 988 0.000533675
+ 989 0.000533675
+ 990 0.354177
+ 991 0.354177
+ 992 0.354177
+ 993 0.354177
+ 994 0.354177
+ 995 0.354177
+ 996 0.0924905
+ 997 0.0924905
+ 998 0.0924905
+ 999 0.0924905
+ 1000 0.0924905
+ 1001 0.0924905
+ 1002 0.091955
+ 1003 0.091955
+ 1004 0.091955
+ 1005 0.091955
+ 1006 0.091955
+ 1007 0.091955
+ 1008 0.00144082
+ 1009 0.00144082
+ 1010 0.00144082
+ 1011 0.00144082
+ 1012 0.00144082
+ 1013 0.00144082
+ 1014 0.360798
+ 1015 0.360798
+ 1016 0.360798
+ 1017 0.360798
+ 1018 0.360798
+ 1019 0.360798
+ 1020 0.0929747
+ 1021 0.0929747
+ 1022 0.0929747
+ 1023 0.0929747
+ 1024 0.0929747
+ 1025 0.0929747
+ 1026 0.0928345
+ 1027 0.0928345
+ 1028 0.0928345
+ 1029 0.0928345
+ 1030 0.0928345
+ 1031 0.0928345
+ 1032 0.00169879
+ 1033 0.00169879
+ 1034 0.00169879
+ 1035 0.00169879
+ 1036 0.00169879
+ 1037 0.00169879
+ 1038 0.363388
+ 1039 0.363388
+ 1040 0.363388
+ 1041 0.363388
+ 1042 0.363388
+ 1043 0.363388
+ 1044 0.0932003
+ 1045 0.0932003
+ 1046 0.0932003
+ 1047 0.0932003
+ 1048 0.0932003
+ 1049 0.0932003
+ 1050 0.093003
+ 1051 0.093003
+ 1052 0.093003
+ 1053 0.093003
+ 1054 0.093003
+ 1055 0.093003
+ 1056 0.00166017
+ 1057 0.00166017
+ 1058 0.00166017
+ 1059 0.00166017
+ 1060 0.00166017
+ 1061 0.00166017
+ 1062 0.361931
+ 1063 0.361931
+ 1064 0.361931
+ 1065 0.361931
+ 1066 0.361931
+ 1067 0.361931
+ 1068 0.0942117
+ 1069 0.0942117
+ 1070 0.0942117
+ 1071 0.0942117
+ 1072 0.0942117
+ 1073 0.0942117
+ 1074 0.0937487
+ 1075 0.0937487
+ 1076 0.0937487
+ 1077 0.0937487
+ 1078 0.0937487
+ 1079 0.0937487
+ 1080 0.00229049
+ 1081 0.00229049
+ 1082 0.00229049
+ 1083 0.00229049
+ 1084 0.00229049
+ 1085 0.00229049
+ 1086 0.36739
+ 1087 0.36739
+ 1088 0.36739
+ 1089 0.36739
+ 1090 0.36739
+ 1091 0.36739
+ 1092 0.36739
+ 1093 0.0912135
+ 1094 0.0912135
+ 1095 0.0912135
+ 1096 0.0912135
+ 1097 0.0912135
+ 1098 0.0912135
+ 1099 0.0925583
+ 1100 0.0925583
+ 1101 0.0925583
+ 1102 0.0925583
+ 1103 0.0925583
+ 1104 0.0925583
+ 1105 0.00101537
+ 1106 0.00101537
+ 1107 0.00101537
+ 1108 0.00101537
+ 1109 0.00101537
+ 1110 0.00101537
+ 1111 0.358702
+ 1112 0.358702
+ 1113 0.358702
+ 1114 0.358702
+ 1115 0.358702
+ 1116 0.358702
+ 1117 0.0935701
+ 1118 0.0935701
+ 1119 0.0935701
+ 1120 0.0935701
+ 1121 0.0935701
+ 1122 0.0935701
+ 1123 0.0941694
+ 1124 0.0941694
+ 1125 0.0941694
+ 1126 0.0941694
+ 1127 0.0941694
+ 1128 0.0941694
+ 1129 0.00214052
+ 1130 0.00214052
+ 1131 0.00214052
+ 1132 0.00214052
+ 1133 0.00214052
+ 1134 0.00214052
+ 1135 0.367403
+ 1136 0.367403
+ 1137 0.367403
+ 1138 0.367403
+ 1139 0.367403
+ 1140 0.367403
+ 1141 0.344778
+ 1142 0.344778
+ 1143 0.344778
+ 1144 0.344778
+ 1145 0.344778
+ 1146 0.344778
+ 1147 0.341213
+ 1148 0.341213
+ 1149 0.341213
+ 1150 0.341213
+ 1151 0.341213
+ 1152 0.341213
+ 1153 0.338537
+ 1154 0.338537
+ 1155 0.338537
+ 1156 0.338537
+ 1157 0.338537
+ 1158 0.338537
+ 1159 0.340241
+ 1160 0.340241
+ 1161 0.340241
+ 1162 0.340241
+ 1163 0.340241
+ 1164 0.340241
+ 1165 0.344023
+ 1166 0.344023
+ 1167 0.344023
+ 1168 0.344023
+ 1169 0.344023
+ 1170 0.344023
+ 1171 0.355873
+ 1172 0.355873
+ 1173 0.355873
+ 1174 0.355873
+ 1175 0.355873
+ 1176 0.355873
+ 1177 0.360703
+ 1178 0.360703
+ 1179 0.360703
+ 1180 0.360703
+ 1181 0.360703
+ 1182 0.360703
+ 1183 0.362605
+ 1184 0.362605
+ 1185 0.362605
+ 1186 0.362605
+ 1187 0.362605
+ 1188 0.362605
+ 1189 0.361302
+ 1190 0.361302
+ 1191 0.361302
+ 1192 0.361302
+ 1193 0.361302
+ 1194 0.361302
+ 1195 0.369038
+ 1196 0.369038
+ 1197 0.369038
+ 1198 0.369038
+ 1199 0.369038
+ 1200 0.369038
+ 1201 0.355162
+ 1202 0.355162
+ 1203 0.355162
+ 1204 0.355162
+ 1205 0.355162
+ 1206 0.355162
+ 1207 0.366015
+ 1208 0.366015
+ 1209 0.366015
+ 1210 0.366015
+ 1211 0.366015
+ 1212 0.366015
+ 1225 0.34812
+ 1226 0.34812
+ 1227 0.34812
+ 1228 0.34812
+ 1229 0.34812
+ 1230 0.34812
+ 1243 0.342883
+ 1244 0.342883
+ 1245 0.342883
+ 1246 0.342883
+ 1247 0.342883
+ 1248 0.342883
+ 1261 0.339765
+ 1262 0.339765
+ 1263 0.339765
+ 1264 0.339765
+ 1265 0.339765
+ 1266 0.339765
+ 1279 0.33926
+ 1280 0.33926
+ 1281 0.33926
+ 1282 0.33926
+ 1283 0.33926
+ 1284 0.33926
+ 1297 0.341455
+ 1298 0.341455
+ 1299 0.341455
+ 1300 0.341455
+ 1301 0.341455
+ 1302 0.341455
+ 1303 0.000918913
+ 1304 0.000918913
+ 1305 0.000918913
+ 1306 0.000918913
+ 1307 0.000918913
+ 1308 0.000918913
+ 1309 0.000292073
+ 1310 0.000292073
+ 1311 0.000292073
+ 1312 0.000292073
+ 1313 0.000292073
+ 1314 0.000292073
+ 1315 0.350388
+ 1316 0.350388
+ 1317 0.350388
+ 1318 0.350388
+ 1319 0.350388
+ 1320 0.350388
+ 1321 0.00168926
+ 1322 0.00168926
+ 1323 0.00168926
+ 1324 0.00168926
+ 1325 0.00168926
+ 1326 0.00168926
+ 1327 0.00134689
+ 1328 0.00134689
+ 1329 0.00134689
+ 1330 0.00134689
+ 1331 0.00134689
+ 1332 0.00134689
+ 1333 0.359153
+ 1334 0.359153
+ 1335 0.359153
+ 1336 0.359153
+ 1337 0.359153
+ 1338 0.359153
+ 1339 0.00173533
+ 1340 0.00173533
+ 1341 0.00173533
+ 1342 0.00173533
+ 1343 0.00173533
+ 1344 0.00173533
+ 1345 0.00181556
+ 1346 0.00181556
+ 1347 0.00181556
+ 1348 0.00181556
+ 1349 0.00181556
+ 1350 0.00181556
+ 1351 0.362299
+ 1352 0.362299
+ 1353 0.362299
+ 1354 0.362299
+ 1355 0.362299
+ 1356 0.362299
+ 1357 0.00192899
+ 1358 0.00192899
+ 1359 0.00192899
+ 1360 0.00192899
+ 1361 0.00192899
+ 1362 0.00192899
+ 1363 0.00163853
+ 1364 0.00163853
+ 1365 0.00163853
+ 1366 0.00163853
+ 1367 0.00163853
+ 1368 0.00163853
+ 1369 0.361682
+ 1370 0.361682
+ 1371 0.361682
+ 1372 0.361682
+ 1373 0.361682
+ 1374 0.361682
+ 1375 0.00244343
+ 1376 0.00244343
+ 1377 0.00244343
+ 1378 0.00244343
+ 1379 0.00244343
+ 1380 0.00244343
+ 1381 0.00227117
+ 1382 0.00227117
+ 1383 0.00227117
+ 1384 0.00227117
+ 1385 0.00227117
+ 1386 0.00227117
+ 1387 0.365688
+ 1388 0.365688
+ 1389 0.365688
+ 1390 0.365688
+ 1391 0.365688
+ 1392 0.000714292
+ 1393 0.000714292
+ 1394 0.000714292
+ 1395 0.000714292
+ 1396 0.000714292
+ 1397 0.000714292
+ 1398 0.00150722
+ 1399 0.00150722
+ 1400 0.00150722
+ 1401 0.00150722
+ 1402 0.00150722
+ 1403 0.00150722
+ 1404 0.36182
+ 1405 0.36182
+ 1406 0.36182
+ 1407 0.36182
+ 1408 0.36182
+ 1409 0.36182
+ 1410 0.00207305
+ 1411 0.00207305
+ 1412 0.00207305
+ 1413 0.00207305
+ 1414 0.00207305
+ 1415 0.00207305
+ 1416 0.00234353
+ 1417 0.00234353
+ 1418 0.00234353
+ 1419 0.00234353
+ 1420 0.00234353
+ 1421 0.00234353
+ 1422 0.368993
+ 1423 0.368993
+ 1424 0.368993
+ 1425 0.368993
+ 1426 0.368993
+ 1427 0.368993
+ }
+ }
+ SourceGeometry TRUE {
+ osg::Geometry {
+ UniqueID 23
+ PrimitiveSetList 1 {
+ DrawElementsUInt GL_TRIANGLES 0 1428 {
+ 1141 653 12 1147
+ 676 20 1153 693
+ 28 1159 710 36
+ 1165 727 44 1171
+ 744 52 1177 761
+ 60 1183 779 68
+ 1189 796 76 1195
+ 813 84 762 246
+ 268 1201 847 4
+ 1207 830 92 303
+ 168 259 1213 852
+ 116 1219 858 371
+ 1225 448 103 1231
+ 876 141 1237 882
+ 377 1243 471 132
+ 1249 900 162 1255
+ 906 383 1261 488
+ 154 1267 924 186
+ 1273 930 389 1279
+ 506 178 1285 948
+ 208 1291 954 395
+ 1297 523 200 1303
+ 972 232 1309 978
+ 401 1315 540 222
+ 1321 996 253 1327
+ 1002 407 1333 557
+ 247 1339 1020 277
+ 1345 1026 413 1351
+ 574 269 1357 1044
+ 297 1363 1050 419
+ 1369 591 290 1375
+ 1068 322 1381 1074
+ 425 608 314 1086
+ 1392 1093 110 1398
+ 1099 431 1404 641
+ 363 1410 1117 348
+ 1416 1123 437 1422
+ 625 336 853 1142
+ 664 372 870 1143
+ 871 104 654 877
+ 1148 681 378 894
+ 1149 895 133 677
+ 901 1154 698 384
+ 918 1155 919 155
+ 694 925 1160 716
+ 390 942 1161 943
+ 179 711 949 1166
+ 732 396 966 1167
+ 967 201 728 973
+ 1172 750 402 990
+ 1173 991 223 745
+ 997 1178 767 408
+ 1014 1179 1015 248
+ 763 1021 1184 785
+ 414 1038 1185 1039
+ 270 780 1045 1190
+ 802 420 1062 1191
+ 1063 291 797 1069
+ 1196 819 426 1087
+ 1197 1088 315 814
+ 1094 1202 454 432
+ 1111 1203 1112 364
+ 848 1118 1208 835
+ 438 1135 1209 1136
+ 337 831 443 1214
+ 460 96 864 1215
+ 865 373 854 647
+ 1220 866 0 658
+ 1221 659 111 859
+ 860 1226 872 112
+ 455 1227 456 5
+ 449 466 1232 477
+ 122 888 1233 889
+ 379 878 670 1238
+ 890 8 461 1239
+ 462 117 883 884
+ 1244 896 118 665
+ 1245 666 13 472
+ 483 1250 494 147
+ 912 1251 913 385
+ 902 687 1256 914
+ 16 478 1257 479
+ 142 907 908 1262
+ 920 143 682 1263
+ 683 21 489 500
+ 1268 512 169 936
+ 1269 937 391 926
+ 704 1274 938 24
+ 495 1275 496 163
+ 931 932 1280 944
+ 164 699 1281 700
+ 29 507 518 1286
+ 528 192 960 1287
+ 961 397 950 722
+ 1292 962 32 513
+ 1293 514 187 955
+ 956 1298 968 188
+ 717 1299 718 37
+ 524 534 1304 546
+ 214 984 1305 985
+ 403 974 738 1310
+ 986 40 529 1311
+ 530 209 979 980
+ 1316 992 210 733
+ 1317 734 45 541
+ 552 1322 562 238
+ 1008 1323 1009 409
+ 998 756 1328 1010
+ 48 547 1329 548
+ 233 1003 1004 1334
+ 1016 234 751 1335
+ 752 53 558 568
+ 1340 579 260 1032
+ 1341 1033 415 1022
+ 773 1346 1034 56
+ 563 1347 564 254
+ 1027 1028 1352 1040
+ 255 768 1353 769
+ 61 575 585 1358
+ 596 283 1056 1359
+ 1057 421 1046 791
+ 1364 1058 64 580
+ 1365 581 278 1051
+ 1052 1370 1064 279
+ 786 1371 787 69
+ 592 602 1376 614
+ 304 1080 1377 1081
+ 427 1070 808 1382
+ 1082 72 597 1383
+ 598 298 1075 1076
+ 1387 1089 299 803
+ 1388 804 77 609
+ 636 1393 660 354
+ 1105 1394 1106 433
+ 1095 841 1399 1107
+ 88 630 1400 631
+ 349 1100 1101 1405
+ 1113 350 836 1406
+ 837 93 642 620
+ 1411 632 328 1129
+ 1412 1130 439 1119
+ 825 1417 1131 80
+ 615 1418 616 323
+ 1124 1125 1423 1137
+ 324 820 1424 821
+ 85 626 667 1144
+ 14 684 1150 22
+ 701 1156 30 719
+ 1162 38 735 1168
+ 46 753 1174 54
+ 770 1180 62 788
+ 1186 70 805 1192
+ 78 822 1198 86
+ 473 15 655 105
+ 450 365 451 6
+ 849 798 316 610
+ 338 643 832 87
+ 815 627 317 339
+ 816 79 799 611
+ 593 781 292 594
+ 71 782 576 764
+ 271 577 63 765
+ 559 746 249 560
+ 55 747 729 202
+ 542 543 47 730
+ 525 712 203 526
+ 39 713 695 156
+ 508 509 31 696
+ 678 134 490 491
+ 23 679 474 106
+ 135 714 180 204
+ 452 850 366 644
+ 94 833 817 340
+ 628 748 224 250
+ 157 136 341 137
+ 158 492 475 656
+ 107 457 1204 7
+ 159 181 510 783
+ 272 293 182 160
+ 342 318 273 225
+ 343 138 367 800
+ 294 319 295 274
+ 320 251 226 275
+ 205 183 227 321
+ 228 344 229 184
+ 345 206 230 544
+ 139 108 368 346
+ 369 645 838 1210
+ 95 637 1 648
+ 444 671 97 445
+ 9 672 467 688
+ 123 468 17 689
+ 484 705 148 485
+ 25 706 723 193
+ 501 502 33 724
+ 519 739 194 520
+ 41 740 757 239
+ 535 536 49 758
+ 553 774 240 554
+ 57 775 792 284
+ 569 570 65 793
+ 809 305 586 587
+ 73 810 826 329
+ 603 604 81 827
+ 621 842 330 622
+ 89 843 355 638
+ 649 331 306 605
+ 98 124 356 776
+ 261 241 463 1216
+ 119 125 307 357
+ 332 358 308 741
+ 215 195 309 285
+ 588 690 149 126
+ 196 170 503 286
+ 262 571 650 99
+ 359 242 216 537
+ 150 171 127 197
+ 217 172 243 263
+ 218 287 310 264
+ 707 173 151 333
+ 844 360 128 174
+ 311 175 219 265
+ 100 673 129 867
+ 1222 374 873 1228
+ 109 480 1234 144
+ 891 1240 380 897
+ 1246 140 497 1252
+ 165 915 1258 386
+ 921 1264 161 515
+ 1270 189 939 1276
+ 392 945 1282 185
+ 531 1288 211 963
+ 1294 398 969 1300
+ 207 549 1306 235
+ 987 1312 404 993
+ 1318 231 565 1324
+ 256 1011 1330 410
+ 1017 1336 252 582
+ 1342 280 1035 1348
+ 416 1041 1354 276
+ 599 1360 300 1059
+ 1366 422 1065 1372
+ 296 617 1378 325
+ 1083 1384 428 1389
+ 612 1090 661 1395
+ 113 1108 1401 434
+ 1114 1407 370 633
+ 1413 351 1132 1419
+ 440 1138 1425 347
+ 120 855 668 856
+ 375 1145 1146 874
+ 657 145 879 685
+ 880 381 1151 1152
+ 898 680 166 903
+ 702 904 387 1157
+ 1158 922 697 190
+ 927 720 928 393
+ 1163 1164 946 715
+ 212 951 736 952
+ 399 1169 1170 970
+ 731 236 975 754
+ 976 405 1175 1176
+ 994 749 257 999
+ 771 1000 411 1181
+ 1182 1018 766 281
+ 1023 789 1024 417
+ 1187 1188 1042 784
+ 301 1047 806 1048
+ 423 1193 1194 1066
+ 801 326 1071 823
+ 1072 429 1199 1200
+ 1091 818 114 1096
+ 458 1097 435 1205
+ 1206 1115 851 352
+ 1120 839 1121 441
+ 1211 1212 1139 834
+ 10 446 464 447
+ 101 1217 1218 868
+ 857 102 651 869
+ 652 2 1223 1224
+ 662 861 376 862
+ 875 863 115 1229
+ 1230 459 453 18
+ 469 481 470 130
+ 1235 1236 892 881
+ 131 674 893 675
+ 11 1241 1242 465
+ 885 382 886 899
+ 887 121 1247 1248
+ 669 476 26 486
+ 498 487 152 1253
+ 1254 916 905 153
+ 691 917 692 19
+ 1259 1260 482 909
+ 388 910 923 911
+ 146 1265 1266 686
+ 493 34 504 516
+ 505 176 1271 1272
+ 940 929 177 708
+ 941 709 27 1277
+ 1278 499 933 394
+ 934 947 935 167
+ 1283 1284 703 511
+ 42 521 532 522
+ 198 1289 1290 964
+ 953 199 725 965
+ 726 35 1295 1296
+ 517 957 400 958
+ 971 959 191 1301
+ 1302 721 527 50
+ 538 550 539 220
+ 1307 1308 988 977
+ 221 742 989 743
+ 43 1313 1314 533
+ 981 406 982 995
+ 983 213 1319 1320
+ 737 545 58 555
+ 566 556 244 1325
+ 1326 1012 1001 245
+ 759 1013 760 51
+ 1331 1332 551 1005
+ 412 1006 1019 1007
+ 237 1337 1338 755
+ 561 66 572 583
+ 573 266 1343 1344
+ 1036 1025 267 777
+ 1037 778 59 1349
+ 1350 567 1029 418
+ 1030 1043 1031 258
+ 1355 1356 772 578
+ 74 589 600 590
+ 288 1361 1362 1060
+ 1049 289 794 1061
+ 795 67 1367 1368
+ 584 1053 424 1054
+ 1067 1055 282 1373
+ 1374 790 595 82
+ 606 618 607 312
+ 1379 1380 1084 1073
+ 313 811 1085 812
+ 75 1385 1386 601
+ 1077 430 1078 1092
+ 1079 302 1390 1391
+ 807 613 3 639
+ 663 640 361 1396
+ 1397 1109 1098 362
+ 845 1110 846 90
+ 1402 1403 634 1102
+ 436 1103 1116 1104
+ 353 1408 1409 840
+ 646 91 623 635
+ 624 334 1414 1415
+ 1133 1122 335 828
+ 1134 829 83 1420
+ 1421 619 1126 442
+ 1127 1140 1128 327
+ 1426 1427 824 629
+ }
+
+ }
+ VertexData {
+ Array TRUE ArrayID 3 Vec3fArray 1428 {
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 -0.0499999
+ 0 0.0499999 0.0499999
+ 0 0.0499999 0.0499999
+ 0 0.0499999 0.0499999
+ 0 0.0499999 0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 -0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.025 0.0433012 0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 -0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0433012 0.025 0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 -0.0499999
+ 0.0499999 0 0.0499999
+ 0.0499999 0 0.0499999
+ 0.0499999 0 0.0499999
+ 0.0499999 0 0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 -0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.0433012 -0.025 0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 -0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0.025 -0.0433012 0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 -0.0499999
+ 0 -0.0499999 0.0499999
+ 0 -0.0499999 0.0499999
+ 0 -0.0499999 0.0499999
+ 0 -0.0499999 0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 -0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.025 -0.0433012 0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 -0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0433012 -0.025 0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 -0.0499999
+ -0.0499999 0 0.0499999
+ -0.0499999 0 0.0499999
+ -0.0499999 0 0.0499999
+ -0.0499999 0 0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 -0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.0433013 0.025 0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 -0.0499999
+ -0.025 0.0433012 0.0499999
+ -0.025 0.0433012 0.0499999
+ -0.025 0.0433012 0.0499999
+ -0.025 0.0433012 0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 -0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0.0499999
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0 0.0499999 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.025 0.0433012 0
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 -0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0341506 0.0341506 0.0499999
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0433012 0.025 0
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 -0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0466506 0.0125 0.0499999
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0499999 0 0
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 -0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0466506 -0.0125 0.0499999
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0433012 -0.025 0
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 -0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.0341506 -0.0341506 0.0499999
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.025 -0.0433012 0
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 -0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0.0125 -0.0466506 0.0499999
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ 0 -0.0499999 0
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 -0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.0125 -0.0466506 0.0499999
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.025 -0.0433012 0
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 -0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0341506 -0.0341506 0.0499999
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0433012 -0.025 0
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 -0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0466506 -0.0125 0.0499999
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0499999 0 0
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 -0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0466506 0.0124999 0.0499999
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0433013 0.025 0
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 -0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.0341507 0.0341506 0.0499999
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.025 0.0433012 0
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 -0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ -0.0125 0.0466506 0.0499999
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0125 0.0466506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0341506 0.0341506 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0466506 -0.0125 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0341506 -0.0341506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ 0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0125 -0.0466506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0341506 -0.0341506 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 -0.0125 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0466506 0.0124999 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0125 0.0466506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ -0.0341507 0.0341506 0
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.01875 0.0449759 -0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0.00624996 0.0483253 0.0499999
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0 0.0499999 0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.025 0.0433012 -0.025
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0387259 0.0295753 -0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0295753 0.0387259 0.0499999
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0433012 0.025 -0.025
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0483253 0.00624996 -0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0449759 0.01875 0.0499999
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0499999 0 -0.025
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0449759 -0.01875 -0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0483253 -0.00624996 0.0499999
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0433012 -0.025 -0.025
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0295753 -0.0387259 -0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.0387259 -0.0295753 0.0499999
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.025 -0.0433012 -0.025
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.00624996 -0.0483253 -0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0.01875 -0.0449759 0.0499999
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ 0 -0.0499999 -0.025
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.01875 -0.0449759 -0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.00624996 -0.0483253 0.0499999
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.025 -0.0433012 -0.025
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0387259 -0.0295753 -0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0295753 -0.0387259 0.0499999
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0433012 -0.025 -0.025
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0483253 -0.00624996 -0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0449759 -0.01875 0.0499999
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0499999 0 -0.025
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0449759 0.01875 -0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0483253 0.00624996 0.0499999
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0433013 0.025 -0.025
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.0295753 0.0387259 -0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.038726 0.0295753 0.0499999
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.025 0.0433012 -0.025
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.00624996 0.0483253 -0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ -0.01875 0.0449759 0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.00624996 0.0483253 -0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0.01875 0.0449759 0.0499999
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0 0.0499999 -0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.025 0.0433012 0.025
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0295753 0.0387259 -0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0387259 0.0295753 0.0499999
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0433012 0.025 0.025
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0449759 0.01875 -0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0483253 0.00624996 0.0499999
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0499999 0 0.025
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0483253 -0.00624996 -0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0449759 -0.01875 0.0499999
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0433012 -0.025 0.025
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0387259 -0.0295753 -0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.0295753 -0.0387259 0.0499999
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.025 -0.0433012 0.025
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.01875 -0.0449759 -0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0.00624996 -0.0483253 0.0499999
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ 0 -0.0499999 0.025
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.00624996 -0.0483253 -0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.01875 -0.0449759 0.0499999
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.025 -0.0433012 0.025
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0295753 -0.0387259 -0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0387259 -0.0295753 0.0499999
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0433012 -0.025 0.025
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0449759 -0.01875 -0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0483253 -0.00624996 0.0499999
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0499999 0 0.025
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0483253 0.00624996 -0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0449759 0.01875 0.0499999
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.0433013 0.025 0.025
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.038726 0.0295753 -0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.0295753 0.0387259 0.0499999
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.025 0.0433012 0.025
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.01875 0.0449759 -0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ -0.00624996 0.0483253 0.0499999
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.01875 0.0449759 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.00624996 0.0483253 0
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 -0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0125 0.0466506 0.025
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0387259 0.0295753 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0295753 0.0387259 0
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 -0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0341506 0.0341506 0.025
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0483253 0.00624996 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0449759 0.01875 0
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 -0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0466506 0.0125 0.025
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0449759 -0.01875 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0483253 -0.00624996 0
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 -0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0466506 -0.0125 0.025
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0295753 -0.0387259 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0387259 -0.0295753 0
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 -0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.0341506 -0.0341506 0.025
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.00624996 -0.0483253 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.01875 -0.0449759 0
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 -0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ 0.0125 -0.0466506 0.025
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.01875 -0.0449759 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.00624996 -0.0483253 0
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 -0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0125 -0.0466506 0.025
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0387259 -0.0295753 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0295753 -0.0387259 0
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 -0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0341506 -0.0341506 0.025
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0483253 -0.00624996 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0449759 -0.01875 0
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 -0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0466506 -0.0125 0.025
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0449759 0.01875 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0483253 0.00624996 0
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 -0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.0466506 0.0124999 0.025
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.00624996 0.0483253 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.01875 0.0449759 0
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 -0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0125 0.0466506 0.025
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.0295753 0.0387259 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.038726 0.0295753 0
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 -0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ -0.0341507 0.0341506 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.01875 0.0449759 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0387259 0.0295753 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0483253 0.00624996 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0449759 -0.01875 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.0295753 -0.0387259 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ 0.00624996 -0.0483253 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0387259 -0.0295753 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0483253 -0.00624996 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.0449759 0.01875 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.00624996 0.0483253 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ -0.0295753 0.0387259 0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.01875 0.0449759 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 -0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.00624996 0.0483253 0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0387259 0.0295753 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 -0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0295753 0.0387259 0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0483253 0.00624996 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 -0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 0.01875 0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0449759 -0.01875 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 -0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0483253 -0.00624996 0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0295753 -0.0387259 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 -0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.0387259 -0.0295753 0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.00624996 -0.0483253 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 -0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ 0.01875 -0.0449759 0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.01875 -0.0449759 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 -0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.00624996 -0.0483253 0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0387259 -0.0295753 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 -0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0295753 -0.0387259 0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0483253 -0.00624996 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 -0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 -0.01875 0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0449759 0.01875 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 -0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.0483253 0.00624996 0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.00624996 0.0483253 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 -0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.01875 0.0449759 0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.0295753 0.0387259 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 -0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ -0.038726 0.0295753 0.025
+ }
+ Indices FALSE
+ Binding BIND_PER_VERTEX
+ Normalize 0
+ }
+ NormalData {
+ Array TRUE ArrayID 4 Vec3fArray 1428 {
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ -0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ -0.707107 -0.707107 0
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ -0.965926 -0.25882 0
+ 0 0 -1
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ 0 0 1
+ -0.965926 0.258819 0
+ 0 0 -1
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ 0 0 1
+ -0.707107 0.707106 0
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ 0 0 1
+ -0.258819 0.965926 0
+ 0 0 -1
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ 0 0 1
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ 0 0 -1
+ -0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ 0 0 1
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ 0 0 -1
+ -0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ 0 0 1
+ 0 0 1
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 -1
+ 0 0 -1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 1
+ 0 0 1
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0 0 1
+ 0 0 1
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0 0 1
+ 0 0 1
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 -1
+ 0 0 -1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ 0 0 1
+ -0.258819 -0.965926 0
+ 0 0 1
+ 0 0 1
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 -1
+ 0 0 -1
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 -1
+ 0 0 -1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ 0 0 1
+ 0 0 1
+ 0 0 1
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 -1
+ 0 0 -1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0 0 1
+ 0 0 1
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ 0 0 -1
+ 0 0 -1
+ 0 0 -1
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ 0 0 1
+ 0 0 1
+ -0.25882 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.258819 0.965926 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707106 0.707107 0
+ 0.707107 0.707107 0
+ 0.707107 0.707107 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.965926 -0.258819 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.707107 -0.707107 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ 0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.258819 -0.965926 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707106 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.707107 -0.707107 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.258819 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 -0.25882 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.965926 0.258819 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.25882 0.965926 0
+ -0.25882 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.258819 0.965926 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ -0.707107 0.707106 0
+ }
+ Indices FALSE
+ Binding BIND_PER_VERTEX
+ Normalize 0
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ Matrix {
+ 0.1 0 0 0
+ 0 0.1 0 0
+ 0 0 0.1 0
+ 0 0 -0.05 1
+ }
+
+ }
+ }
+}
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/CheckerBoard.png b/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/CheckerBoard.png
deleted file mode 100644
index aafbea2..0000000
Binary files a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/CheckerBoard.png and /dev/null differ
diff --git a/SurgSim/Graphics/UnitTests/ManagerTests.cpp b/SurgSim/Graphics/UnitTests/ManagerTests.cpp
index 5bd9059..8a0215d 100644
--- a/SurgSim/Graphics/UnitTests/ManagerTests.cpp
+++ b/SurgSim/Graphics/UnitTests/ManagerTests.cpp
@@ -207,7 +207,7 @@ TEST_F(GraphicsManagerTest, UpdateTest)
sceneElement->addComponent(mockRepresentation);
runtime->getScene()->addSceneElement(sceneElement);
- EXPECT_TRUE(mockRepresentation->isVisible());
+ EXPECT_TRUE(mockRepresentation->isActive());
// When a graphics representation is inactive, it will be set to invisible by Graphics Manager.
// And it won't be updated by Graphics Manager.
@@ -216,21 +216,21 @@ TEST_F(GraphicsManagerTest, UpdateTest)
EXPECT_TRUE(graphicsManager->isInitialized());
boost::this_thread::sleep(boost::posix_time::milliseconds(100));
EXPECT_EQ(0, mockRepresentation->getNumUpdates());
- EXPECT_FALSE(mockRepresentation->isVisible());
+ EXPECT_FALSE(mockRepresentation->isActive());
// 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());
+ EXPECT_TRUE(mockRepresentation->isActive());
// 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());
+ EXPECT_FALSE(mockRepresentation->isActive());
runtime->stop();
}
diff --git a/SurgSim/Graphics/UnitTests/MockObjects.h b/SurgSim/Graphics/UnitTests/MockObjects.h
index 45d4979..bb75935 100644
--- a/SurgSim/Graphics/UnitTests/MockObjects.h
+++ b/SurgSim/Graphics/UnitTests/MockObjects.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2012-2013, SimQuest Solutions Inc.
+// Copyright 2012-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,18 +16,18 @@
#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/Program.h"
#include "SurgSim/Graphics/RenderTarget.h"
#include "SurgSim/Graphics/Representation.h"
-#include "SurgSim/Graphics/Shader.h"
+#include "SurgSim/Graphics/Texture.h"
#include "SurgSim/Graphics/UniformBase.h"
#include "SurgSim/Graphics/View.h"
#include "SurgSim/Graphics/ViewElement.h"
-#include "SurgSim/Graphics/Texture.h"
+#include "SurgSim/Math/Vector.h"
#include <array>
@@ -41,14 +41,14 @@ public:
}
/// Sets whether the group is currently visible
- /// \param visible True for visible, false for invisible
+ /// \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
+ /// \return visible True for visible, false for invisible
virtual bool isVisible() const
{
return m_isVisible;
@@ -90,7 +90,7 @@ public:
return;
}
- virtual int getType() const override
+ int getType() const override
{
return SurgSim::Framework::MANAGER_TYPE_NONE;
}
@@ -137,9 +137,7 @@ public:
/// \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),
@@ -149,20 +147,6 @@ public:
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
{
@@ -179,8 +163,11 @@ public:
/// \post m_numUpdates is incremented and dt is added to m_sumDt
virtual void update(double dt)
{
- ++m_numUpdates;
- m_sumDt += dt;
+ if (isActive())
+ {
+ ++m_numUpdates;
+ m_sumDt += dt;
+ }
}
/// Gets whether the representation has been initialized
@@ -197,7 +184,7 @@ public:
/// 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)
+ virtual bool setMaterial(std::shared_ptr<SurgSim::Framework::Component> material)
{
return false;
}
@@ -224,6 +211,17 @@ public:
return m_drawAsWireFrame;
}
+
+ void setGenerateTangents(bool value)
+ {
+
+ }
+
+ bool isGeneratingTangents() const
+ {
+ return false;
+ }
+
private:
/// Initializes the representation
/// \post m_isInitialized is set to true
@@ -240,8 +238,6 @@ private:
return true;
}
- /// Whether this representation is currently visible or not
- bool m_isVisible;
/// Number of times the representation has been updated
int m_numUpdates;
@@ -273,30 +269,13 @@ public:
SurgSim::Graphics::Representation(name),
SurgSim::Graphics::Camera(name),
m_numUpdates(0),
- m_sumDt(0.0),
- m_isVisible(true)
+ m_sumDt(0.0)
{
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
{
@@ -350,6 +329,11 @@ public:
return m_projectionMatrix;
}
+ virtual SurgSim::Math::Matrix44d getInverseProjectionMatrix() const
+ {
+ return m_projectionMatrix.inverse();
+ }
+
/// 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
@@ -362,7 +346,7 @@ public:
/// 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)
+ virtual bool setMaterial(std::shared_ptr<SurgSim::Framework::Component> material)
{
return false;
}
@@ -408,9 +392,17 @@ public:
return nullptr;
}
- virtual void setRenderOrder(RenderOrder bin, int value) override
+ void setRenderOrder(RenderOrder bin, int value) override
{
+ }
+ void setMainCamera(bool val) override
+ {
+ }
+
+ bool isMainCamera() override
+ {
+ return false;
}
virtual SurgSim::Math::Matrix44d getInverseViewMatrix() const
@@ -428,6 +420,50 @@ public:
throw std::logic_error("The method or operation is not implemented.");
}
+ void setGenerateTangents(bool value)
+ {
+ throw std::logic_error("The method or operation is not implemented.");
+ }
+
+ bool isGeneratingTangents() const
+ {
+ throw std::logic_error("The method or operation is not implemented.");
+ }
+
+ void setPerspectiveProjection(double fovy, double aspect, double near, double far)
+ {
+ throw std::logic_error("The method or operation is not implemented.");
+ }
+
+ void setOrthogonalProjection(double left, double right, double bottom, double top, double near, double far)
+ {
+ throw std::logic_error("The method or operation is not implemented.");
+ }
+
+ void setViewport(int x, int y, int width, int height)
+ {
+ throw std::logic_error("The method or operation is not implemented.");
+ }
+
+ std::array<int, 4> getViewport() const
+ {
+ throw std::logic_error("The method or operation is not implemented.");
+ }
+
+ void getViewport(int* x, int* y, int* width, int* height) const
+ {
+ throw std::logic_error("The method or operation is not implemented.");
+ }
+
+ void setViewportSize(std::array<double, 2> dimensions)
+ {
+ throw std::logic_error("The method or operation is not implemented.");
+ }
+
+ std::array<double, 2> getViewportSize() const
+ {
+ throw std::logic_error("The method or operation is not implemented.");
+ }
private:
/// Number of times the camera has been updated
@@ -443,10 +479,6 @@ private:
/// 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
@@ -475,7 +507,7 @@ public:
/// 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
+ void setPosition(const std::array<int, 2>& position) override
{
m_x = position[0];
m_y = position[1];
@@ -483,15 +515,15 @@ public:
/// 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> getPosition() const override
{
std::array<int, 2> result = {m_x, m_y};
- return std::move(result);
+ return 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
+ void setDimensions(const std::array<int, 2>& dimensions) override
{
m_width = dimensions[0];
m_height = dimensions[1];
@@ -499,21 +531,33 @@ public:
/// 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> getDimensions() const override
{
std::array<int, 2> result = {m_width, m_height};
+ return result;
+ }
+
+ void setDimensionsDouble(const std::array<double, 2>& dimensions)
+ {
+ m_width = static_cast<int>(dimensions[0]);
+ m_height = static_cast<int>(dimensions[1]);
+ }
+
+ std::array<double, 2> getDimensionsDouble() const
+ {
+ std::array<double, 2> result = {static_cast<double>(m_width), static_cast<double>(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
+ 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
+ bool isWindowBorderEnabled() const override
{
return m_isWindowBorderEnabled;
}
@@ -565,6 +609,11 @@ private:
return true;
}
+ int doSetTargetScreen(int val)
+ {
+ return 0;
+ }
+
/// Position of the view on the screen (in pixels)
int m_x, m_y;
/// Dimensions of the view on the screen (in pixels)
@@ -581,6 +630,7 @@ private:
bool m_isInitialized;
/// Whether the view has been awoken
bool m_isAwoken;
+
};
/// Representation that does not subclass any graphics components
diff --git a/SurgSim/Graphics/UnitTests/MockOsgObjects.h b/SurgSim/Graphics/UnitTests/MockOsgObjects.h
index 0921cbe..1bf4b55 100644
--- a/SurgSim/Graphics/UnitTests/MockOsgObjects.h
+++ b/SurgSim/Graphics/UnitTests/MockOsgObjects.h
@@ -37,27 +37,11 @@ public:
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
@@ -74,7 +58,7 @@ public:
/// 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)
+ virtual void doUpdate(double dt)
{
++m_numUpdates;
m_sumDt += dt;
@@ -107,9 +91,6 @@ private:
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
@@ -119,9 +100,6 @@ private:
bool m_isInitialized;
/// Whether the representation has been awoken
bool m_isAwoken;
-
- /// Rigid transform describing pose of the representation
- SurgSim::Math::RigidTransform3d m_transform;
};
@@ -134,7 +112,7 @@ public:
osg::setNotifyLevel(osg::DEBUG_FP);
}
- virtual void notify(osg::NotifySeverity severity, const char *message) override
+ void notify(osg::NotifySeverity severity, const char *message) override
{
reset();
if (severity <= osg::FATAL)
diff --git a/SurgSim/Graphics/UnitTests/OsgAxesRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgAxesRepresentationTests.cpp
new file mode 100644
index 0000000..4071911
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgAxesRepresentationTests.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 <gtest/gtest.h>
+
+#include "SurgSim/Graphics/OsgAxesRepresentation.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgAxesRepresentationTests, Serialization)
+{
+ using SurgSim::Framework::Component;
+
+ std::shared_ptr<Component> axes = std::make_shared<OsgAxesRepresentation>("axes");
+ axes->setValue("Size", 12.0);
+
+ YAML::Node node = YAML::convert<Component>::encode(*axes);
+
+ std::shared_ptr<Component> component = node.as<std::shared_ptr<Component>>();
+
+ ASSERT_NE(nullptr, component);
+
+ std::shared_ptr<OsgAxesRepresentation> newAxes = std::dynamic_pointer_cast<OsgAxesRepresentation>(component);
+
+ ASSERT_NE(nullptr, newAxes);
+
+ EXPECT_DOUBLE_EQ(12.0, newAxes->getValue<double>("Size"));
+}
+
+}
+}
+
diff --git a/SurgSim/Graphics/UnitTests/OsgBoxRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgBoxRepresentationTests.cpp
index 1cd095c..8a9856b 100644
--- a/SurgSim/Graphics/UnitTests/OsgBoxRepresentationTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgBoxRepresentationTests.cpp
@@ -72,17 +72,6 @@ TEST(OsgBoxRepresentationTests, AccessibleTest)
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");
@@ -154,84 +143,6 @@ TEST(OsgBoxRepresentationTests, SizeVector3dTest)
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
index 58a55d6..de294cf 100644
--- a/SurgSim/Graphics/UnitTests/OsgCameraTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgCameraTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2012-2013, SimQuest Solutions Inc.
+// Copyright 2012-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,31 +16,32 @@
/// \file
/// Tests for the OsgCamera class.
-#include "SurgSim/Graphics/UnitTests/MockObjects.h"
-#include "SurgSim/Graphics/UnitTests/MockOsgObjects.h"
+
+#include <gtest/gtest.h>
+#include <osg/Camera>
+#include <osg/ref_ptr>
+#include <random>
#include "SurgSim/Framework/BasicSceneElement.h"
#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.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/Graphics/OsgTexture2d.h"
+#include "SurgSim/Graphics/UnitTests/MockObjects.h"
+#include "SurgSim/Graphics/UnitTests/MockOsgObjects.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::makeRigidTransform;
using SurgSim::Math::Matrix44d;
using SurgSim::Math::Quaterniond;
using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Vector3d;
-using SurgSim::Math::makeRigidTransform;
+
namespace SurgSim
{
@@ -56,7 +57,7 @@ TEST(OsgCameraTests, InitTest)
EXPECT_EQ("test name", camera->getName());
- EXPECT_TRUE(camera->isVisible());
+ EXPECT_TRUE(camera->isActive());
EXPECT_TRUE(camera->getPose().matrix().isApprox(
fromOsg(osgCamera->getOsgCamera()->getViewMatrix()).inverse())) <<
@@ -68,7 +69,7 @@ TEST(OsgCameraTests, InitTest)
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());
+ EXPECT_EQ(0, camera->getRenderGroups().size());
}
TEST(OsgCameraTests, OsgNodesTest)
@@ -86,7 +87,7 @@ TEST(OsgCameraTests, OsgNodesTest)
EXPECT_EQ(camera.get(), switchNode->getChild(0));
}
-TEST(OsgCameraTests, VisibilityTest)
+TEST(OsgCameraTests, ActivenessTest)
{
std::shared_ptr<OsgCamera> osgCamera = std::make_shared<OsgCamera>("test name");
std::shared_ptr<OsgRepresentation> osgRepresentation = osgCamera;
@@ -97,15 +98,15 @@ TEST(OsgCameraTests, VisibilityTest)
osg::ref_ptr<osg::Switch> switchNode = dynamic_cast<osg::Switch*>(osgRepresentation->getOsgNode().get());
EXPECT_TRUE(switchNode.valid());
- EXPECT_TRUE(camera->isVisible());
+ EXPECT_TRUE(camera->isActive());
EXPECT_TRUE(switchNode->getChildValue(osgCamera->getOsgCamera()));
- camera->setVisible(false);
- EXPECT_FALSE(camera->isVisible());
+ camera->setLocalActive(false);
+ EXPECT_FALSE(camera->isActive());
EXPECT_FALSE(switchNode->getChildValue(osgCamera->getOsgCamera()));
- camera->setVisible(true);
- EXPECT_TRUE(camera->isVisible());
+ camera->setLocalActive(true);
+ EXPECT_TRUE(camera->isActive());
EXPECT_TRUE(switchNode->getChildValue(osgCamera->getOsgCamera()));
}
@@ -114,34 +115,37 @@ TEST(OsgCameraTests, GroupTest)
{
std::shared_ptr<OsgCamera> osgCamera = std::make_shared<OsgCamera>("test name");
std::shared_ptr<Camera> camera = osgCamera;
+ camera->addRenderGroupReference("test group");
- EXPECT_EQ(nullptr, camera->getRenderGroup());
+ EXPECT_EQ(0, camera->getRenderGroups().size());
/// Adding an OsgGroup should succeed
- std::shared_ptr<OsgGroup> osgGroup = std::make_shared<OsgGroup>(camera->getRenderGroupReference());
+ std::shared_ptr<OsgGroup> osgGroup = std::make_shared<OsgGroup>(camera->getRenderGroupReferences().front());
std::shared_ptr<Group> group = osgGroup;
EXPECT_TRUE(camera->setRenderGroup(group));
- EXPECT_EQ(group, camera->getRenderGroup());
+ EXPECT_EQ(group, camera->getRenderGroups().front());
/// 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());
+ std::shared_ptr<Group> mockGroup = std::make_shared<MockGroup>(camera->getRenderGroupReferences().front());
EXPECT_FALSE(camera->setRenderGroup(mockGroup));
- EXPECT_EQ(group, camera->getRenderGroup());
+ EXPECT_EQ(group, camera->getRenderGroups().front());
EXPECT_EQ(osgGroup->getOsgGroup(), osgCamera->getOsgCamera()->getChild(0)->asGroup()->getChild(0));
}
TEST(OsgCameraTests, PoseTest)
{
+ auto runtime = std::make_shared<Framework::Runtime>();
+ auto scene = runtime->getScene();
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();
+ scene->addSceneElement(element);
camera->wakeUp();
RigidTransform3d elementPose = SurgSim::Math::makeRigidTransform(
@@ -189,6 +193,7 @@ TEST(OsgCameraTests, MatricesTest)
Matrix44d projectionMatrix = Matrix44d::Random();
camera->setProjectionMatrix(projectionMatrix);
EXPECT_TRUE(camera->getProjectionMatrix().isApprox(projectionMatrix));
+ EXPECT_TRUE(camera->getInverseProjectionMatrix().isApprox(projectionMatrix.inverse()));
}
TEST(OsgCameraTests, RenderTargetTest)
@@ -231,9 +236,11 @@ TEST(OsgCameraTests, Serialization)
// Set values.
SurgSim::Math::Matrix44d projection = SurgSim::Math::Matrix44d::Random();
+ std::array<double, 2> viewport = { 1024, 768};
camera->setValue("ProjectionMatrix", projection);
- camera->setValue("Visible", true);
+ camera->setValue("ViewportSize", viewport);
camera->setValue("AmbientColor", SurgSim::Math::Vector4d(0.1, 0.2, 0.3, 0.4));
+ camera->setViewport(10, 20, 30, 50);
// Serialize.
YAML::Node node;
@@ -246,11 +253,106 @@ TEST(OsgCameraTests, Serialization)
// 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")));
+ boost::any_cast<SurgSim::Math::Matrix44d>(newCamera->getValue("ProjectionMatrix"))));
+
+ typedef std::array<double, 2> ParamType;
+ EXPECT_TRUE(boost::any_cast<ParamType>(camera->getValue("ViewportSize")) ==
+ boost::any_cast<ParamType>(newCamera->getValue("ViewportSize")));
+
EXPECT_TRUE(boost::any_cast<SurgSim::Math::Vector4d>(camera->getValue("AmbientColor")).isApprox(
- boost::any_cast<SurgSim::Math::Vector4d>(newCamera->getValue("AmbientColor"))));
+ boost::any_cast<SurgSim::Math::Vector4d>(newCamera->getValue("AmbientColor"))));
+}
+
+TEST(OsgCameraTests, SetProjection)
+{
+ std::shared_ptr<OsgCamera> camera = std::make_shared<OsgCamera>("TestOsgCamera");
+
+ Math::Matrix44d identity = Math::Matrix44d::Identity();
+
+ camera->setProjectionMatrix(identity);
+ osg::ref_ptr<osg::Camera> osgCamera(new osg::Camera);
+
+ // Windows does not support initializer lists ...
+ std::array<double, 4> persp = {{90.0, 1.0, 0.01, 10.0}};
+
+ osgCamera->setProjectionMatrixAsPerspective(persp[0], persp[1], persp[2], persp[3]);
+ auto expectedPerspective = Graphics::fromOsg(osgCamera->getProjectionMatrix());
+
+ camera->setPerspectiveProjection(persp[0], persp[1], persp[2], persp[3]);
+ EXPECT_TRUE(expectedPerspective.isApprox(camera->getProjectionMatrix()));
+
+ camera->setProjectionMatrix(identity);
+
+ EXPECT_NO_THROW(camera->setValue("PerspectiveProjection", persp));
+ EXPECT_TRUE(expectedPerspective.isApprox(camera->getProjectionMatrix()));
+
+ camera->setProjectionMatrix(identity);
+
+ std::array<double, 6> ortho = {{ -1.0, 1.0, 2.0, -2.0, 3.0, -3.0}};
+
+ osgCamera->setProjectionMatrixAsOrtho(ortho[0], ortho[1], ortho[2], ortho[3], ortho[4], ortho[5]);
+ auto expectedOrtho = Graphics::fromOsg(osgCamera->getProjectionMatrix());
+
+ camera->setOrthogonalProjection(ortho[0], ortho[1], ortho[2], ortho[3], ortho[4], ortho[5]);
+ EXPECT_TRUE(expectedOrtho.isApprox(camera->getProjectionMatrix()));
+
+ camera->setProjectionMatrix(identity);
+
+ EXPECT_NO_THROW(camera->setValue("OrthogonalProjection", ortho));
+ EXPECT_TRUE(expectedOrtho.isApprox(camera->getProjectionMatrix()));
+}
+
+TEST(OsgCameraTests, Viewport)
+{
+ std::shared_ptr<OsgCamera> camera = std::make_shared<OsgCamera>("TestOsgCamera");
+
+ std::array<int, 4> original = {10, 20, 30, 40};
+ std::array<int, 4> result = {0, 0, 0, 0};
+
+ camera->setViewport(original[0], original[1], original[2], original[3]);
+ camera->getViewport(&result[0], &result[1], &result[2], &result[3]);
+
+ EXPECT_EQ(original, result);
+ camera->setViewport(0, 0, 0, 0);
+
+ camera->setValue("Viewport", original);
+ result = camera->getValue<std::array<int, 4>>("Viewport");
+ EXPECT_EQ(original, result);
+}
+
+TEST(OsgCameraTests, MultipleRenderGroups)
+{
+ std::shared_ptr<OsgCamera> camera = std::make_shared<OsgCamera>("TestOsgCamera");
+
+ EXPECT_EQ(0, camera->getRenderGroupReferences().size());
+
+ camera->addRenderGroupReference("Group1");
+ camera->addRenderGroupReference("Group2");
+ EXPECT_EQ(2, camera->getRenderGroupReferences().size());
+
+ std::vector<std::string> references;
+ references.push_back("Group3");
+ references.push_back("Group4");
+ references.push_back("Group5");
+ camera->setRenderGroupReferences(references);
+ EXPECT_EQ(3, camera->getRenderGroupReferences().size());
+
+ camera->addRenderGroup(std::make_shared<Graphics::OsgGroup>("Group3"));
+ camera->addRenderGroup(std::make_shared<Graphics::OsgGroup>("Group4"));
+ camera->addRenderGroup(std::make_shared<Graphics::OsgGroup>("Group5"));
+
+ std::vector<std::shared_ptr<Group>> groups;
+ camera->getOsgCamera()->removeChildren(0, 1);
+ references.push_back("Group6");
+ references.push_back("Group7");
+ references.push_back("Group8");
+ for (auto reference : references)
+ {
+ camera->addRenderGroupReference(reference);
+ groups.push_back(std::make_shared<Graphics::OsgGroup>(reference));
+ }
+ camera->setRenderGroups(groups);
+ EXPECT_EQ(groups.size(), camera->getRenderGroups().size());
}
} // namespace Graphics
diff --git a/SurgSim/Graphics/UnitTests/OsgCurveRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgCurveRepresentationTests.cpp
new file mode 100644
index 0000000..ba3eaea
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgCurveRepresentationTests.cpp
@@ -0,0 +1,87 @@
+// 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/Graphics/OsgCurveRepresentation.h"
+#include "SurgSim/DataStructures/Vertices.h"
+
+#include <gtest/gtest.h>
+
+namespace
+{
+const double epsilon = 1e-5;
+}
+namespace SurgSim
+{
+
+namespace Graphics
+{
+
+TEST(OsgCurveRepresentationTests, Init)
+{
+ ASSERT_NO_THROW({std::shared_ptr<Representation> representation =
+ std::make_shared<OsgCurveRepresentation>("Test");
+ });
+}
+
+// Tests setters and getters through the properties, thereby also testing the properties
+TEST(OsgCurveRepresentationTests, SetterGettersProperties)
+{
+ auto curve = std::make_shared<OsgCurveRepresentation>("Test");
+
+ double width = 1234.0;
+ curve->setValue("Width", width);
+ EXPECT_NEAR(width, curve->getValue<double>("Width"), epsilon);
+ EXPECT_NEAR(width, curve->getWidth(), epsilon);
+
+ double tension = 0.1234;
+ curve->setValue("Tension", tension);
+ EXPECT_NEAR(tension, curve->getValue<double>("Tension"), epsilon);
+ EXPECT_NEAR(tension, curve->getTension(), epsilon);
+
+ bool val = curve->isAntiAliasing();
+ curve->setValue("AntiAliasing", !val);
+ EXPECT_EQ(!val, curve->isAntiAliasing());
+ EXPECT_EQ(!val, curve->getValue<bool>("AntiAliasing"));
+
+ size_t subdivisions = 4321;
+ curve->setValue("Subdivisions", subdivisions);
+ EXPECT_EQ(subdivisions, curve->getValue<size_t>("Subdivisions"));
+ EXPECT_EQ(subdivisions, curve->getSubdivisions());
+
+ Math::Vector4d color(4.0, 3.0, 2.0, 1.0);
+ curve->setValue("Color", color);
+ EXPECT_TRUE(color.isApprox(curve->getValue<Math::Vector4d>("Color")));
+ EXPECT_TRUE(color.isApprox(curve->getColor()));
+}
+
+TEST(OsgCurveRepresentationTests, ControlPoints)
+{
+ typedef DataStructures::VerticesPlain::VertexType Vertex;
+ DataStructures::VerticesPlain vertices;
+ vertices.addVertex(Vertex(Math::Vector3d(1.0, 2.0, 3.0)));
+
+ auto curve = std::make_shared<OsgCurveRepresentation>("Test");
+ EXPECT_ANY_THROW(curve->updateControlPoints(vertices));
+
+ vertices.addVertex(Vertex(Math::Vector3d(1.0, 2.0, 3.0)));
+ vertices.addVertex(Vertex(Math::Vector3d(1.0, 2.0, 3.0)));
+
+ EXPECT_NO_THROW(curve->updateControlPoints(vertices));
+
+ EXPECT_NO_THROW(curve->setValue("Vertices", vertices));
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgGroupTests.cpp b/SurgSim/Graphics/UnitTests/OsgGroupTests.cpp
index 10ad965..72d1a3d 100644
--- a/SurgSim/Graphics/UnitTests/OsgGroupTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgGroupTests.cpp
@@ -128,7 +128,7 @@ TEST(OsgGroupTests, AddRemoveTest)
"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)) <<
+ std::find(group->getMembers().begin(), group->getMembers().end(), nonOsgRepresentation)) <<
"Only subclasses of OsgRepresentation should be in an OsgGroup!";
EXPECT_EQ(1u, osgSwitch->getNumChildren());
}
@@ -159,11 +159,11 @@ TEST(OsgGroupTests, AppendTest)
// 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));
+ std::find(group2->getMembers().begin(), group2->getMembers().end(), representation1));
EXPECT_NE(group2->getMembers().end(),
- std::find(group2->getMembers().begin(), group2->getMembers().end(), representation2));
+ std::find(group2->getMembers().begin(), group2->getMembers().end(), representation2));
EXPECT_NE(group2->getMembers().end(),
- std::find(group2->getMembers().begin(), group2->getMembers().end(), representation3));
+ 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!";
@@ -172,9 +172,9 @@ TEST(OsgGroupTests, AppendTest)
/// 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));
+ std::find(group1->getMembers().begin(), group1->getMembers().end(), representation1));
EXPECT_NE(group1->getMembers().end(),
- std::find(group1->getMembers().begin(), group1->getMembers().end(), representation2));
+ 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");
@@ -188,9 +188,9 @@ TEST(OsgGroupTests, AppendTest)
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));
+ std::find(group1->getMembers().begin(), group1->getMembers().end(), representation3));
EXPECT_EQ(group1->getMembers().end(),
- std::find(group1->getMembers().begin(), group1->getMembers().end(), nonOsgRepresentation));
+ std::find(group1->getMembers().begin(), group1->getMembers().end(), nonOsgRepresentation));
}
TEST(OsgGroupTests, ClearTest)
diff --git a/SurgSim/Graphics/UnitTests/OsgLightTests.cpp b/SurgSim/Graphics/UnitTests/OsgLightTests.cpp
index 7682f1a..0498212 100644
--- a/SurgSim/Graphics/UnitTests/OsgLightTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgLightTests.cpp
@@ -17,9 +17,11 @@
/// Basic logic tests for OsgLight
#include <gtest/gtest.h>
+#include "SurgSim/Framework/FrameworkConvert.h"
+
#include "SurgSim/Graphics/Light.h"
-#include "SurgSim/Graphics/OsgLight.h"
#include "SurgSim/Graphics/OsgGroup.h"
+#include "SurgSim/Graphics/OsgLight.h"
#include "SurgSim/Graphics/OsgConversions.h"
#include "SurgSim/Graphics/UnitTests/MockObjects.h"
@@ -85,7 +87,6 @@ TEST_F(OsgLightTests, InitTest)
}
-
TEST_F(OsgLightTests, GroupAccessorTest)
{
std::shared_ptr<OsgLight> light = std::make_shared<OsgLight>("TestLight");
@@ -170,5 +171,40 @@ TEST_F(OsgLightTests, AttenuationAccessorTests)
EXPECT_NEAR(quadraticAttenuation, static_cast<double>(osgValue), epsilon);
}
-}; // namespace Graphics
-}; // namespace SurgSim
+TEST_F(OsgLightTests, Serialization)
+{
+ std::shared_ptr<SurgSim::Framework::Component> component;
+ ASSERT_NO_THROW(component = SurgSim::Framework::Component::getFactory().create(
+ "SurgSim::Graphics::OsgLight",
+ "light"));
+
+ SurgSim::Math::Vector4d diffuse(0.1, 0.2, 0.3, 0.4);
+ SurgSim::Math::Vector4d specular(0.4, 0.3, 0.2, 0.1);
+ double constant = 0.1;
+ double linear = 0.2;
+ double quadratic = 0.3;
+ std::string lightGroupRef = "test";
+
+ component->setValue("DiffuseColor", diffuse);
+ component->setValue("SpecularColor", specular);
+ component->setValue("ConstantAttenuation", constant);
+ component->setValue("LinearAttenuation", linear);
+ component->setValue("QuadraticAttenuation", quadratic);
+ component->setValue("LightGroupReference", lightGroupRef);
+
+ YAML::Node node(YAML::convert<SurgSim::Framework::Component>::encode(*component));
+
+ auto decode = std::dynamic_pointer_cast<OsgLight>(
+ node.as<std::shared_ptr<OsgLight>>());
+
+ EXPECT_NE(nullptr, decode);
+ EXPECT_TRUE(diffuse.isApprox(decode->getValue<SurgSim::Math::Vector4d>("DiffuseColor")));
+ EXPECT_TRUE(specular.isApprox(decode->getValue<SurgSim::Math::Vector4d>("SpecularColor")));
+ EXPECT_NEAR(constant, decode->getValue<double>("ConstantAttenuation"), 1e-9);
+ EXPECT_NEAR(linear, decode->getValue<double>("LinearAttenuation"), 1e-9);
+ EXPECT_NEAR(quadratic, decode->getValue<double>("QuadraticAttenuation"), 1e-9);
+ EXPECT_EQ(lightGroupRef, decode->getValue<std::string>("LightGroupReference"));
+}
+
+} // namespace Graphics
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgMaterialTests.cpp b/SurgSim/Graphics/UnitTests/OsgMaterialTests.cpp
index e2081ca..d91604d 100644
--- a/SurgSim/Graphics/UnitTests/OsgMaterialTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgMaterialTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2012-2013, SimQuest Solutions Inc.
+// Copyright 2012-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,13 +16,15 @@
/// \file
/// Tests for the OsgMaterial class.
+#include <boost/any.hpp>
+#include <gtest/gtest.h>
+#include <gmock/gmock.h>
+
#include "SurgSim/Framework/Runtime.h"
#include "SurgSim/Graphics/OsgMaterial.h"
-#include "SurgSim/Graphics/OsgShader.h"
+#include "SurgSim/Graphics/OsgProgram.h"
#include "SurgSim/Graphics/OsgUniform.h"
-#include <gtest/gtest.h>
-#include <gmock/gmock.h>
using SurgSim::Math::Vector2f;
@@ -41,7 +43,7 @@ public:
}
};
-class MockShader : public Shader
+class MockProgram : public Program
{
public:
MOCK_CONST_METHOD0(hasGeometryShader, bool());
@@ -52,9 +54,9 @@ public:
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_METHOD1(loadGeometryShader, bool(const std::string&));
+ MOCK_METHOD1(loadVertexShader, bool(const std::string&));
+ MOCK_METHOD1(loadFragmentShader, bool(const std::string&));
MOCK_CONST_METHOD1(getGeometryShaderSource, bool(std::string*));
MOCK_CONST_METHOD1(getVertexShaderSource, bool(std::string*));
@@ -68,22 +70,53 @@ public:
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_EQ(nullptr, material->getProgram());
EXPECT_NE(nullptr, material->getOsgStateSet());
}
+TEST(OsgMaterialTests, AddUniformTest)
+{
+ auto material = std::make_shared<OsgMaterial>("material");
+ {
+ float value = 2.0;
+ ASSERT_NO_THROW(material->addUniform("float", "test_float_uniform", value));
+ EXPECT_EQ(value, material->getValue<float>("test_float_uniform"));
+ }
+ {
+ float value = 2.0;
+ EXPECT_THROW(material->addUniform("invalid", "test_float_uniform", value), Framework::AssertionFailure);
+ }
+ {
+ double value = 2.0;
+ EXPECT_THROW(material->addUniform("float", "test_float_uniform", value), boost::bad_any_cast);
+ }
+ {
+ Math::Vector4f vector(1.0, 2.0, 3.0, 4.0);
+ ASSERT_NO_THROW(material->addUniform("vec4", "test_vector_uniform", vector));
+ EXPECT_TRUE(vector.isApprox(material->getValue<Math::Vector4f>("test_vector_uniform")));
+ }
+ {
+ Math::Vector4f vector(1.0, 2.0, 3.0, 4.0);
+ EXPECT_THROW(material->addUniform("dvec4", "test_vector_uniform", vector), boost::bad_any_cast);
+ }
+ {
+ auto texture = std::make_shared<OsgTextureCubeMap>();
+ ASSERT_NO_THROW(material->addUniform("samplerCube", "test_texture_uniform", texture));
+ EXPECT_EQ(texture, material->getValue<std::shared_ptr<OsgTextureCubeMap>>("test_texture_uniform"));
+ }
+}
+
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>();
+ auto runtime = std::make_shared<Framework::Runtime>();
material->initialize(runtime);
EXPECT_EQ(0u, material->getNumUniforms());
@@ -96,7 +129,7 @@ TEST(OsgMaterialTests, AddAndRemoveUniformsTest)
const osg::StateSet::UniformList& uniforms = osgMaterial->getOsgStateSet()->getUniformList();
// Add a uniform to the material
- EXPECT_TRUE(material->addUniform(uniform1));
+ EXPECT_NO_THROW(material->addUniform(uniform1));
EXPECT_EQ(1u, material->getNumUniforms());
EXPECT_EQ(uniform1, material->getUniform(0));
@@ -104,7 +137,7 @@ TEST(OsgMaterialTests, AddAndRemoveUniformsTest)
EXPECT_EQ(osgUniform1->getOsgUniform(), uniforms.at("float uniform").first);
/// Add another uniform to the material
- EXPECT_TRUE(material->addUniform(uniform2));
+ EXPECT_NO_THROW(material->addUniform(uniform2));
EXPECT_EQ(2u, material->getNumUniforms());
EXPECT_EQ(uniform2, material->getUniform(1));
@@ -126,7 +159,7 @@ TEST(OsgMaterialTests, AddAndRemoveUniformsTest)
/// Try adding a non-OSG Uniform
std::shared_ptr<MockUniform> nonOsgUniform = std::make_shared<MockUniform>();
- EXPECT_FALSE(material->addUniform(nonOsgUniform)) <<
+ EXPECT_THROW(material->addUniform(nonOsgUniform), Framework::AssertionFailure) <<
"Should not be able to add a uniform that is not a subclass of OsgUniformBase!";
EXPECT_EQ(1u, material->getNumUniforms());
@@ -141,32 +174,32 @@ TEST(OsgMaterialTests, SetAndClearShaderTest)
std::shared_ptr<OsgMaterial> osgMaterial = std::make_shared<OsgMaterial>("material");
std::shared_ptr<Material> material = osgMaterial;
- EXPECT_EQ(nullptr, material->getShader());
+ EXPECT_EQ(nullptr, material->getProgram());
- std::shared_ptr<OsgShader> osgShader = std::make_shared<OsgShader>();
- std::shared_ptr<Shader> shader = osgShader;
+ std::shared_ptr<OsgProgram> osgProgram = std::make_shared<OsgProgram>();
+ std::shared_ptr<Program> program = osgProgram;
const osg::StateSet::AttributeList& attributes = osgMaterial->getOsgStateSet()->getAttributeList();
- // Set the material's shader
- EXPECT_TRUE(material->setShader(shader));
- EXPECT_EQ(shader, material->getShader());
+ // Set the material's program
+ EXPECT_TRUE(material->setProgram(program));
+ EXPECT_EQ(program, material->getProgram());
EXPECT_EQ(1u, attributes.size());
- EXPECT_EQ(osgShader->getOsgProgram(), attributes.at(osg::StateAttribute::TypeMemberPair(
+ EXPECT_EQ(osgProgram->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!";
+ "Program should have been added to the material's state attributes!";
+
+ /// Try setting a non-OSG Program
+ std::shared_ptr<MockProgram> nonOsgProgram = std::make_shared<MockProgram>();
+ EXPECT_FALSE(material->setProgram(nonOsgProgram)) <<
+ "Should not be able to set a program that is not a subclass of OsgProgram!";
+ EXPECT_NE(nonOsgProgram, material->getProgram());
+
+ /// Clear the program
+ material->clearProgram();
+ EXPECT_EQ(nullptr, material->getProgram());
+ EXPECT_EQ(0u, attributes.size()) << "Program should have been removed from the material's state attributes!";
}
TEST(OsgMaterialTests, NamedAccessTest)
@@ -237,4 +270,4 @@ TEST(OsgMaterialTests, AccessibleUniformTest)
} // namespace Graphics
-} // namespace SurgSim
\ No newline at end of file
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgMeshRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgMeshRepresentationTests.cpp
index 310059f..32d6aa0 100644
--- a/SurgSim/Graphics/UnitTests/OsgMeshRepresentationTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgMeshRepresentationTests.cpp
@@ -95,21 +95,13 @@ TEST(OsgMeshRepresentationTests, InitialisationTest)
}
-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<Runtime> runtime = std::make_shared<Runtime>("config.txt");
std::shared_ptr<SurgSim::Framework::Component> osgMesh = std::make_shared<OsgMeshRepresentation>("TestMesh");
- std::string filename = "Geometry/arm_collision.ply";
+ std::string filename = "Geometry/Cube_with_texture.ply";
- osgMesh->setValue("Filename", filename);
+ osgMesh->setValue("MeshFileName", filename);
osgMesh->setValue("UpdateOptions", 2);
osgMesh->setValue("DrawAsWireFrame", true);
@@ -119,15 +111,14 @@ TEST(OsgMeshRepresentationTests, SerializationTest)
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>>()));
+ 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(filename, newOsgMesh->getMesh()->getFileName());
EXPECT_EQ(2u, newOsgMesh->getValue<int>("UpdateOptions"));
EXPECT_TRUE(newOsgMesh->getValue<bool>("DrawAsWireFrame"));
}
@@ -135,7 +126,7 @@ TEST(OsgMeshRepresentationTests, SerializationTest)
TEST(OsgMeshRepresentationTests, MeshDelegateTest)
{
SurgSim::Framework::ApplicationData data("config.txt");
- SurgSim::DataStructures::PlyReader reader(data.findFile("OsgMeshRepresentationTests/Cube.ply"));
+ SurgSim::DataStructures::PlyReader reader(data.findFile("Geometry/Cube_with_texture.ply"));
auto delegate = std::make_shared<SurgSim::Graphics::MeshPlyReaderDelegate>();
EXPECT_NO_THROW(EXPECT_TRUE(reader.parseWithDelegate(delegate)));
diff --git a/SurgSim/Graphics/UnitTests/OsgOctreeRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgOctreeRepresentationTests.cpp
index b3f44c1..2c323fb 100644
--- a/SurgSim/Graphics/UnitTests/OsgOctreeRepresentationTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgOctreeRepresentationTests.cpp
@@ -50,7 +50,7 @@ TEST(OsgOctreeRepresentationTests, GetSetUpdateTest)
EXPECT_TRUE(octreeRepresentation->wakeUp());
// Set the octree after wake up will cause a assertion failure.
- ASSERT_ANY_THROW( { octreeRepresentation->setOctreeShape(octreeShape); } );
+ ASSERT_ANY_THROW({ octreeRepresentation->setOctreeShape(octreeShape); });
ASSERT_NO_THROW(octreeRepresentation->update(0.1));
}
@@ -61,8 +61,8 @@ TEST(OsgOctreeRepresentationTests, SetNodeVisibilityTest)
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);
+ octreeNode->addData(Vector3d(0.0, 0.0, 0.0), 2, emptyData);
+ octreeNode->addData(Vector3d(0.0, 0.0, 1.0), 3, emptyData);
auto octreeShape = std::make_shared<SurgSim::Math::OctreeShape>(*octreeNode);
auto octreeRepresentation = std::make_shared<OsgOctreeRepresentation>("TestOctree");
@@ -94,8 +94,8 @@ 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::string filename = "Geometry/staple.ply";
+ std::static_pointer_cast<SurgSim::Math::OctreeShape>(octreeShape)->loadOctree(filename);
std::shared_ptr<SurgSim::Framework::Component> osgOctree = std::make_shared<OsgOctreeRepresentation>("TestOctree");
osgOctree->setValue("OctreeShape", octreeShape);
@@ -105,19 +105,18 @@ TEST(OsgOctreeRepresentationTests, SerializationTest)
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>>()));
+ 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()));
+ auto oldOctree = std::static_pointer_cast<OctreeShape>(octreeShape)->getOctree();
+ EXPECT_TRUE(newOctree->getOctree()->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
index e8fd329..7a89252 100644
--- a/SurgSim/Graphics/UnitTests/OsgPlaneRepresentationTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgPlaneRepresentationTests.cpp
@@ -59,7 +59,7 @@ TEST(OsgPlaneRepresentationTests, AccessibleTest)
std::shared_ptr<SurgSim::Framework::Component> component;
ASSERT_NO_THROW(component = SurgSim::Framework::Component::getFactory().create(
"SurgSim::Graphics::OsgPlaneRepresentation",
- "capsule"));
+ "plane"));
EXPECT_EQ("SurgSim::Graphics::OsgPlaneRepresentation", component->getClassName());
@@ -71,124 +71,6 @@ TEST(OsgPlaneRepresentationTests, AccessibleTest)
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/OsgPointCloudRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgPointCloudRepresentationTests.cpp
index 1cd0d7a..bfedf53 100644
--- a/SurgSim/Graphics/UnitTests/OsgPointCloudRepresentationTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgPointCloudRepresentationTests.cpp
@@ -103,7 +103,6 @@ TEST(OsgPointCloudRepresentationTests, SerializationTest)
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>
diff --git a/SurgSim/Graphics/UnitTests/OsgProgramTests.cpp b/SurgSim/Graphics/UnitTests/OsgProgramTests.cpp
new file mode 100644
index 0000000..9443c8a
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgProgramTests.cpp
@@ -0,0 +1,294 @@
+// 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 OsgProgram class.
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Graphics/OsgProgram.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(OsgProgramTests, InitTest)
+{
+ OsgProgram program;
+
+ EXPECT_NE(nullptr, program.getOsgProgram());
+
+ EXPECT_EQ(0u, program.getOsgProgram()->getNumShaders());
+}
+
+TEST(OsgProgramTests, StateSetTest)
+{
+ OsgProgram program;
+
+ // 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 program to the state set
+ program.addToStateSet(stateSet.get());
+
+ EXPECT_EQ(1u, attributes.size()) << "State set has no attributes, one program should have been added!";
+ EXPECT_EQ(program.getOsgProgram(),
+ attributes.at(osg::StateAttribute::TypeMemberPair(osg::StateAttribute::PROGRAM, 0)).first)
+ << "First attribute in state set should be the added program!";
+
+ /// Remove the program from the state set
+ program.removeFromStateSet(stateSet.get());
+
+ EXPECT_EQ(0u, attributes.size())
+ << "State set should no longer have any attributes, the program should have been removed!";
+}
+
+TEST(OsgProgramTests, SetShaderSourceTest)
+{
+ std::shared_ptr<OsgProgram> osgProgram = std::make_shared<OsgProgram>();
+ std::shared_ptr<Program> program = osgProgram;
+
+ EXPECT_FALSE(program->hasVertexShader());
+ EXPECT_FALSE(program->hasGeometryShader());
+ EXPECT_FALSE(program->hasFragmentShader());
+
+ {
+ SCOPED_TRACE("Set vertex shader source");
+ program->setVertexShaderSource(vertexShader);
+
+ std::string resultVertexShader;
+ EXPECT_TRUE(program->hasVertexShader());
+ EXPECT_TRUE(program->getVertexShaderSource(&resultVertexShader));
+ EXPECT_EQ(vertexShader, resultVertexShader);
+
+ EXPECT_EQ(1u, osgProgram->getOsgProgram()->getNumShaders());
+ }
+
+ {
+ SCOPED_TRACE("Set geometry shader source");
+ program->setGeometryShaderSource(geometryShader);
+
+ std::string resultGeometryShader;
+ EXPECT_TRUE(program->hasGeometryShader());
+ EXPECT_TRUE(program->getGeometryShaderSource(&resultGeometryShader));
+ EXPECT_EQ(geometryShader, resultGeometryShader);
+
+ EXPECT_EQ(2u, osgProgram->getOsgProgram()->getNumShaders());
+ }
+
+ {
+ SCOPED_TRACE("Set fragment shader source");
+ program->setFragmentShaderSource(fragmentShader);
+
+ std::string resultFragmentShader;
+ EXPECT_TRUE(program->hasFragmentShader());
+ EXPECT_TRUE(program->getFragmentShaderSource(&resultFragmentShader));
+ EXPECT_EQ(fragmentShader, resultFragmentShader);
+
+ EXPECT_EQ(3u, osgProgram->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(OsgProgramTests, LoadShaderSourceTest)
+{
+ SurgSim::Framework::ApplicationData data("config.txt");
+
+ std::string vertexShaderPath = data.findFile("OsgProgramTests/shader.vert");
+ std::string geometryShaderPath = data.findFile("OsgProgramTests/shader.geom");
+ std::string fragmentShaderPath = data.findFile("OsgProgramTests/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<OsgProgram> osgProgram = std::make_shared<OsgProgram>();
+ std::shared_ptr<Program> program = osgProgram;
+
+ {
+ SCOPED_TRACE("Load vertex shader source from file");
+ EXPECT_TRUE(program->loadVertexShader(vertexShaderPath));
+ std::string resultVertexShader;
+ EXPECT_TRUE(program->getVertexShaderSource(&resultVertexShader));
+ expectFileContents(vertexShaderPath, resultVertexShader);
+
+ EXPECT_EQ(1u, osgProgram->getOsgProgram()->getNumShaders());
+ }
+
+ {
+ SCOPED_TRACE("Load geometry shader source from file");
+ EXPECT_TRUE(program->loadGeometryShader(geometryShaderPath));
+ std::string resultGeometryShader;
+ EXPECT_TRUE(program->getGeometryShaderSource(&resultGeometryShader));
+ expectFileContents(geometryShaderPath, resultGeometryShader);
+
+ EXPECT_EQ(2u, osgProgram->getOsgProgram()->getNumShaders());
+ }
+
+ {
+ SCOPED_TRACE("Load fragment shader source from file");
+ EXPECT_TRUE(program->loadFragmentShader(fragmentShaderPath));
+ std::string resultFragmentShader;
+ EXPECT_TRUE(program->getFragmentShaderSource(&resultFragmentShader));
+ expectFileContents(fragmentShaderPath, resultFragmentShader);
+
+ EXPECT_EQ(3u, osgProgram->getOsgProgram()->getNumShaders());
+ }
+}
+
+TEST(OsgProgramTests, ClearShaderTest)
+{
+ std::shared_ptr<OsgProgram> osgProgram = std::make_shared<OsgProgram>();
+ std::shared_ptr<Program> program = osgProgram;
+
+ // Set vertex, geometry, and fragment shaders
+ EXPECT_FALSE(program->hasVertexShader());
+ program->setVertexShaderSource(vertexShader);
+ EXPECT_TRUE(program->hasVertexShader());
+
+ EXPECT_FALSE(program->hasGeometryShader());
+ program->setGeometryShaderSource(geometryShader);
+ EXPECT_TRUE(program->hasGeometryShader());
+
+ EXPECT_FALSE(program->hasFragmentShader());
+ program->setFragmentShaderSource(fragmentShader);
+ EXPECT_TRUE(program->hasFragmentShader());
+
+ EXPECT_EQ(3u, osgProgram->getOsgProgram()->getNumShaders());
+
+ {
+ SCOPED_TRACE("Clear vertex shader");
+ program->clearVertexShader();
+ EXPECT_FALSE(program->hasVertexShader());
+ EXPECT_TRUE(program->hasGeometryShader());
+ EXPECT_TRUE(program->hasFragmentShader());
+
+ EXPECT_EQ(2u, osgProgram->getOsgProgram()->getNumShaders());
+ }
+ program->setVertexShaderSource(vertexShader);
+ EXPECT_TRUE(program->hasVertexShader());
+
+ {
+ SCOPED_TRACE("Clear geometry shader");
+ program->clearGeometryShader();
+ EXPECT_TRUE(program->hasVertexShader());
+ EXPECT_FALSE(program->hasGeometryShader());
+ EXPECT_TRUE(program->hasFragmentShader());
+
+ EXPECT_EQ(2u, osgProgram->getOsgProgram()->getNumShaders());
+ }
+ program->setGeometryShaderSource(geometryShader);
+ EXPECT_TRUE(program->hasGeometryShader());
+
+ {
+ SCOPED_TRACE("Clear fragment shader");
+ program->clearFragmentShader();
+ EXPECT_TRUE(program->hasVertexShader());
+ EXPECT_TRUE(program->hasGeometryShader());
+ EXPECT_FALSE(program->hasFragmentShader());
+
+ EXPECT_EQ(2u, osgProgram->getOsgProgram()->getNumShaders());
+ }
+ program->setFragmentShaderSource(fragmentShader);
+ EXPECT_TRUE(program->hasFragmentShader());
+
+ {
+ SCOPED_TRACE("Clear the entire shader");
+ program->clear();
+ EXPECT_FALSE(program->hasVertexShader());
+ EXPECT_FALSE(program->hasGeometryShader());
+ EXPECT_FALSE(program->hasFragmentShader());
+
+ EXPECT_EQ(0u, osgProgram->getOsgProgram()->getNumShaders());
+ }
+}
+
+} // namespace Graphics
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgRepresentationTests.cpp
index 758f084..c5f6e38 100644
--- a/SurgSim/Graphics/UnitTests/OsgRepresentationTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgRepresentationTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2012-2013, SimQuest Solutions Inc.
+// Copyright 2012-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,24 +16,28 @@
/// \file
/// Tests for the OsgRepresentation class.
+#include <gtest/gtest.h>
+#include <osg/Geode>
+#include <osg/Geometry>
+#include <random>
+
#include "SurgSim/Framework/BasicSceneElement.h"
-#include "SurgSim/Graphics/UnitTests/MockOsgObjects.h"
-#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
#include "SurgSim/Graphics/OsgCamera.h"
-
+#include "SurgSim/Graphics/OsgMaterial.h"
+#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
+#include "SurgSim/Graphics/UnitTests/MockOsgObjects.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
@@ -48,7 +52,21 @@ TEST(OsgRepresentationTests, InitTest)
std::shared_ptr<Representation> representation = std::make_shared<MockOsgRepresentation>("test name");
EXPECT_EQ("test name", representation->getName());
- EXPECT_TRUE(representation->isVisible());
+ EXPECT_TRUE(representation->isActive());
+}
+
+TEST(OsgRepresentationsTests, ParameterTests)
+{
+ auto representation = std::make_shared<MockOsgRepresentation>("name");
+
+ ASSERT_NO_THROW(representation->setValue("GroupReferences", std::vector<std::string>()));
+ ASSERT_NO_THROW(representation->getValue("GroupReferences"));
+
+ ASSERT_NO_THROW(representation->setValue("DrawAsWireFrame", true));
+ ASSERT_NO_THROW(representation->getValue("DrawAsWireFrame"));
+
+ ASSERT_NO_THROW(representation->setValue("GenerateTangents", true));
+ ASSERT_NO_THROW(representation->getValue("GenerateTangents"));
}
TEST(OsgRepresentationTests, OsgNodeTest)
@@ -63,15 +81,28 @@ TEST(OsgRepresentationTests, OsgNodeTest)
EXPECT_TRUE(osgGroup.valid()) << "Representation's OSG node should be a group!";
}
-TEST(OsgRepresentationTests, VisibilityTest)
+TEST(OsgRepresentationTests, ActivityTest)
{
- 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());
+ std::shared_ptr<OsgRepresentation> osgRepresentation = std::make_shared<MockOsgRepresentation>("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->isActive());
+ representation->update(0.0);
+ EXPECT_TRUE(switchNode->getChildValue(switchNode->getChild(0)));
+
+ representation->setLocalActive(false);
+ EXPECT_FALSE(representation->isActive());
+ representation->update(0.0);
+ EXPECT_FALSE(switchNode->getChildValue(switchNode->getChild(0)));
+
+ representation->setLocalActive(true);
+ EXPECT_TRUE(representation->isActive());
+ representation->update(0.0);
+ EXPECT_TRUE(switchNode->getChildValue(switchNode->getChild(0)));
}
TEST(OsgRepresentationTests, WireFrameTest)
@@ -86,10 +117,13 @@ TEST(OsgRepresentationTests, WireFrameTest)
TEST(OsgRepresentationTests, PoseTest)
{
+ auto runtime = std::make_shared<Framework::Runtime>();
+ auto scene = runtime->getScene();
+
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();
+ scene->addSceneElement(element);
representation->wakeUp();
{
@@ -101,8 +135,7 @@ TEST(OsgRepresentationTests, PoseTest)
RigidTransform3d localPose;
{
SCOPED_TRACE("Set Local Pose");
- localPose = SurgSim::Math::makeRigidTransform(
- Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ localPose = Math::makeRigidTransform(Quaterniond(Math::Vector4d::Random()).normalized(), Vector3d::Random());
representation->setLocalPose(localPose);
EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
EXPECT_TRUE(representation->getPose().isApprox(localPose));
@@ -111,8 +144,7 @@ TEST(OsgRepresentationTests, PoseTest)
RigidTransform3d elementPose;
{
SCOPED_TRACE("Set Element Pose");
- elementPose = SurgSim::Math::makeRigidTransform(
- Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ elementPose = Math::makeRigidTransform(Quaterniond(Math::Vector4d::Random()).normalized(), Vector3d::Random());
element->setPose(elementPose);
EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
EXPECT_TRUE(representation->getPose().isApprox(elementPose * localPose));
@@ -120,8 +152,7 @@ TEST(OsgRepresentationTests, PoseTest)
{
SCOPED_TRACE("Change Local Pose");
- localPose = SurgSim::Math::makeRigidTransform(
- Quaterniond(SurgSim::Math::Vector4d::Random()).normalized(), Vector3d::Random());
+ localPose = Math::makeRigidTransform(Quaterniond(Math::Vector4d::Random()).normalized(), Vector3d::Random());
representation->setLocalPose(localPose);
EXPECT_TRUE(representation->getLocalPose().isApprox(localPose));
EXPECT_TRUE(representation->getPose().isApprox(elementPose * localPose));
@@ -130,19 +161,62 @@ TEST(OsgRepresentationTests, PoseTest)
TEST(OsgRepresentationTests, MaterialTest)
{
- std::shared_ptr<Representation> representation = std::make_shared<MockOsgRepresentation>("test name");
+ std::shared_ptr<OsgRepresentation> osgRepresentation = std::make_shared<MockOsgRepresentation>("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");
- std::shared_ptr<Material> material = std::make_shared<OsgMaterial>("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!";
+ }
+}
+
+TEST(OsgRepresentationTests, AddUniformTest)
+{
+ std::shared_ptr<OsgRepresentation> representation = std::make_shared<MockOsgRepresentation>("test name");
+ {
+ float value = 2.0;
+ ASSERT_NO_THROW(representation->addUniform("float", "test_float_uniform", value));
+ }
+ {
+ float value = 2.0;
+ EXPECT_THROW(representation->addUniform("invalid", "test_float_uniform", value), Framework::AssertionFailure);
+ }
+ {
+ double value = 2.0;
+ EXPECT_THROW(representation->addUniform("float", "test_float_uniform", value), boost::bad_any_cast);
+ }
+ {
+ Math::Vector4f vector(1.0, 2.0, 3.0, 4.0);
+ ASSERT_NO_THROW(representation->addUniform("vec4", "test_vector_uniform", vector));
+ }
+ {
+ Math::Vector4f vector(1.0, 2.0, 3.0, 4.0);
+ EXPECT_THROW(representation->addUniform("dvec4", "test_vector_uniform", vector), boost::bad_any_cast);
+ }
+ {
+ auto texture = std::make_shared<OsgTextureCubeMap>();
+ ASSERT_NO_THROW(representation->addUniform("samplerCube", "test_texture_uniform", texture));
}
}
@@ -236,6 +310,69 @@ TEST(OsgRepresentationTests, SetGroupsTests)
EXPECT_NE(std::end(groups), std::find(std::begin(groups), std::end(groups), "group3"));
}
+class CheckTangentsVisitor : public osg::NodeVisitor
+{
+public :
+ CheckTangentsVisitor() :
+ NodeVisitor(NodeVisitor::TRAVERSE_ALL_CHILDREN)
+ {
+
+ }
+
+ virtual ~CheckTangentsVisitor()
+ {
+
+ }
+
+ void apply(osg::Node& node) // NOLINT
+ {
+ traverse(node);
+ }
+
+ void apply(osg::Geode& geode) // NOLINT
+ {
+ // Test object only has 1 geometry ...
+ if (geode.getNumDrawables() > 0)
+ {
+ osg::Geometry* curGeom = geode.getDrawable(0)->asGeometry();
+ if (curGeom)
+ {
+ osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>(curGeom->getVertexArray());
+
+ auto tangents =
+ dynamic_cast<osg::Vec4Array*>(curGeom->getVertexAttribArray(TANGENT_VERTEX_ATTRIBUTE_ID));
+ ASSERT_NE(nullptr, tangents)
+ << "Looks like no tangents where produced, or they are not of type osg::Vec4Array";
+ ASSERT_EQ(vertices->size(), tangents->size())
+ << "Looks like number of tangents does not match number of vertices";
+
+ auto bitangents =
+ dynamic_cast<osg::Vec4Array*>(curGeom->getVertexAttribArray(BITANGENT_VERTEX_ATTRIBUTE_ID));
+ ASSERT_NE(nullptr, bitangents)
+ << "Looks like no bitangents tangents where produced, or they are not of type osg::Vec4Array";
+ ASSERT_EQ(vertices->size(), bitangents->size())
+ << "Looks like number of bitangents does not match number of vertices";
+
+ }
+ }
+ }
+};
+
+TEST(OsgRepresentationTests, TangentGenerationTest)
+{
+ auto runtime = std::make_shared<Framework::Runtime>("config.txt");
+ auto scenery = std::make_shared<OsgSceneryRepresentation>("scenery");
+ scenery->loadModel("Geometry/sphere0_5.obj");
+
+ EXPECT_FALSE(scenery->isGeneratingTangents());
+ scenery->setGenerateTangents(true);
+ EXPECT_TRUE(scenery->isGeneratingTangents());
+
+ CheckTangentsVisitor visitor;
+
+ scenery->getOsgNode()->accept(visitor);
+}
+
} // namespace Graphics
} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgSceneryRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgSceneryRepresentationTests.cpp
index 127428b..c050d92 100644
--- a/SurgSim/Graphics/UnitTests/OsgSceneryRepresentationTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgSceneryRepresentationTests.cpp
@@ -26,6 +26,11 @@
#include "SurgSim/Graphics/OsgManager.h"
#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/OsgModel.h"
+
+#include <osg/Group>
+#include <osg/Geode>
+#include <osg/Geometry>
using SurgSim::Graphics::OsgSceneryRepresentation;
using SurgSim::Graphics::OsgViewElement;
@@ -34,7 +39,7 @@ using SurgSim::Graphics::SceneryRepresentation;
class OsgSceneryRepresentationTest: public ::testing::Test
{
public:
- virtual void SetUp() override
+ void SetUp() override
{
sceneryObject = std::make_shared<OsgSceneryRepresentation>("test");
sceneryObject2 = std::make_shared<OsgSceneryRepresentation>("test2");
@@ -48,7 +53,7 @@ public:
}
- virtual void TearDown() override
+ void TearDown() override
{
}
@@ -62,16 +67,19 @@ public:
TEST_F(OsgSceneryRepresentationTest, FileNameTest)
{
- sceneryObject->setFileName("OsgSceneryRepresentationTests/Torus.obj");
- EXPECT_EQ("OsgSceneryRepresentationTests/Torus.obj", sceneryObject->getFileName());
+ sceneryObject->loadModel("Geometry/Torus.obj");
+ EXPECT_EQ("Geometry/Torus.obj", sceneryObject->getModel()->getFileName());
+
}
TEST_F(OsgSceneryRepresentationTest, InitTest)
{
- sceneryObject->setFileName("OsgSceneryRepresentationTests/Torus.obj");
+ EXPECT_EQ(nullptr, sceneryObject->getModelNode());
+ sceneryObject->loadModel("Geometry/Torus.obj");
EXPECT_NO_THROW(viewElement->addComponent(sceneryObject));
+ EXPECT_NE(nullptr, sceneryObject->getModelNode());
- sceneryObject2->setFileName("OsgSceneryRepresentationTests/Torus.osgb");
+ sceneryObject2->loadModel("Geometry/Torus.osgb");
EXPECT_NO_THROW(viewElement->addComponent(sceneryObject2));
}
@@ -82,25 +90,70 @@ TEST_F(OsgSceneryRepresentationTest, AccessibleTest)
"SurgSim::Graphics::OsgSceneryRepresentation",
"scenery"));
- std::string fileName("TestFileName");
- component->setValue("FileName", fileName);
- EXPECT_EQ(fileName, component->getValue<std::string>("FileName"));
+ std::string fileName("Geometry/Torus.obj");
+ component->setValue("ModelFileName", fileName);
+ auto asset = component->getValue<std::shared_ptr<SurgSim::Graphics::Model>>("Model");
+ EXPECT_EQ(fileName, asset->getFileName());
}
TEST_F(OsgSceneryRepresentationTest, SerializationTests)
{
std::shared_ptr<SceneryRepresentation> scenery = std::make_shared<OsgSceneryRepresentation>("OsgScenery");
- std::string fileName("TestFileName");
- scenery->setFileName(fileName);
+ std::string fileName("Geometry/Torus.obj");
+ scenery->loadModel(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());
+ EXPECT_EQ(fileName, result->getModel()->getFileName());
+}
+
+// This checks a fix for an intermittent bug where on deserialization setGenerateTangents would sometimes be called
+// before setModel, in which case no tangents where generated on scenery representations
+TEST_F(OsgSceneryRepresentationTest, GenerateTangents)
+{
+ {
+ SCOPED_TRACE("setGenerateTangents before loadModel");
+ auto object = std::make_shared<OsgSceneryRepresentation>("representation");
+
+ object->setGenerateTangents(true);
+ object->loadModel("Geometry/cube.osgt");
+
+ // Structure from the osgt file, we need to get to the geometry to make sure the tangents where generated
+ auto group = object->getModelNode()->asGroup();
+ ASSERT_NE(nullptr, group);
+ auto geode = group->getChild(0)->asGeode();
+ ASSERT_NE(nullptr, geode);
+ auto geometry = geode->getDrawable(0)->asGeometry();
+ ASSERT_NE(nullptr, geometry);
+
+ ASSERT_NE(nullptr, geometry->getVertexAttribArray(SurgSim::Graphics::TANGENT_VERTEX_ATTRIBUTE_ID));
+ ASSERT_NE(nullptr, geometry->getVertexAttribArray(SurgSim::Graphics::BITANGENT_VERTEX_ATTRIBUTE_ID));
+ }
+
+
+ {
+ SCOPED_TRACE("setGenerateTangents after loadModel");
+ auto object = std::make_shared<OsgSceneryRepresentation>("representation");
+
+ object->loadModel("Geometry/cube.osgt");
+ object->setGenerateTangents(true);
+
+ // Structure from the osgt file, we need to get to the geometry to make sure the tangents where generated
+ auto group = object->getModelNode()->asGroup();
+ ASSERT_NE(nullptr, group);
+ auto geode = group->getChild(0)->asGeode();
+ ASSERT_NE(nullptr, geode);
+ auto geometry = geode->getDrawable(0)->asGeometry();
+ ASSERT_NE(nullptr, geometry);
+
+ ASSERT_NE(nullptr, geometry->getVertexAttribArray(SurgSim::Graphics::TANGENT_VERTEX_ATTRIBUTE_ID));
+ ASSERT_NE(nullptr, geometry->getVertexAttribArray(SurgSim::Graphics::BITANGENT_VERTEX_ATTRIBUTE_ID));
+ }
+
}
diff --git a/SurgSim/Graphics/UnitTests/OsgScreenSpaceQuadTests.cpp b/SurgSim/Graphics/UnitTests/OsgScreenSpaceQuadTests.cpp
index 573077b..d877c81 100644
--- a/SurgSim/Graphics/UnitTests/OsgScreenSpaceQuadTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgScreenSpaceQuadTests.cpp
@@ -130,6 +130,14 @@ TEST(OsgScreenSpaceQuadRepresentationTests, SetLocation)
}
+TEST(OsgScreenSpaceQuadRepresentationTests, SetTransparent)
+{
+ auto quad = std::make_shared<OsgScreenSpaceQuadRepresentation>("quad");
+ EXPECT_FALSE(quad->isTransparent());
+ quad->setTransparent(true);
+ EXPECT_TRUE(quad->isTransparent());
+}
+
} // namespace Graphics
} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgShaderTests.cpp b/SurgSim/Graphics/UnitTests/OsgShaderTests.cpp
deleted file mode 100644
index 0dabd8b..0000000
--- a/SurgSim/Graphics/UnitTests/OsgShaderTests.cpp
+++ /dev/null
@@ -1,297 +0,0 @@
-// 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/OsgSkeletonRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgSkeletonRepresentationTests.cpp
new file mode 100644
index 0000000..7ac2177
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgSkeletonRepresentationTests.cpp
@@ -0,0 +1,235 @@
+// 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 OsgSkeletonRepresentation class.
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Graphics/OsgManager.h"
+#include "SurgSim/Graphics/OsgSkeletonRepresentation.h"
+#include "SurgSim/Graphics/OsgViewElement.h"
+#include "SurgSim/Graphics/OsgModel.h"
+
+using SurgSim::Graphics::OsgSkeletonRepresentation;
+using SurgSim::Graphics::OsgViewElement;
+using SurgSim::Graphics::SkeletonRepresentation;
+using SurgSim::Math::makeRigidTransform;
+using SurgSim::Math::makeRotationQuaternion;
+using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::Vector3d;
+
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+class OsgSkeletonRepresentationTest: public ::testing::Test
+{
+public:
+ void SetUp() override
+ {
+ runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ }
+
+ void TearDown() override
+ {
+ }
+
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime;
+};
+
+TEST_F(OsgSkeletonRepresentationTest, CanConstruct)
+{
+ EXPECT_NO_THROW({OsgSkeletonRepresentation skeleton("test");});
+ EXPECT_NO_THROW({auto skeleton = std::make_shared<OsgSkeletonRepresentation>("test");});
+}
+
+TEST_F(OsgSkeletonRepresentationTest, FileNameTest)
+{
+ auto skeleton = std::make_shared<OsgSkeletonRepresentation>("test");
+ skeleton->loadModel("OsgSkeletonRepresentationTests/rigged_cylinder.osgt");
+ EXPECT_EQ("OsgSkeletonRepresentationTests/rigged_cylinder.osgt", skeleton->getModel()->getFileName());
+}
+
+TEST_F(OsgSkeletonRepresentationTest, SkinningShaderFileNameTest)
+{
+ auto skeleton = std::make_shared<OsgSkeletonRepresentation>("test");
+ skeleton->setSkinningShaderFileName("Shaders/skinning.vert");
+ EXPECT_EQ("Shaders/skinning.vert", skeleton->getSkinningShaderFileName());
+}
+
+TEST_F(OsgSkeletonRepresentationTest, InitTest)
+{
+ {
+ auto skeleton = std::make_shared<OsgSkeletonRepresentation>("test");
+ skeleton->loadModel("OsgSkeletonRepresentationTests/rigged_cylinder.osgt");
+ skeleton->setSkinningShaderFileName("Shaders/skinning.vert");
+ EXPECT_NO_THROW(skeleton->initialize(runtime));
+ EXPECT_TRUE(skeleton->isInitialized()) << "Should initialize with both model and shader.";
+ }
+ {
+ auto skeleton = std::make_shared<OsgSkeletonRepresentation>("test");
+ skeleton->setSkinningShaderFileName("Shaders/skinning.vert");
+ EXPECT_NO_THROW(skeleton->initialize(runtime));
+ EXPECT_FALSE(skeleton->isInitialized()) << "Should not be initialized, no model set";
+ }
+ {
+ auto skeleton = std::make_shared<OsgSkeletonRepresentation>("test");
+ skeleton->loadModel("OsgSkeletonRepresentationTests/rigged_cylinder.osgt");
+ EXPECT_NO_THROW(skeleton->initialize(runtime));
+ EXPECT_FALSE(skeleton->isInitialized()) << "Should not be initialized, no shader set";
+ }
+}
+
+TEST_F(OsgSkeletonRepresentationTest, PosesTest)
+{
+ RigidTransform3d pose = makeRigidTransform(makeRotationQuaternion(2.143, Vector3d::UnitZ().eval()),
+ Vector3d(2.3, 4.5, 6.7));
+ {
+ auto skeleton = std::make_shared<OsgSkeletonRepresentation>("test");
+ skeleton->loadModel("OsgSkeletonRepresentationTests/rigged_cylinder.osgt");
+ skeleton->setSkinningShaderFileName("Shaders/skinning.vert");
+
+ EXPECT_NO_THROW(skeleton->initialize(runtime));
+ RigidTransform3d actualPose;
+ EXPECT_NO_THROW({actualPose = skeleton->getBonePose("Bone");});
+ EXPECT_TRUE(RigidTransform3d::Identity().isApprox(actualPose));
+ EXPECT_THROW(skeleton->getBonePose("BadBoneName"), SurgSim::Framework::AssertionFailure);
+ }
+ {
+ auto skeleton = std::make_shared<OsgSkeletonRepresentation>("test");
+ skeleton->loadModel("OsgSkeletonRepresentationTests/rigged_cylinder.osgt");
+ skeleton->setSkinningShaderFileName("Shaders/skinning.vert");
+
+ EXPECT_NO_THROW(skeleton->setBonePose("Bone", pose));
+
+ RigidTransform3d actualPose;
+ EXPECT_NO_THROW({actualPose = skeleton->getBonePose("Bone");});
+ EXPECT_TRUE(pose.isApprox(actualPose));
+
+ EXPECT_NO_THROW(skeleton->initialize(runtime));
+ EXPECT_NO_THROW({actualPose = skeleton->getBonePose("Bone");});
+ EXPECT_TRUE(pose.isApprox(actualPose));
+ }
+ {
+ auto skeleton = std::make_shared<OsgSkeletonRepresentation>("test");
+ skeleton->loadModel("OsgSkeletonRepresentationTests/rigged_cylinder.osgt");
+ skeleton->setSkinningShaderFileName("Shaders/skinning.vert");
+
+ EXPECT_NO_THROW(skeleton->setBonePose("BadBoneName", pose));
+
+ RigidTransform3d actualPose;
+ EXPECT_NO_THROW({actualPose = skeleton->getBonePose("BadBoneName");});
+ EXPECT_TRUE(pose.isApprox(actualPose));
+
+ EXPECT_THROW(skeleton->initialize(runtime), SurgSim::Framework::AssertionFailure);
+ }
+}
+
+TEST_F(OsgSkeletonRepresentationTest, NeutralPosesTest)
+{
+ RigidTransform3d pose = makeRigidTransform(makeRotationQuaternion(2.143, Vector3d::UnitZ().eval()),
+ Vector3d(2.3, 4.5, 6.7));
+ {
+ auto skeleton = std::make_shared<OsgSkeletonRepresentation>("test");
+ skeleton->loadModel("OsgSkeletonRepresentationTests/rigged_cylinder.osgt");
+ skeleton->setSkinningShaderFileName("Shaders/skinning.vert");
+
+ EXPECT_NO_THROW(skeleton->initialize(runtime));
+ RigidTransform3d actualPose;
+ EXPECT_NO_THROW({actualPose = skeleton->getNeutralBonePose("Bone");});
+ EXPECT_TRUE(RigidTransform3d::Identity().isApprox(actualPose));
+ EXPECT_THROW(skeleton->getNeutralBonePose("BadBoneName"), SurgSim::Framework::AssertionFailure);
+ }
+ {
+ auto skeleton = std::make_shared<OsgSkeletonRepresentation>("test");
+ skeleton->loadModel("OsgSkeletonRepresentationTests/rigged_cylinder.osgt");
+ skeleton->setSkinningShaderFileName("Shaders/skinning.vert");
+
+ EXPECT_NO_THROW(skeleton->setNeutralBonePose("Bone", pose));
+
+ RigidTransform3d actualPose;
+ EXPECT_NO_THROW({actualPose = skeleton->getNeutralBonePose("Bone");});
+ EXPECT_TRUE(pose.isApprox(actualPose));
+
+ EXPECT_NO_THROW(skeleton->initialize(runtime));
+ EXPECT_NO_THROW({actualPose = skeleton->getNeutralBonePose("Bone");});
+ EXPECT_TRUE(pose.isApprox(actualPose));
+ }
+ {
+ auto skeleton = std::make_shared<OsgSkeletonRepresentation>("test");
+ skeleton->loadModel("OsgSkeletonRepresentationTests/rigged_cylinder.osgt");
+ skeleton->setSkinningShaderFileName("Shaders/skinning.vert");
+
+ EXPECT_NO_THROW(skeleton->setNeutralBonePose("BadBoneName", pose));
+
+ RigidTransform3d actualPose;
+ EXPECT_NO_THROW({actualPose = skeleton->getNeutralBonePose("BadBoneName");});
+ EXPECT_TRUE(pose.isApprox(actualPose));
+
+ EXPECT_THROW(skeleton->initialize(runtime), SurgSim::Framework::AssertionFailure);
+ }
+}
+
+TEST_F(OsgSkeletonRepresentationTest, AccessibleTest)
+{
+ std::shared_ptr<SurgSim::Framework::Component> component;
+ ASSERT_NO_THROW(component = SurgSim::Framework::Component::getFactory().create(
+ "SurgSim::Graphics::OsgSkeletonRepresentation", "skeleton"));
+
+ std::string fileName("OsgSkeletonRepresentationTests/rigged_cylinder.osgt");
+ component->setValue("ModelFileName", fileName);
+ std::string skinningShaderFileName("Shaders/skinning.vert");
+ component->setValue("SkinningShaderFileName", skinningShaderFileName);
+
+ auto asset = component->getValue<std::shared_ptr<Model>>("Model");
+ EXPECT_EQ(fileName, asset->getFileName());
+ auto shaderName = component->getValue<std::string>("SkinningShaderFileName");
+ EXPECT_EQ(skinningShaderFileName, shaderName);
+}
+
+TEST_F(OsgSkeletonRepresentationTest, SerializationTests)
+{
+ auto skeleton = std::make_shared<OsgSkeletonRepresentation>("test");
+
+ std::string fileName("OsgSkeletonRepresentationTests/rigged_cylinder.osgt");
+ skeleton->loadModel(fileName);
+ std::string skinningShaderFileName("Shaders/skinning.vert");
+ skeleton->setSkinningShaderFileName(skinningShaderFileName);
+ RigidTransform3d pose = makeRigidTransform(makeRotationQuaternion(2.143,
+ Vector3d(2.463, 6.346, 7.135).normalized()),
+ Vector3d(2.3, 4.5, 6.7));
+ skeleton->setNeutralBonePose("Bone", pose);
+
+ YAML::Node node;
+ node = skeleton->encode();
+ ASSERT_NO_THROW(node = skeleton->encode());
+ EXPECT_TRUE(node.IsMap());
+
+ std::shared_ptr<OsgSkeletonRepresentation> result = std::make_shared<OsgSkeletonRepresentation>("Skeleton");
+ ASSERT_NO_THROW(result->decode(node));
+ EXPECT_EQ("SurgSim::Graphics::OsgSkeletonRepresentation", result->getClassName());
+ EXPECT_EQ(fileName, result->getModel()->getFileName());
+ EXPECT_EQ(skinningShaderFileName, result->getSkinningShaderFileName());
+ EXPECT_TRUE(pose.isApprox(result->getNeutralBonePose("Bone")));
+}
+
+};
+};
diff --git a/SurgSim/Graphics/UnitTests/OsgSphereRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgSphereRepresentationTests.cpp
index 871a3fb..8deaf3d 100644
--- a/SurgSim/Graphics/UnitTests/OsgSphereRepresentationTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgSphereRepresentationTests.cpp
@@ -74,48 +74,6 @@ TEST(OsgSphereRepresentationTests, AccessibleTest)
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");
@@ -129,82 +87,6 @@ TEST(OsgSphereRepresentationTests, RadiusTest)
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/OsgTextRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgTextRepresentationTests.cpp
new file mode 100644
index 0000000..fbb4aa7
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/OsgTextRepresentationTests.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 OsgInfo class.
+
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Graphics/Graphics.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Graphics/OsgFont.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/DataStructures/DataStructuresConvert.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::RigidTransform3d;
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+TEST(OsgTextRepresentationTests, SetLocation)
+{
+ auto text = std::make_shared<OsgTextRepresentation>("text");
+
+ double x = 100.0;
+ double y = 100.0;
+
+ ASSERT_ANY_THROW(text->getLocation(nullptr, &y));
+ ASSERT_ANY_THROW(text->getLocation(&x, nullptr));
+ ASSERT_ANY_THROW(text->getLocation(nullptr, nullptr));
+
+ text->getLocation(&x, &y);
+ EXPECT_DOUBLE_EQ(0.0, x);
+ EXPECT_DOUBLE_EQ(0.0, y);
+
+ text->setLocation(100.0, 200.0);
+ text->getLocation(&x, &y);
+
+ EXPECT_DOUBLE_EQ(100.0, x);
+ EXPECT_DOUBLE_EQ(200.0, y);
+
+ Vector3d position(300.0, 400.0, 0.0);
+ text->setLocalPose(SurgSim::Math::makeRigidTransform(Quaterniond::Identity(), position));
+ text->getLocation(&x, &y);
+ EXPECT_DOUBLE_EQ(300.0, x);
+ EXPECT_DOUBLE_EQ(400.0, y);
+}
+
+TEST(OsgTextRepresentationTests, TextTest)
+{
+ auto text = std::make_shared<OsgTextRepresentation>("text");
+ std::string testText("HelloWorld");
+ EXPECT_NO_THROW(text->setText(testText));
+}
+
+TEST(OsgTextRepresentationTests, FontTests)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ auto text = std::make_shared<OsgTextRepresentation>("text");
+
+ std::shared_ptr<Font> font = std::make_shared<OsgFont>();
+ font->load("Fonts/Vera.ttf");
+
+ // Should have default font
+ EXPECT_NE(nullptr, text->getFont());
+
+ // Font setting should work
+ EXPECT_NO_THROW(text->setFont(font));
+ EXPECT_EQ(font, text->getFont());
+
+ EXPECT_ANY_THROW(text->setFont(nullptr));
+
+ // Other ways to set the font
+ EXPECT_NO_THROW(text->loadFont("Fonts/Vera.ttf"));
+ EXPECT_NO_THROW(text->setValue("FontFileName", std::string("Fonts/Vera.ttf")));
+}
+
+TEST(OsgTextRepresentationTests, MaximumWidth)
+{
+ auto text = std::make_shared<OsgTextRepresentation>("text");
+ SurgSim::DataStructures::OptionalValue<double> optional;
+ EXPECT_DOUBLE_EQ(0.0, text->getMaximumWidth());
+ EXPECT_EQ(optional, text->getOptionalMaximumWidth());
+
+ text->setMaximumWidth(10.0);
+ EXPECT_DOUBLE_EQ(10.0, text->getMaximumWidth());
+ optional.setValue(10.0);
+ EXPECT_DOUBLE_EQ(*optional, *(text->getOptionalMaximumWidth()));
+ optional.invalidate();
+ text->setOptionalMaximumWidth(optional);
+ EXPECT_DOUBLE_EQ(0.0, text->getMaximumWidth());
+ EXPECT_FALSE(text->getOptionalMaximumWidth().hasValue());
+ optional.setValue(20.0);
+ text->setOptionalMaximumWidth(optional);
+ EXPECT_DOUBLE_EQ(20.0, text->getMaximumWidth());
+ EXPECT_DOUBLE_EQ(*optional, *(text->getOptionalMaximumWidth()));
+
+}
+
+TEST(OsgTextRepresentationTests, Serialization)
+{
+ using SurgSim::Framework::Component;
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ std::shared_ptr<TextRepresentation> text = std::make_shared<OsgTextRepresentation>("text");
+
+ text->setText("TestTest");
+ text->setFontSize(123.0);
+ text->setColor(SurgSim::Math::Vector4d(1.0, 2.0, 3.0, 4.0));
+ text->setMaximumWidth(321.0);
+
+ typedef YAML::convert<Component> Converter;
+
+ YAML::Node node;
+
+ ASSERT_NO_THROW(node = Converter::encode(*text));
+
+ std::shared_ptr<Component> result;
+
+ ASSERT_NO_THROW(result = node.as<std::shared_ptr<Component>>());
+ ASSERT_NE(nullptr, result);
+
+ auto textResult = std::dynamic_pointer_cast<OsgTextRepresentation>(result);
+ ASSERT_NE(nullptr, textResult);
+
+ EXPECT_EQ("TestTest", textResult->getText());
+ EXPECT_DOUBLE_EQ(123.0, textResult->getFontSize());
+ EXPECT_DOUBLE_EQ(321.0, textResult->getMaximumWidth());
+ EXPECT_TRUE(SurgSim::Math::Vector4d(1.0, 2.0, 3.0, 4.0).isApprox(textResult->getColor()));
+}
+
+}; // Graphics
+}; // SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgTexture1dTests.cpp b/SurgSim/Graphics/UnitTests/OsgTexture1dTests.cpp
index 3ac4e9e..51bb56f 100644
--- a/SurgSim/Graphics/UnitTests/OsgTexture1dTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgTexture1dTests.cpp
@@ -54,13 +54,9 @@ TEST(OsgTexture1dTests, SetSizeTest)
TEST(OsgTexture1dTests, LoadAndClearImageTest)
{
- ASSERT_TRUE(boost::filesystem::exists("Data"));
+ SurgSim::Framework::ApplicationData data("config.txt");
- std::vector<std::string> paths;
- paths.push_back("Data/OsgTextureTests");
- SurgSim::Framework::ApplicationData data(paths);
-
- std::string imagePath = data.findFile("Gradient.png");
+ std::string imagePath = data.findFile("Textures/Gradient.png");
ASSERT_NE("", imagePath) << "Could not find image file!";
@@ -95,4 +91,4 @@ TEST(OsgTexture1dTests, LoadAndClearImageTest)
}
} // namespace Graphics
-} // namespace SurgSim
\ No newline at end of file
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgTexture2dTests.cpp b/SurgSim/Graphics/UnitTests/OsgTexture2dTests.cpp
index 4d1a44f..b6dd68c 100644
--- a/SurgSim/Graphics/UnitTests/OsgTexture2dTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgTexture2dTests.cpp
@@ -52,13 +52,9 @@ TEST(OsgTexture2dTests, SetSizeTest)
TEST(OsgTexture2dTests, LoadAndClearImageTest)
{
- ASSERT_TRUE(boost::filesystem::exists("Data"));
+ SurgSim::Framework::ApplicationData data("config.txt");
- std::vector<std::string> paths;
- paths.push_back("Data/OsgTextureTests");
- SurgSim::Framework::ApplicationData data(paths);
-
- std::string imagePath = data.findFile("CheckerBoard.png");
+ std::string imagePath = data.findFile("Textures/CheckerBoard.png");
ASSERT_NE("", imagePath) << "Could not find image file!";
@@ -94,4 +90,4 @@ TEST(OsgTexture2dTests, LoadAndClearImageTest)
}
} // namespace Graphics
-} // namespace SurgSim
\ No newline at end of file
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgTexture3dTests.cpp b/SurgSim/Graphics/UnitTests/OsgTexture3dTests.cpp
index e016d2e..1e43563 100644
--- a/SurgSim/Graphics/UnitTests/OsgTexture3dTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgTexture3dTests.cpp
@@ -53,13 +53,9 @@ TEST(OsgTexture3dTests, SetSizeTest)
TEST(OsgTexture3dTests, LoadAndClearImageTest)
{
- ASSERT_TRUE(boost::filesystem::exists("Data"));
+ SurgSim::Framework::ApplicationData data("config.txt");
- std::vector<std::string> paths;
- paths.push_back("Data/OsgTextureTests");
- SurgSim::Framework::ApplicationData data(paths);
-
- std::string imagePath = data.findFile("CheckerBoard.png");
+ std::string imagePath = data.findFile("Textures/CheckerBoard.png");
ASSERT_NE("", imagePath) << "Could not find image file!";
@@ -97,16 +93,12 @@ TEST(OsgTexture3dTests, LoadAndClearImageTest)
TEST(OsgTexture3dTests, LoadImageSlicesTest)
{
- ASSERT_TRUE(boost::filesystem::exists("Data"));
-
- std::vector<std::string> paths;
- paths.push_back("Data/OsgTextureTests");
- SurgSim::Framework::ApplicationData data(paths);
+ SurgSim::Framework::ApplicationData data("config.txt");
- std::string slice0Path = data.findFile("Brdf0.png");
+ std::string slice0Path = data.findFile("Textures/Brdf0.png");
ASSERT_NE("", slice0Path) << "Could not find image file for slice 0!";
- std::string slice1Path = data.findFile("Brdf1.png");
+ std::string slice1Path = data.findFile("Textures/Brdf1.png");
ASSERT_NE("", slice1Path) << "Could not find image file for slice 1!";
std::vector<std::string> slicePaths;
@@ -148,4 +140,4 @@ TEST(OsgTexture3dTests, LoadImageSlicesTest)
}
} // namespace Graphics
-} // namespace SurgSim
\ No newline at end of file
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgTextureCubeMapTests.cpp b/SurgSim/Graphics/UnitTests/OsgTextureCubeMapTests.cpp
index 754e2a9..7372411 100644
--- a/SurgSim/Graphics/UnitTests/OsgTextureCubeMapTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgTextureCubeMapTests.cpp
@@ -55,13 +55,9 @@ TEST(OsgTextureCubeMapTests, SetSizeTest)
TEST(OsgTextureCubeMapTests, LoadAndClearImageTest)
{
- ASSERT_TRUE(boost::filesystem::exists("Data"));
+ SurgSim::Framework::ApplicationData data("config.txt");
- std::vector<std::string> paths;
- paths.push_back("Data/OsgTextureTests");
- SurgSim::Framework::ApplicationData data(paths);
-
- std::string imagePath = data.findFile("CubeMap.png");
+ std::string imagePath = data.findFile("Textures/CubeMap_axes.png");
ASSERT_NE("", imagePath) << "Could not find image file!";
@@ -100,28 +96,24 @@ TEST(OsgTextureCubeMapTests, LoadAndClearImageTest)
TEST(OsgTextureCubeMapTests, LoadImageFacesTest)
{
- ASSERT_TRUE(boost::filesystem::exists("Data"));
-
- std::vector<std::string> paths;
- paths.push_back("Data/OsgTextureTests");
- SurgSim::Framework::ApplicationData data(paths);
+ SurgSim::Framework::ApplicationData data("config.txt");
- std::string negativeXPath = data.findFile("NegativeX.png");
+ std::string negativeXPath = data.findFile("Textures/NegativeX.png");
ASSERT_NE("", negativeXPath) << "Could not find image file for (-X) face!";
- std::string positiveXPath = data.findFile("PositiveX.png");
+ std::string positiveXPath = data.findFile("Textures/PositiveX.png");
ASSERT_NE("", positiveXPath) << "Could not find image file for (+X) face!";
- std::string negativeYPath = data.findFile("NegativeY.png");
+ std::string negativeYPath = data.findFile("Textures/NegativeY.png");
ASSERT_NE("", negativeYPath) << "Could not find image file for (-Y) face!";
- std::string positiveYPath = data.findFile("PositiveY.png");
+ std::string positiveYPath = data.findFile("Textures/PositiveY.png");
ASSERT_NE("", positiveYPath) << "Could not find image file for (+Y) face!";
- std::string negativeZPath = data.findFile("NegativeZ.png");
+ std::string negativeZPath = data.findFile("Textures/NegativeZ.png");
ASSERT_NE("", negativeZPath) << "Could not find image file for (-Z) face!";
- std::string positiveZPath = data.findFile("PositiveZ.png");
+ std::string positiveZPath = data.findFile("Textures/PositiveZ.png");
ASSERT_NE("", positiveZPath) << "Could not find image file for (+Z) face!";
// Load the images
@@ -160,4 +152,4 @@ TEST(OsgTextureCubeMapTests, LoadImageFacesTest)
}
} // namespace Graphics
-} // namespace SurgSim
\ No newline at end of file
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgTextureRectangleTests.cpp b/SurgSim/Graphics/UnitTests/OsgTextureRectangleTests.cpp
index 0eae628..6137176 100644
--- a/SurgSim/Graphics/UnitTests/OsgTextureRectangleTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgTextureRectangleTests.cpp
@@ -52,13 +52,9 @@ TEST(OsgTextureRectangleTests, SetSizeTest)
TEST(OsgTextureRectangleTests, LoadAndClearImageTest)
{
- ASSERT_TRUE(boost::filesystem::exists("Data"));
+ SurgSim::Framework::ApplicationData data("config.txt");
- std::vector<std::string> paths;
- paths.push_back("Data/OsgTextureTests");
- SurgSim::Framework::ApplicationData data(paths);
-
- std::string imagePath = data.findFile("CheckerBoard.png");
+ std::string imagePath = data.findFile("Textures/CheckerBoard.png");
ASSERT_NE("", imagePath) << "Could not find image file!";
@@ -94,4 +90,4 @@ TEST(OsgTextureRectangleTests, LoadAndClearImageTest)
}
} // namespace Graphics
-} // namespace SurgSim
\ No newline at end of file
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgTextureTests.cpp b/SurgSim/Graphics/UnitTests/OsgTextureTests.cpp
index 803c822..3a36c2d 100644
--- a/SurgSim/Graphics/UnitTests/OsgTextureTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgTextureTests.cpp
@@ -55,13 +55,9 @@ TEST(OsgTextureTests, InitTest)
TEST(OsgTextureTests, LoadAndClearImageTest)
{
- ASSERT_TRUE(boost::filesystem::exists("Data"));
+ SurgSim::Framework::ApplicationData data("config.txt");
- std::vector<std::string> paths;
- paths.push_back("Data/OsgTextureTests");
- SurgSim::Framework::ApplicationData data(paths);
-
- std::string imagePath = data.findFile("CheckerBoard.png");
+ std::string imagePath = data.findFile("Textures/CheckerBoard.png");
ASSERT_NE("", imagePath) << "Could not find image file!";
@@ -78,4 +74,4 @@ TEST(OsgTextureTests, LoadAndClearImageTest)
}
} // namespace Graphics
-} // namespace SurgSim
\ No newline at end of file
+} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/OsgUniformBaseTests.cpp b/SurgSim/Graphics/UnitTests/OsgUniformBaseTests.cpp
index bd10de9..3713272 100644
--- a/SurgSim/Graphics/UnitTests/OsgUniformBaseTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgUniformBaseTests.cpp
@@ -34,6 +34,11 @@ public:
explicit MockOsgUniformBase(const std::string& name) : OsgUniformBase(name)
{
}
+
+ void set(const YAML::Node&)
+ {
+
+ }
};
TEST(OsgUniformBaseTests, InitTest)
@@ -60,13 +65,13 @@ TEST(OsgUniformBaseTests, StateSetTest)
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!";
+ "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!";
+ EXPECT_EQ(0u, uniforms.size())
+ << "State set should no longer have any uniforms, the uniform should have been removed!";
}
} // namespace Graphics
diff --git a/SurgSim/Graphics/UnitTests/OsgUniformTests.cpp b/SurgSim/Graphics/UnitTests/OsgUniformTests.cpp
index f0d6261..37bc0e8 100644
--- a/SurgSim/Graphics/UnitTests/OsgUniformTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgUniformTests.cpp
@@ -19,6 +19,7 @@
#include "SurgSim/Graphics/OsgUniform.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/MathConvert.h"
#include <gtest/gtest.h>
@@ -82,6 +83,15 @@ std::pair<Type, boost::any> testAccessible(const Type& value)
return std::make_pair(osgUniform->get(), osgUniform->getValue("Value"));
}
+template <class Type>
+Type testYamlSetter(const Type& value)
+{
+ YAML::Node node = YAML::convert<Type>::encode(value);
+ auto osgUniform = std::make_shared<OsgUniform<Type>>("test name");
+ osgUniform->set(node);
+ return osgUniform->get();
+}
+
/// 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
@@ -127,10 +137,14 @@ void testUniformFloat(FloatType min, FloatType max)
EXPECT_NEAR(value, result.second, Eigen::NumTraits<FloatType>::dummy_precision());
auto accessibleResult = testAccessible<FloatType>(value);
- FloatType resultValue;
+ FloatType accessibleValue;
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());
+ accessibleValue = boost::any_cast<FloatType>(accessibleResult.second);
+ EXPECT_NEAR(value, accessibleValue, Eigen::NumTraits<FloatType>::dummy_precision());
+
+ FloatType nodeValue = 0.0;
+ ASSERT_NO_THROW(nodeValue = testYamlSetter<FloatType>(value));
+ EXPECT_NEAR(value, nodeValue, Eigen::NumTraits<FloatType>::dummy_precision());
}
/// Tests OsgUniform with a vector of random floating point type values.
@@ -175,9 +189,13 @@ void testUniformInt(IntType min, IntType max)
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);
+ IntType accessibleValue;
+ ASSERT_NO_THROW({accessibleValue = boost::any_cast<IntType>(accessibleResult.second);});
+ EXPECT_EQ(value, accessibleValue);
+
+ IntType nodeValue;
+ EXPECT_NO_THROW(nodeValue = testYamlSetter<IntType>(value));
+ EXPECT_EQ(value, nodeValue);
}
/// Tests OsgUniform with a vector of random integer type values.
@@ -220,10 +238,14 @@ void testUniformEigen()
EXPECT_TRUE(fromOsg(result.second).isApprox(value));
auto accessibleResult = testAccessible<Type>(value);
- Type resultValue;
+ Type accessibleValue;
ASSERT_NO_THROW({boost::any_cast<Type>(accessibleResult.second);});
- resultValue = boost::any_cast<Type>(accessibleResult.second);
- EXPECT_TRUE(value.isApprox(resultValue));
+ accessibleValue = boost::any_cast<Type>(accessibleResult.second);
+ EXPECT_TRUE(value.isApprox(accessibleValue));
+
+ Type nodeValue;
+ EXPECT_NO_THROW(nodeValue = testYamlSetter<Type>(value));
+ EXPECT_TRUE(value.isApprox(nodeValue));
}
/// Tests OsgUniform with a vector of random Eigen type values.
diff --git a/SurgSim/Graphics/UnitTests/OsgVectorFieldRepresentationTests.cpp b/SurgSim/Graphics/UnitTests/OsgVectorFieldRepresentationTests.cpp
index 0a05e9f..9b0f5e7 100644
--- a/SurgSim/Graphics/UnitTests/OsgVectorFieldRepresentationTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgVectorFieldRepresentationTests.cpp
@@ -24,7 +24,7 @@
namespace
{
- const double epsilon = 1e-10;
+const double epsilon = 1e-6;
};
using SurgSim::Graphics::OsgVectorFieldRepresentation;
@@ -60,4 +60,20 @@ TEST(OsgVectorFieldRepresentationTests, PointSizeTest)
std::make_shared<OsgVectorFieldRepresentation>("Vector Field");
vectorFieldRepresentation->setPointSize(1.25);
EXPECT_NEAR(1.25, vectorFieldRepresentation->getPointSize(), epsilon);
+}
+
+TEST(OsgVectorFieldRepresentation, Properties)
+{
+ auto representation = std::make_shared<OsgVectorFieldRepresentation>("Vector Field");
+ EXPECT_EQ("SurgSim::Graphics::OsgVectorFieldRepresentation", representation->getClassName());
+
+ EXPECT_NO_THROW(representation->setValue("Scale", 2.34));
+ EXPECT_NEAR(2.34, representation->getValue<double>("Scale"), epsilon);
+
+ EXPECT_NO_THROW(representation->setValue("LineWidth", 4.56));
+ EXPECT_NEAR(4.56, representation->getValue<double>("LineWidth"), epsilon);
+
+ EXPECT_NO_THROW(representation->setValue("PointSize", 7.89));
+ EXPECT_NEAR(7.89, representation->getValue<double>("PointSize"), epsilon);
+
}
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/OsgViewTests.cpp b/SurgSim/Graphics/UnitTests/OsgViewTests.cpp
index 4e25191..92546fd 100644
--- a/SurgSim/Graphics/UnitTests/OsgViewTests.cpp
+++ b/SurgSim/Graphics/UnitTests/OsgViewTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2012-2013, SimQuest Solutions Inc.
+// Copyright 2012-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -27,6 +27,7 @@
#include <gtest/gtest.h>
#include <random>
+#include "osg/GraphicsContext"
namespace SurgSim
{
@@ -86,6 +87,32 @@ TEST(OsgViewTests, PositionAndDimensionsTest)
EXPECT_FALSE(view->isWindowBorderEnabled());
}
+TEST(OsgViewTests, FullScreenSize)
+{
+ std::shared_ptr<OsgView> osgView = std::make_shared<OsgView>("test name");
+ std::shared_ptr<View> view = osgView;
+
+ std::array<int, 2> dimensions = { 20, 30 };
+ view->setDimensions(dimensions);
+ EXPECT_EQ(dimensions, view->getDimensions());
+
+ view->setFullScreen(true);
+
+ osg::GraphicsContext::WindowingSystemInterface* wsi =
+ osg::GraphicsContext::getWindowingSystemInterface();
+
+ ASSERT_NE(nullptr, wsi);
+
+ unsigned int width, height;
+ wsi->getScreenResolution(osg::GraphicsContext::ScreenIdentifier(view->getTargetScreen()), width, height);
+
+ std::array<int, 2> screen = { static_cast<int>(width), static_cast<int>(height) };
+ EXPECT_EQ(screen, view->getDimensions());
+
+ view->setFullScreen(false);
+ EXPECT_EQ(dimensions, view->getDimensions());
+}
+
TEST(OsgViewTests, CameraTest)
{
std::shared_ptr<View> view = std::make_shared<OsgView>("test name");
@@ -143,9 +170,9 @@ void expectEqual(std::shared_ptr<OsgView> expected, std::shared_ptr<OsgView> act
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"))));
+ boost::any_cast<Vector3d>(actual->getValue("CameraPosition"))));
EXPECT_TRUE(boost::any_cast<Vector3d>(expected->getValue("CameraLookAt")).isApprox(
- boost::any_cast<Vector3d>(actual->getValue("CameraLookAt"))));
+ 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")),
@@ -168,7 +195,7 @@ TEST(OsgViewTests, Serialization)
/// Deserialize
std::shared_ptr<OsgView> newView;
EXPECT_NO_THROW(newView = std::dynamic_pointer_cast<OsgView>(
- node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
EXPECT_NE(nullptr, newView);
// Verify
@@ -203,8 +230,6 @@ TEST(OsgViewTests, Serialization)
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;
@@ -213,7 +238,7 @@ TEST(OsgViewTests, Serialization)
/// Deserialize
std::shared_ptr<OsgView> newView;
EXPECT_NO_THROW(newView = std::dynamic_pointer_cast<OsgView>(
- node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
EXPECT_NE(nullptr, newView);
// Verify
@@ -225,5 +250,25 @@ TEST(OsgViewTests, Serialization)
}
}
+
+TEST(OsgViewTests, OnlyOneDevice)
+{
+ std::shared_ptr<OsgView> view1 = std::make_shared<OsgView>("One");
+ std::shared_ptr<OsgView> view2 = std::make_shared<OsgView>("Two");
+
+ view1->enableKeyboardDevice(true);
+ view1->enableMouseDevice(true);
+ EXPECT_ANY_THROW(view2->enableKeyboardDevice(true));
+ EXPECT_ANY_THROW(view2->enableMouseDevice(true));
+}
+
+TEST(OsgViewTest, TargetScreenTest)
+{
+ std::shared_ptr<View> view = std::make_shared<OsgView>("view");
+
+ EXPECT_NO_THROW(view->setTargetScreen(100));
+ EXPECT_NE(100, view->getTargetScreen()); // The actual number is dependent on the number of screens
+}
+
} // namespace Graphics
} // namespace SurgSim
diff --git a/SurgSim/Graphics/UnitTests/PaintBehaviorTests.cpp b/SurgSim/Graphics/UnitTests/PaintBehaviorTests.cpp
new file mode 100644
index 0000000..b717830
--- /dev/null
+++ b/SurgSim/Graphics/UnitTests/PaintBehaviorTests.cpp
@@ -0,0 +1,55 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/FrameworkConvert.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Graphics/PaintBehavior.h"
+
+namespace SurgSim
+{
+namespace Graphics
+{
+
+class PaintBehaviorTests : public testing::Test
+{
+
+};
+
+TEST_F(PaintBehaviorTests, Serialization)
+{
+ auto component = std::make_shared<Graphics::PaintBehavior>("Painter");
+
+ auto representation = std::make_shared<Graphics::OsgMeshRepresentation>("MeshRepresentation");
+ auto color = Math::Vector4d(1.0, 0.0, 0.0, 1.0);
+
+ component->setRepresentation(representation);
+ component->setColor(color);
+ component->setAntiAlias(true);
+
+ YAML::Node node(YAML::convert<SurgSim::Framework::Component>::encode(*component));
+
+ auto decode = std::dynamic_pointer_cast<PaintBehavior>(
+ node.as<std::shared_ptr<PaintBehavior>>());
+
+ EXPECT_NE(nullptr, decode);
+ EXPECT_EQ(representation->getFullName(), decode->getRepresentation()->getFullName());
+ EXPECT_TRUE(color.isApprox(decode->getColor()));
+ EXPECT_TRUE(decode->getAntiAlias());
+}
+
+} // namespace Graphics
+} // namespace SurgSim
\ No newline at end of file
diff --git a/SurgSim/Graphics/UnitTests/RenderPassTests.cpp b/SurgSim/Graphics/UnitTests/RenderPassTests.cpp
index ed31cdd..9d1c780 100644
--- a/SurgSim/Graphics/UnitTests/RenderPassTests.cpp
+++ b/SurgSim/Graphics/UnitTests/RenderPassTests.cpp
@@ -36,7 +36,8 @@ TEST(RenderPassTests, InitTest)
EXPECT_NE(nullptr, renderPass->getCamera());
EXPECT_NE(nullptr, renderPass->getMaterial());
EXPECT_EQ(renderPass->getCamera()->getMaterial(), renderPass->getMaterial());
- EXPECT_EQ(renderPass->getCamera()->getRenderGroupReference(), renderPass->getName());
+ auto references = renderPass->getCamera()->getRenderGroupReferences();
+ EXPECT_TRUE(std::find(references.begin(), references.end(), renderPass->getName()) != references.end());
}
diff --git a/SurgSim/Graphics/UnitTests/ViewElementTests.cpp b/SurgSim/Graphics/UnitTests/ViewElementTests.cpp
index 172dbf6..19bfdfa 100644
--- a/SurgSim/Graphics/UnitTests/ViewElementTests.cpp
+++ b/SurgSim/Graphics/UnitTests/ViewElementTests.cpp
@@ -47,22 +47,22 @@ public:
getCamera()->setRenderGroupReference("Test");
}
- virtual std::shared_ptr<SurgSim::Input::CommonDevice> getKeyboardDevice() override
+ std::shared_ptr<SurgSim::Input::CommonDevice> getKeyboardDevice() override
{
return nullptr;
}
- virtual void enableKeyboardDevice(bool val) override
+ void enableKeyboardDevice(bool val) override
{
return;
}
- virtual std::shared_ptr<SurgSim::Input::CommonDevice> getMouseDevice() override
+ std::shared_ptr<SurgSim::Input::CommonDevice> getMouseDevice() override
{
return nullptr;
}
- virtual void enableMouseDevice(bool val) override
+ void enableMouseDevice(bool val) override
{
return;
}
@@ -124,12 +124,16 @@ TEST(ViewElementTests, StartUpTest)
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());
+ runtime->stop();
+
+ /// Check that the view element was initialized and retired again
+ EXPECT_TRUE(viewElement->isInitialized());
+ EXPECT_TRUE(viewElement->getView()->isInitialized());
+ EXPECT_FALSE(viewElement->getView()->isAwake());
}
TEST(ViewElementTests, ViewTest)
diff --git a/SurgSim/Graphics/UnitTests/config.txt.in b/SurgSim/Graphics/UnitTests/config.txt.in
index f614965..1f4c944 100644
--- a/SurgSim/Graphics/UnitTests/config.txt.in
+++ b/SurgSim/Graphics/UnitTests/config.txt.in
@@ -1,3 +1,3 @@
-${SURGSIM_SOURCE_DIR}/Data/
-${SURGSIM_SOURCE_DIR}/SurgSim/Testing/
-${CMAKE_CURRENT_SOURCE_DIR}/Data/
\ No newline at end of file
+${PROJECT_BINARY_DIR}/Data/
+${CMAKE_CURRENT_BINARY_DIR}/Data/
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data/
diff --git a/SurgSim/Graphics/VectorFieldRepresentation.h b/SurgSim/Graphics/VectorFieldRepresentation.h
index 4afd5e4..2a6d87a 100644
--- a/SurgSim/Graphics/VectorFieldRepresentation.h
+++ b/SurgSim/Graphics/VectorFieldRepresentation.h
@@ -24,6 +24,7 @@ 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
@@ -42,7 +43,11 @@ public:
/// Gets the vector field
/// \return The vector field
- virtual std::shared_ptr< SurgSim::Graphics::VectorField > getVectorField() const = 0;
+ virtual std::shared_ptr<SurgSim::Graphics::VectorField> getVectorField() const = 0;
+
+ /// Updates the vector field in a threadsafe manner
+ /// \param vectorfield the new data
+ virtual void updateVectorField(const SurgSim::Graphics::VectorField& vectorfield) = 0;
/// Sets vector line width
/// \param width Width of vector line
diff --git a/SurgSim/Graphics/View.cpp b/SurgSim/Graphics/View.cpp
index 192bf8e..9db5232 100644
--- a/SurgSim/Graphics/View.cpp
+++ b/SurgSim/Graphics/View.cpp
@@ -21,6 +21,7 @@
#include "SurgSim/Graphics/Camera.h"
using SurgSim::Framework::Component;
+using SurgSim::Framework::checkAndConvert;
namespace SurgSim
{
@@ -39,8 +40,10 @@ View::View(const std::string& name) :
m_screenHeight(0.0)
{
typedef std::array<int, 2> CoordinateType;
+ typedef std::array<double, 2> DoubleType;
SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, CoordinateType, Position, getPosition, setPosition);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, CoordinateType, Dimensions, getDimensions, setDimensions);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, DoubleType, DimensionsDouble, getDimensionsDouble, setDimensionsDouble);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, std::shared_ptr<SurgSim::Framework::Component>, Camera,
getCamera, setCamera);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(View, bool, WindowBorder, isWindowBorderEnabled, setWindowBorderEnabled);
@@ -56,9 +59,12 @@ View::View(const std::string& name) :
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;
+ if (m_camera != nullptr)
+ {
+ m_camera->setMainCamera(false);
+ }
+ m_camera = checkAndConvert<Camera>(camera, "SurgSim::Graphics::Camera");
+ m_camera->setMainCamera(true);
}
std::shared_ptr<Camera> View::getCamera() const
@@ -116,7 +122,7 @@ bool View::isFullScreen() const
void View::setTargetScreen(int val)
{
SURGSIM_ASSERT(!isAwake()) << "Can't change the view settings once the view has been woken up.";
- m_targetScreen = val;
+ m_targetScreen = doSetTargetScreen(val);
}
int View::getTargetScreen() const
@@ -148,12 +154,12 @@ double View::getEyeSeparation() const
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)
{
+ SURGSIM_ASSERT(!isAwake()) << "Can't change the view settings once the view has been woken up.";
m_screenWidth = val;
}
diff --git a/SurgSim/Graphics/View.h b/SurgSim/Graphics/View.h
index 1792e49..dc4440e 100644
--- a/SurgSim/Graphics/View.h
+++ b/SurgSim/Graphics/View.h
@@ -82,6 +82,14 @@ public:
/// \return Dimensions on the screen (in pixels)
virtual std::array<int, 2> getDimensions() const = 0;
+ /// Set the dimensions of this view in doubles
+ /// \param dimensions Dimensions on the screen (in pixels)
+ virtual void setDimensionsDouble(const std::array<double, 2>& dimensions) = 0;
+
+ /// Get the dimensions of this view in doubles
+ /// \return Dimensions on the screen (in pixels)
+ virtual std::array<double, 2> getDimensionsDouble() 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;
@@ -181,7 +189,9 @@ public:
private:
- virtual bool doInitialize() override;
+ bool doInitialize() override;
+
+ virtual int doSetTargetScreen(int val) = 0;
/// Camera whose image will be shown in this view
std::shared_ptr<Camera> m_camera;
diff --git a/SurgSim/Graphics/ViewElement.h b/SurgSim/Graphics/ViewElement.h
index cc31899..32a2897 100644
--- a/SurgSim/Graphics/ViewElement.h
+++ b/SurgSim/Graphics/ViewElement.h
@@ -81,7 +81,7 @@ public:
protected:
/// Initializes the scene element
/// \return True if it succeeds, false if it fails
- virtual bool doInitialize() override;
+ bool doInitialize() override;
private:
/// View component that provides the visualization of the graphics representations
diff --git a/SurgSim/Input/CMakeLists.txt b/SurgSim/Input/CMakeLists.txt
index 30519ce..f1131c3 100644
--- a/SurgSim/Input/CMakeLists.txt
+++ b/SurgSim/Input/CMakeLists.txt
@@ -15,6 +15,7 @@
set(SURGSIM_INPUT_SOURCES
+ CombiningOutputComponent.cpp
CommonDevice.cpp
InputComponent.cpp
InputManager.cpp
@@ -22,6 +23,7 @@ set(SURGSIM_INPUT_SOURCES
)
set(SURGSIM_INPUT_HEADERS
+ CombiningOutputComponent.h
CommonDevice.h
DeviceInterface.h
InputComponent.h
@@ -30,12 +32,12 @@ set(SURGSIM_INPUT_HEADERS
OutputComponent.h
OutputProducerInterface.h
)
+surgsim_create_library_header(Input.h "${SURGSIM_INPUT_HEADERS}")
surgsim_add_library(
SurgSimInput
"${SURGSIM_INPUT_SOURCES}"
"${SURGSIM_INPUT_HEADERS}"
- "SurgSim/Input"
)
set(LIBS
diff --git a/SurgSim/Input/CombiningOutputComponent.cpp b/SurgSim/Input/CombiningOutputComponent.cpp
new file mode 100644
index 0000000..018eb87
--- /dev/null
+++ b/SurgSim/Input/CombiningOutputComponent.cpp
@@ -0,0 +1,167 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/CombiningOutputComponent.h"
+
+#include <boost/thread/locks.hpp>
+#include <unordered_set>
+
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+
+namespace
+{
+
+/// This functor accumulates the force and torque. No other data is passed along.
+/// This can be used to combine the forces and torques from multiple OutputComponents (e.g., from multiple
+/// VirtualToolCouplers) to drive a single haptic device.
+auto DEFAULT_FUNCTOR = [](const std::vector<std::shared_ptr<SurgSim::Input::OutputComponent>>& outputs,
+ SurgSim::DataStructures::DataGroup *resultData)
+{
+ bool result = false;
+ SurgSim::Math::Vector3d cumulativeForce = SurgSim::Math::Vector3d::Zero();
+ SurgSim::Math::Vector3d cumulativeTorque = SurgSim::Math::Vector3d::Zero();
+ for (const auto& output : outputs)
+ {
+ if (output != nullptr)
+ {
+ SurgSim::DataStructures::DataGroup data;
+ if (output->requestOutput("", &data))
+ {
+ result = true;
+ SurgSim::Math::Vector3d force;
+ if (data.vectors().get(SurgSim::DataStructures::Names::FORCE, &force))
+ {
+ cumulativeForce += force;
+ }
+ SurgSim::Math::Vector3d torque;
+ if (data.vectors().get(SurgSim::DataStructures::Names::TORQUE, &torque))
+ {
+ cumulativeTorque += torque;
+ }
+ }
+ }
+ }
+
+ if (resultData->isEmpty())
+ {
+ SurgSim::DataStructures::DataGroupBuilder builder;
+ builder.addVector(SurgSim::DataStructures::Names::FORCE);
+ builder.addVector(SurgSim::DataStructures::Names::TORQUE);
+ *resultData = builder.createData();
+ }
+
+ if (result && resultData->vectors().hasEntry(SurgSim::DataStructures::Names::FORCE))
+ {
+ resultData->vectors().set(SurgSim::DataStructures::Names::FORCE, cumulativeForce);
+ }
+ if (result && resultData->vectors().hasEntry(SurgSim::DataStructures::Names::TORQUE))
+ {
+ resultData->vectors().set(SurgSim::DataStructures::Names::TORQUE, cumulativeTorque);
+ }
+ return result;
+};
+}
+
+namespace SurgSim
+{
+namespace Input
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Input::CombiningOutputComponent, CombiningOutputComponent);
+
+CombiningOutputComponent::CombiningOutputComponent(const std::string& name) :
+ SurgSim::Input::OutputComponent(name),
+ m_combiner(DEFAULT_FUNCTOR)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(CombiningOutputComponent,
+ std::vector<std::shared_ptr<SurgSim::Framework::Component>>,
+ Outputs, getOutputs, setOutputs);
+}
+
+CombiningOutputComponent::~CombiningOutputComponent()
+{
+}
+
+void CombiningOutputComponent::setData(const SurgSim::DataStructures::DataGroup& dataGroup)
+{
+ SURGSIM_FAILURE() << "Cannot setData on CombiningOutputComponent " << getFullName();
+}
+
+std::vector<std::shared_ptr<SurgSim::Framework::Component>> CombiningOutputComponent::getOutputs() const
+{
+ std::vector<std::shared_ptr<SurgSim::Framework::Component>> outputs;
+ for (const auto& output : m_outputs)
+ {
+ auto shared = output.lock();
+ if (shared != nullptr)
+ {
+ outputs.push_back(shared);
+ }
+ }
+ return outputs;
+}
+
+void CombiningOutputComponent::setOutputs(const std::vector<std::shared_ptr<SurgSim::Framework::Component>>& outputs)
+{
+ std::unordered_set<std::shared_ptr<SurgSim::Input::OutputComponent>> uniqueOutputs;
+ m_outputs.clear();
+ for (const auto& component : outputs)
+ {
+ auto output = SurgSim::Framework::checkAndConvert<SurgSim::Input::OutputComponent>(component,
+ "SurgSim::Input::OutputComponent");
+ if (uniqueOutputs.insert(output).second)
+ {
+ m_outputs.push_back(output);
+ }
+ }
+}
+
+void CombiningOutputComponent::setCombiner(FunctorType combiner)
+{
+ m_combiner = combiner;
+}
+
+bool CombiningOutputComponent::requestOutput(const std::string& device,
+ SurgSim::DataStructures::DataGroup* outputData)
+{
+ std::vector<std::shared_ptr<OutputComponent>> shareds;
+ {
+ boost::lock_guard<boost::mutex> lock(m_mutex);
+ std::vector<std::vector<std::weak_ptr<OutputComponent>>::const_iterator> stale;
+ for (auto weak = m_outputs.cbegin(); weak != m_outputs.cend(); ++weak)
+ {
+ auto shared = weak->lock();
+ if (shared == nullptr)
+ {
+ stale.push_back(weak);
+ }
+ else
+ {
+ shareds.push_back(shared);
+ }
+ }
+
+ std::reverse(stale.begin(), stale.end());
+ for (auto it : stale)
+ {
+ m_outputs.erase(it);
+ }
+ }
+ return m_combiner(shareds, outputData);
+}
+
+}; // namespace Input
+}; // namespace SurgSim
diff --git a/SurgSim/Input/CombiningOutputComponent.h b/SurgSim/Input/CombiningOutputComponent.h
new file mode 100644
index 0000000..7f08213
--- /dev/null
+++ b/SurgSim/Input/CombiningOutputComponent.h
@@ -0,0 +1,86 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_COMBININGOUTPUTCOMPONENT_H
+#define SURGSIM_INPUT_COMBININGOUTPUTCOMPONENT_H
+
+#include <boost/thread/mutex.hpp>
+#include <functional>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Input/OutputComponent.h"
+
+namespace SurgSim
+{
+namespace Input
+{
+
+SURGSIM_STATIC_REGISTRATION(CombiningOutputComponent);
+
+/// CombiningOutputComponents is-a OutputComponent that takes one or more OutputComponents and combines their
+/// datagroups into a single output datagroup.
+/// The combining functor can be set, with the default functor accumulating all vectors named "force", and all vectors
+/// named "torque". The resulting DataGroup has only those two specific entries, and can be used to drive a haptic
+/// device from multiple OutputComponents.
+class CombiningOutputComponent : public SurgSim::Input::OutputComponent
+{
+public:
+ typedef std::function<bool(const std::vector<std::shared_ptr<SurgSim::Input::OutputComponent>>&,
+ SurgSim::DataStructures::DataGroup*)> FunctorType;
+
+ /// Constructor
+ /// \param name Name of this output component
+ explicit CombiningOutputComponent(const std::string& name);
+
+ /// Destructor
+ virtual ~CombiningOutputComponent();
+
+ SURGSIM_CLASSNAME(SurgSim::Input::CombiningOutputComponent);
+
+ void setData(const SurgSim::DataStructures::DataGroup& dataGroup) override;
+
+ /// \return The OutputComponents that will be combined.
+ std::vector<std::shared_ptr<SurgSim::Framework::Component>> getOutputs() const;
+
+ /// \param outputs The OutputComponents that will be combined. They should have their device names set to the
+ /// empty string.
+ void setOutputs(const std::vector<std::shared_ptr<SurgSim::Framework::Component>>& outputs);
+
+ /// Set the function to do the combining.
+ /// The parameter is a function that receives all the OutputComponents and returns a DataGroup and a bool
+ /// indicating success.
+ void setCombiner(FunctorType combiner);
+
+ bool requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData) override;
+
+private:
+ /// The OutputComponents that will be combined.
+ std::vector<std::weak_ptr<SurgSim::Input::OutputComponent>> m_outputs;
+
+ /// The function that takes the OutputComponents and returns a DataGroup and a bool that specifies whether
+ /// it was successful.
+ FunctorType m_combiner;
+
+ /// The mutex that protects the outputs.
+ boost::mutex m_mutex;
+};
+
+}; // namespace Input
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Input/CommonDevice.cpp b/SurgSim/Input/CommonDevice.cpp
index 920da61..35c2fc0 100644
--- a/SurgSim/Input/CommonDevice.cpp
+++ b/SurgSim/Input/CommonDevice.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -14,11 +14,13 @@
// limitations under the License.
#include "SurgSim/Input/CommonDevice.h"
-#include "SurgSim/Framework/Log.h"
#include <boost/thread/mutex.hpp>
#include <boost/thread/locks.hpp>
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
namespace SurgSim
{
@@ -26,51 +28,32 @@ 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)
+ m_inputData(DataStructures::DataGroup())
{
}
-CommonDevice::CommonDevice(const std::string& name, const SurgSim::DataStructures::DataGroup& inputData) :
+CommonDevice::CommonDevice(const std::string& name, const DataStructures::DataGroup& inputData) :
m_name(name),
m_nameForCallback(name),
- m_inputData(inputData),
- m_state(new State)
+ m_inputData(inputData)
{
}
-CommonDevice::CommonDevice(const std::string& name, SurgSim::DataStructures::DataGroup&& inputData) :
+CommonDevice::CommonDevice(const std::string& name, DataStructures::DataGroup&& inputData) :
m_name(name),
m_nameForCallback(name),
- m_inputData(std::move(inputData)),
- m_state(new State)
+ m_inputData(std::move(inputData))
{
}
CommonDevice::~CommonDevice()
{
+ clearInputConsumers();
+ clearOutputProducer();
}
std::string CommonDevice::getName() const
@@ -78,6 +61,15 @@ std::string CommonDevice::getName() const
return m_name;
}
+std::string CommonDevice::getClassName() const
+{
+ SURGSIM_LOG_WARNING(Framework::Logger::getDefaultLogger())
+ << "getClassName() called on CommonDevice base class, this is wrong in almost all cases," <<
+ " this means there is a class that does not have getClassName() defined.";
+ return "SurgSim::Devices::CommonDevice";
+}
+
+
void CommonDevice::setNameForCallback(const std::string& name)
{
m_nameForCallback = name;
@@ -90,38 +82,40 @@ std::string CommonDevice::getNameForCallback() const
bool CommonDevice::addInputConsumer(std::shared_ptr<InputConsumerInterface> inputConsumer)
{
- if (! inputConsumer)
+ if (inputConsumer == nullptr)
{
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())
+ boost::lock_guard<boost::mutex> lock(m_consumerProducerMutex);
+ for (const auto& input : m_inputConsumerList)
{
- return false;
+ if (input.lock() == inputConsumer)
+ {
+ 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));
+ m_inputConsumerList.emplace_back(std::move(inputConsumer));
return true;
}
bool CommonDevice::removeInputConsumer(std::shared_ptr<InputConsumerInterface> inputConsumer)
{
- if (! inputConsumer)
+ if (inputConsumer == nullptr)
{
return false;
}
- boost::lock_guard<boost::mutex> lock(m_state->consumerProducerMutex);
- for (auto it = m_state->inputConsumerList.begin(); it != m_state->inputConsumerList.end(); ++it)
+ boost::lock_guard<boost::mutex> lock(m_consumerProducerMutex);
+ for (auto it = m_inputConsumerList.begin(); it != m_inputConsumerList.end(); ++it)
{
- if (*it == inputConsumer)
+ if (it->lock() == inputConsumer)
{
- m_state->inputConsumerList.erase(it);
+ m_inputConsumerList.erase(it);
// The iterator is now invalid.
return true;
}
@@ -129,19 +123,25 @@ bool CommonDevice::removeInputConsumer(std::shared_ptr<InputConsumerInterface> i
return false;
}
+void CommonDevice::clearInputConsumers()
+{
+ boost::lock_guard<boost::mutex> lock(m_consumerProducerMutex);
+ m_inputConsumerList.clear();
+}
+
bool CommonDevice::setOutputProducer(std::shared_ptr<OutputProducerInterface> outputProducer)
{
- if (! outputProducer)
+ if (outputProducer == nullptr)
{
return false;
}
- boost::lock_guard<boost::mutex> lock(m_state->consumerProducerMutex);
- if (m_state->outputProducer == outputProducer)
+ boost::lock_guard<boost::mutex> lock(m_consumerProducerMutex);
+ if (m_outputProducer.lock() == outputProducer)
{
return false;
}
- m_state->outputProducer = std::move(outputProducer);
+ m_outputProducer = std::move(outputProducer);
return true;
}
@@ -152,40 +152,51 @@ bool CommonDevice::removeOutputProducer(std::shared_ptr<OutputProducerInterface>
return false;
}
- boost::lock_guard<boost::mutex> lock(m_state->consumerProducerMutex);
- if (m_state->outputProducer == outputProducer)
+ boost::lock_guard<boost::mutex> lock(m_consumerProducerMutex);
+ if (m_outputProducer.lock() == outputProducer)
{
- m_state->outputProducer.reset();
+ m_outputProducer.reset();
return true;
}
return false;
}
+void CommonDevice::clearOutputProducer()
+{
+ boost::lock_guard<boost::mutex> lock(m_consumerProducerMutex);
+ m_outputProducer.reset();
+}
+
bool CommonDevice::hasOutputProducer()
{
- return (m_state->outputProducer != nullptr);
+ return (m_outputProducer.lock() != 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)
+ boost::lock_guard<boost::mutex> lock(m_consumerProducerMutex);
+ for (auto it = m_inputConsumerList.begin(); it != m_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);
+ auto inputConsumer = it->lock();
+ if (inputConsumer != nullptr)
+ {
+ inputConsumer->handleInput(m_nameForCallback, m_inputData);
+ }
}
}
bool CommonDevice::pullOutput()
{
- boost::lock_guard<boost::mutex> lock(m_state->consumerProducerMutex);
- if (m_state->outputProducer)
+ boost::lock_guard<boost::mutex> lock(m_consumerProducerMutex);
+ auto outputProducer = m_outputProducer.lock();
+ if (outputProducer != nullptr)
{
// 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);
+ bool gotOutput = outputProducer->requestOutput(m_nameForCallback, &m_outputData);
if (gotOutput)
{
return true;
@@ -200,12 +211,12 @@ bool CommonDevice::pullOutput()
return false;
}
-SurgSim::DataStructures::DataGroup& CommonDevice::getInputData()
+DataStructures::DataGroup& CommonDevice::getInputData()
{
return m_inputData;
}
-const SurgSim::DataStructures::DataGroup& CommonDevice::getOutputData() const
+const DataStructures::DataGroup& CommonDevice::getOutputData() const
{
return m_outputData;
}
diff --git a/SurgSim/Input/CommonDevice.h b/SurgSim/Input/CommonDevice.h
index ae81157..6d83e57 100644
--- a/SurgSim/Input/CommonDevice.h
+++ b/SurgSim/Input/CommonDevice.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -20,7 +20,6 @@
#include <string>
#include "SurgSim/Input/DeviceInterface.h"
-#include "SurgSim/Input/InputConsumerInterface.h"
#include "SurgSim/DataStructures/DataGroup.h"
namespace SurgSim
@@ -28,6 +27,8 @@ namespace SurgSim
namespace Input
{
+class InputConsumerInterface;
+class OutputProducerInterface;
/// 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.
@@ -44,7 +45,7 @@ public:
/// \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);
+ CommonDevice(const std::string& name, const DataStructures::DataGroup& inputData);
/// Constructor.
///
@@ -52,13 +53,14 @@ public:
/// \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);
+ CommonDevice(const std::string& name, DataStructures::DataGroup&& inputData);
/// Destructor.
virtual ~CommonDevice();
- /// Return a (hopefully unique) device name.
- virtual std::string getName() const override;
+ std::string getName() const override;
+
+ std::string getClassName() 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.
@@ -70,29 +72,19 @@ public:
/// \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;
+ bool addInputConsumer(std::shared_ptr<InputConsumerInterface> inputConsumer) override;
+
+ bool removeInputConsumer(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;
+ void clearInputConsumers() 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;
+ 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;
+ 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;
+ bool hasOutputProducer() override;
+
+ void clearOutputProducer() override;
protected:
@@ -106,14 +98,14 @@ protected:
/// 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();
+ 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;
+ const DataStructures::DataGroup& getOutputData() const;
private:
struct State;
@@ -124,23 +116,20 @@ private:
std::string m_nameForCallback;
/// The data the device is providing to its input consumers.
- SurgSim::DataStructures::DataGroup m_inputData;
+ 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;
+ DataStructures::DataGroup m_outputData;
+
+ /// The list of input consumers.
+ std::vector<std::weak_ptr<InputConsumerInterface>> m_inputConsumerList;
+
+ /// The output producer, if any.
+ std::weak_ptr<OutputProducerInterface> m_outputProducer;
+
+ /// The mutex that protects the consumers and the producer.
+ boost::mutex m_consumerProducerMutex;
+
};
diff --git a/SurgSim/Input/DeviceInterface.h b/SurgSim/Input/DeviceInterface.h
index 2080d86..88a1528 100644
--- a/SurgSim/Input/DeviceInterface.h
+++ b/SurgSim/Input/DeviceInterface.h
@@ -21,6 +21,8 @@
#include "SurgSim/Input/InputConsumerInterface.h"
#include "SurgSim/Input/OutputProducerInterface.h"
+#include "SurgSim/Framework/Accessible.h"
+#include "SurgSim/Framework/ObjectFactory.h"
namespace SurgSim
{
@@ -36,7 +38,7 @@ namespace Input
///
/// Derived classes will likely want to hide their constructor and only
/// allow creation through a manager object for that type of device.
-class DeviceInterface
+class DeviceInterface : public Framework::Accessible, public Framework::FactoryBase1<DeviceInterface, std::string>
{
public:
/// Virtual destructor (empty).
@@ -47,12 +49,20 @@ public:
/// Return a (hopefully unique) device name.
virtual std::string getName() const = 0;
+ /// The class name for this class
+ /// \note Use the SURGSIM_CLASSNAME macro in derived classes.
+ /// \return The fully namespace qualified name of this class.
+ virtual std::string getClassName() 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;
+ /// \return true if the device has been initialized and has not yet been finalized.
+ virtual bool isInitialized() const = 0;
+
/// Adds an input consumer that will be notified when the application input state is updated.
///
/// \param inputConsumer The input consumer to be added.
@@ -63,6 +73,9 @@ public:
/// \param inputConsumer The input consumer to be removed.
virtual bool removeInputConsumer(std::shared_ptr<InputConsumerInterface> inputConsumer) = 0;
+ /// Removes all InputConsumers.
+ virtual void clearInputConsumers() = 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.
///
@@ -78,7 +91,10 @@ public:
/// \return true if there is an output producer, false if not.
virtual bool hasOutputProducer() = 0;
-protected:
+ /// Removes any OutputProducer
+ virtual void clearOutputProducer() = 0;
+
+private:
/// Finalize (de-initialize) the device.
virtual bool finalize() = 0;
};
diff --git a/SurgSim/Input/InputComponent.cpp b/SurgSim/Input/InputComponent.cpp
index bb2c642..578c838 100644
--- a/SurgSim/Input/InputComponent.cpp
+++ b/SurgSim/Input/InputComponent.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -15,11 +15,8 @@
#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
{
@@ -27,54 +24,9 @@ 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>())
+ Representation(name),
+ m_hasInput(false)
{
SURGSIM_ADD_SERIALIZABLE_PROPERTY(InputComponent, std::string, DeviceName,
getDeviceName, setDeviceName);
@@ -89,16 +41,17 @@ void InputComponent::setDeviceName(const std::string& deviceName)
m_deviceName = deviceName;
}
-bool InputComponent::isDeviceConnected()
+std::string InputComponent::getDeviceName() const
{
- return m_deviceConnected;
+ return m_deviceName;
}
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);
+ if (m_hasInput)
+ {
+ m_lastInput.get(dataGroup);
+ }
}
bool InputComponent::doInitialize()
@@ -111,21 +64,17 @@ bool InputComponent::doWakeUp()
return true;
}
-std::string InputComponent::getDeviceName() const
-{
- return m_deviceName;
-}
-
-void InputComponent::connectDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device)
+void InputComponent::initializeInput(const std::string& device,
+ const SurgSim::DataStructures::DataGroup& initialData)
{
- device->addInputConsumer(m_input);
- m_deviceConnected = true;
+ m_hasInput = true;
+ m_lastInput.set(initialData);
}
-void InputComponent::disconnectDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device)
+void InputComponent::handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData)
{
- device->removeInputConsumer(m_input);
- m_deviceConnected = false;
+ m_hasInput = true;
+ m_lastInput.set(inputData);
}
}; // namespace Input
diff --git a/SurgSim/Input/InputComponent.h b/SurgSim/Input/InputComponent.h
index 335a805..0f6afe4 100644
--- a/SurgSim/Input/InputComponent.h
+++ b/SurgSim/Input/InputComponent.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,29 +16,26 @@
#ifndef SURGSIM_INPUT_INPUTCOMPONENT_H
#define SURGSIM_INPUT_INPUTCOMPONENT_H
-#include <string>
+#include <atomic>
#include <memory>
-#include "SurgSim/Framework/Component.h"
+#include <string>
-namespace SurgSim
-{
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Framework/LockedContainer.h"
+#include "SurgSim/Framework/Representation.h"
+#include "SurgSim/Input/InputConsumerInterface.h"
-namespace DataStructures
+namespace SurgSim
{
-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
+/// InputComponents connect devices to SceneElements, facilitating data transfer
+/// from a device to SceneElements and other Components.
+class InputComponent : public SurgSim::Framework::Representation, public InputConsumerInterface
{
public:
/// Constructor
@@ -54,42 +51,31 @@ public:
/// \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 device name.
+ /// \return The device name.
+ std::string getDeviceName() const;
/// 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;
+ bool doInitialize() override;
- /// Overridden from Component, do nothing
- virtual bool doWakeUp() override;
+ bool doWakeUp() override;
- /// Gets device name.
- /// \return The device name.
- std::string getDeviceName() const;
+ void initializeInput(const std::string& device, const SurgSim::DataStructures::DataGroup& initialData) override;
+
+ void handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override;
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;
+
+ /// Thread safe container of most recent input data
+ SurgSim::Framework::LockedContainer<SurgSim::DataStructures::DataGroup> m_lastInput;
+
+ std::atomic<bool> m_hasInput;
};
}; // namespace Input
diff --git a/SurgSim/Input/InputManager.cpp b/SurgSim/Input/InputManager.cpp
index 5bdd8db..14b1ec1 100644
--- a/SurgSim/Input/InputManager.cpp
+++ b/SurgSim/Input/InputManager.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -46,13 +46,17 @@ bool InputManager::doStartUp()
bool InputManager::doUpdate(double dt)
{
- // Add all components that came in before the last update
+ processBehaviors(dt);
processComponents();
+ return true;
+}
- // Process specific behaviors belongs to this manager
- processBehaviors(dt);
+void InputManager::doBeforeStop()
+{
+ retireComponents(m_inputs);
+ retireComponents(m_outputs);
- return true;
+ ComponentManager::doBeforeStop();
}
bool InputManager::executeAdditions(const std::shared_ptr<SurgSim::Framework::Component>& component)
@@ -61,16 +65,16 @@ bool InputManager::executeAdditions(const std::shared_ptr<SurgSim::Framework::Co
if (input != nullptr)
{
boost::lock_guard<boost::mutex> lock(m_mutex);
- // Early exit
- return addInputComponent(input);
+ addInputComponent(input);
+ return true;
}
auto output = tryAddComponent(component, &m_outputs);
if (output != nullptr)
{
boost::lock_guard<boost::mutex> lock(m_mutex);
- // Early exit
- return addOutputComponent(output);
+ addOutputComponent(output);
+ return true;
}
// If we got he the component was neither an Input nor and OutputComponent, no add was performed
@@ -80,19 +84,26 @@ bool InputManager::executeAdditions(const std::shared_ptr<SurgSim::Framework::Co
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()]);
+ DeviceInterface* device = nullptr;
+ if (tryFindDevice(input->getDeviceName(), &device))
+ {
+ device->removeInputConsumer(input);
+ }
result = true;
}
- else if(tryRemoveComponent(component, &m_outputs))
+ else if (tryRemoveComponent(component, &m_outputs))
{
auto output = std::dynamic_pointer_cast<OutputComponent>(component);
- m_devices[output->getDeviceName()]->setOutputProducer(nullptr);
+ DeviceInterface* device = nullptr;
+ if (tryFindDevice(output->getDeviceName(), &device))
+ {
+ device->removeOutputProducer(output);
+ }
result = true;
}
return result;
@@ -101,17 +112,18 @@ bool InputManager::executeRemovals(const std::shared_ptr<SurgSim::Framework::Com
bool InputManager::addInputComponent(const std::shared_ptr<InputComponent>& input)
{
bool result = false;
- if (m_devices.find(input->getDeviceName()) != m_devices.end())
+ DeviceInterface* device = nullptr;
+ if (tryFindDevice(input->getDeviceName(), &device))
{
- input->connectDevice(m_devices[input->getDeviceName()]);
- SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Added input component " << input->getName() <<
- " connected to device " << input->getDeviceName();
+ device->addInputConsumer(input);
+ SURGSIM_LOG_INFO(m_logger) << "Added input component " << input->getFullName()
+ << " 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() << "'.";
+ SURGSIM_LOG_CRITICAL(m_logger) << " Could not find Device named '"
+ << input->getDeviceName() << "' when adding input component named '" << input->getFullName() << "'.";
}
return result;
}
@@ -119,26 +131,29 @@ bool InputManager::addInputComponent(const std::shared_ptr<InputComponent>& inpu
bool InputManager::addOutputComponent(const std::shared_ptr<OutputComponent>& output)
{
bool result = false;
- if (m_devices.find(output->getDeviceName()) != m_devices.end())
+ const auto outputName = output->getFullName();
+ const auto deviceName = output->getDeviceName();
+ DeviceInterface* device = nullptr;
+ if (tryFindDevice(deviceName, &device))
{
- if (!m_devices[output->getDeviceName()]->hasOutputProducer())
+ if (device->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;
+ SURGSIM_LOG_WARNING(m_logger) << "Trying to add OutputProducer " << outputName
+ << " to device " << deviceName
+ << ", but the device already has an OutputProducer assigned, this add will be ignored!";
}
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!";
+ device->setOutputProducer(output);
+ SURGSIM_LOG_INFO(m_logger) << "Added output component " << outputName << " connected to device "
+ << deviceName;
+ result = true;
}
}
else
{
- SURGSIM_LOG_CRITICAL(m_logger) << __FUNCTION__ << " Could not find Device with name " <<
- output->getDeviceName() << " when adding output component " << output->getName();
+ SURGSIM_LOG_CRITICAL(m_logger) << "Could not find Device with name " << deviceName
+ << " when adding output component " << outputName;
}
return result;
}
@@ -147,16 +162,21 @@ bool InputManager::addDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> de
{
bool result = false;
boost::lock_guard<boost::mutex> lock(m_mutex);
- if (m_devices.find(device->getName()) == m_devices.cend())
+ DeviceInterface* foundDevice = nullptr;
+ const auto& deviceName = device->getName();
+ if (deviceName == "")
{
- m_devices[device->getName()] = device;
- SURGSIM_LOG_INFO(m_logger) << __FUNCTION__ << " Added device " << device->getName();
- result = true;
+ SURGSIM_LOG_WARNING(m_logger) << "Cannot add a device that has an empty name.";
+ }
+ else if (tryFindDevice(deviceName, &foundDevice))
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Device " << deviceName << " is already available in Input Manager";
}
else
{
- SURGSIM_LOG_WARNING(m_logger) << __FUNCTION__ << " Device " << device->getName() <<
- " is already available in Input Manager";
+ m_devices[deviceName] = device;
+ SURGSIM_LOG_INFO(m_logger) << "Added device " << deviceName;
+ result = true;
}
return result;
}
@@ -169,12 +189,12 @@ bool InputManager::removeDevice(std::shared_ptr<SurgSim::Input::DeviceInterface>
if (it != m_devices.end())
{
m_devices.erase(it);
- SURGSIM_LOG_DEBUG(m_logger) << __FUNCTION__ << " Removed device " << device->getName();
+ SURGSIM_LOG_DEBUG(m_logger) << "Removed device " << device->getName();
result = true;
}
else
{
- SURGSIM_LOG_DEBUG(m_logger) << __FUNCTION__ << " Failed to remove device " << device->getName();
+ SURGSIM_LOG_DEBUG(m_logger) << "Failed to find device to remove, " << device->getName();
}
return result;
}
@@ -184,5 +204,16 @@ int InputManager::getType() const
return SurgSim::Framework::MANAGER_TYPE_INPUT;
}
+bool InputManager::tryFindDevice(const std::string& name, DeviceInterface** device)
+{
+ auto found = m_devices.find(name);
+ if (found != m_devices.end())
+ {
+ *device = found->second.get();
+ return true;
+ }
+ return false;
+}
+
} // Input
} // SurgSim
diff --git a/SurgSim/Input/InputManager.h b/SurgSim/Input/InputManager.h
index f8f4ccc..a8b0bd4 100644
--- a/SurgSim/Input/InputManager.h
+++ b/SurgSim/Input/InputManager.h
@@ -52,12 +52,13 @@ public:
/// \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;
+ int getType() const override;
private:
- virtual bool doInitialize() override;
- virtual bool doStartUp() override;
- virtual bool doUpdate(double dt) override;
+ bool doInitialize() override;
+ bool doStartUp() override;
+ bool doUpdate(double dt) override;
+ void doBeforeStop() 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
@@ -67,12 +68,12 @@ private:
/// \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;
+ 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;
+ bool executeRemovals(const std::shared_ptr<SurgSim::Framework::Component>& component) override;
/// Specific call for input components.
@@ -82,6 +83,12 @@ private:
/// Specific call for output components.
bool addOutputComponent(const std::shared_ptr<OutputComponent>& output);
+ /// Returns a device with the given name, if one is available.
+ /// \param name The name of the device.
+ /// \param device [out] The device. Unchanged if the return value is false.
+ /// \return true if the device was found.
+ bool tryFindDevice(const std::string& name, DeviceInterface** device);
+
/// Collection of all input components.
std::vector<std::shared_ptr<InputComponent>> m_inputs;
/// Collection of all output components.
diff --git a/SurgSim/Input/OutputComponent.cpp b/SurgSim/Input/OutputComponent.cpp
index a046f60..fa14882 100644
--- a/SurgSim/Input/OutputComponent.cpp
+++ b/SurgSim/Input/OutputComponent.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -15,69 +15,23 @@
#include "SurgSim/Input/OutputComponent.h"
-#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Framework/Log.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;
-};
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Input::OutputComponent, OutputComponent);
OutputComponent::OutputComponent(const std::string& name) :
- Component(name),
+ Representation(name),
m_deviceName(),
- m_deviceConnected(false),
- m_output(std::make_shared<OutputProducer>())
+ m_haveData(false)
{
- SURGSIM_ADD_SERIALIZABLE_PROPERTY(OutputComponent, std::string, DeviceName,
- getDeviceName, setDeviceName);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(OutputComponent, std::string, DeviceName, getDeviceName, setDeviceName);
}
OutputComponent::~OutputComponent()
@@ -86,17 +40,15 @@ OutputComponent::~OutputComponent()
void OutputComponent::setDeviceName(const std::string& deviceName)
{
+ SURGSIM_ASSERT(!isInitialized()) << "Cannot call OutputComponent::setDeviceName after initialization of "
+ << getFullName();
m_deviceName = deviceName;
}
-bool OutputComponent::isDeviceConnected()
-{
- return m_deviceConnected;
-}
-
void OutputComponent::setData(const SurgSim::DataStructures::DataGroup& dataGroup)
{
- m_output->setData(dataGroup);
+ m_lastOutput.set(dataGroup);
+ m_haveData = true;
}
bool OutputComponent::doInitialize()
@@ -114,17 +66,17 @@ std::string OutputComponent::getDeviceName() const
return m_deviceName;
}
-void OutputComponent::connectDevice(std::shared_ptr<SurgSim::Input::DeviceInterface> device)
+bool OutputComponent::requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData)
{
- device->setOutputProducer(m_output);
- m_deviceConnected = true;
+ 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;
}
-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
index 3098de3..2df2c15 100644
--- a/SurgSim/Input/OutputComponent.h
+++ b/SurgSim/Input/OutputComponent.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -18,26 +18,25 @@
#include <string>
#include <memory>
-#include "SurgSim/Framework/Component.h"
-namespace SurgSim
-{
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Framework/LockedContainer.h"
+#include "SurgSim/Framework/Representation.h"
+#include "SurgSim/Input/OutputProducerInterface.h"
-namespace DataStructures
+
+namespace SurgSim
{
-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
+/// OutputComponents connect SceneElements to devices, facilitating data
+/// transfer from a SceneElement to a device.
+class OutputComponent : public SurgSim::Framework::Representation, public OutputProducerInterface
{
public:
/// Constructor
@@ -49,26 +48,16 @@ public:
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.
+ /// \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);
+ /// Gets device name.
+ /// \return The device name.
+ std::string getDeviceName() const;
/// Sets the output data.
/// \param dataGroup The data to output.
- void setData(const SurgSim::DataStructures::DataGroup& dataGroup);
+ virtual void setData(const SurgSim::DataStructures::DataGroup& dataGroup);
/// Overridden from Component, do nothing
virtual bool doInitialize();
@@ -76,17 +65,17 @@ public:
/// Overridden from Component, do nothing
virtual bool doWakeUp();
- /// Gets device name.
- /// \return The device name.
- std::string getDeviceName() const;
+ bool requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData) override;
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;
+
+ /// Thread safe container of most recent output data
+ SurgSim::Framework::LockedContainer<SurgSim::DataStructures::DataGroup> m_lastOutput;
+
+ /// True if there is data available
+ bool m_haveData;
};
}; // namespace Input
diff --git a/SurgSim/Input/UnitTests/CMakeLists.txt b/SurgSim/Input/UnitTests/CMakeLists.txt
index 95f34e9..f19e3ba 100644
--- a/SurgSim/Input/UnitTests/CMakeLists.txt
+++ b/SurgSim/Input/UnitTests/CMakeLists.txt
@@ -1,5 +1,5 @@
# This file is a part of the OpenSurgSim project.
-# Copyright 2012-2013, SimQuest Solutions Inc.
+# Copyright 2012-2016, SimQuest Solutions Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -18,6 +18,7 @@ include_directories(
)
set(UNIT_TEST_SOURCES
+ CombiningOutputComponentTest.cpp
CommonDeviceTests.cpp
InputComponentTest.cpp
InputManagerTest.cpp
@@ -29,10 +30,19 @@ set(UNIT_TEST_HEADERS
TestDevice.h
)
-set(LIBS
- SurgSimInput
+set(LIBS
+ IdentityPoseDevice
+ SurgSimInput
)
surgsim_add_unit_tests(SurgSimInputTest)
set_target_properties(SurgSimInputTest PROPERTIES FOLDER "Input")
+
+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"
+)
diff --git a/SurgSim/Input/UnitTests/CombiningOutputComponentTest.cpp b/SurgSim/Input/UnitTests/CombiningOutputComponentTest.cpp
new file mode 100644
index 0000000..27ef96b
--- /dev/null
+++ b/SurgSim/Input/UnitTests/CombiningOutputComponentTest.cpp
@@ -0,0 +1,309 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 CombiningOutputComponent class.
+
+#include <boost/chrono.hpp>
+#include <boost/thread.hpp>
+#include <gtest/gtest.h>
+#include <memory>
+#include <set>
+#include <string>
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Input/CombiningOutputComponent.h"
+#include "SurgSim/Input/CommonDevice.h"
+#include "SurgSim/Input/InputManager.h"
+#include "SurgSim/Input/OutputComponent.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace
+{
+const double ERROR_EPSILON = 1e-7;
+
+auto DO_NOTHING_FUNCTOR = [](const std::vector<std::shared_ptr<SurgSim::Input::OutputComponent>>&,
+ SurgSim::DataStructures::DataGroup*)
+{
+ return false;
+};
+
+/// Device that exposes pullOutput and getOutputData.
+class MockDevice : public SurgSim::Input::CommonDevice
+{
+public:
+
+ explicit MockDevice(const std::string& name) : SurgSim::Input::CommonDevice(name)
+ {
+ }
+
+ ~MockDevice()
+ {
+ }
+
+ using CommonDevice::pullOutput;
+
+ using CommonDevice::getOutputData;
+
+ bool initialize() override
+ {
+ return true;
+ }
+
+ bool isInitialized() const override
+ {
+ return true;
+ }
+
+ bool finalize() override
+ {
+ return true;
+ }
+};
+
+};
+
+TEST(CombiningOutputComponentTest, NoOutputs)
+{
+ auto combiningOutputComponent = std::make_shared<SurgSim::Input::CombiningOutputComponent>("combiner");
+ auto mockDevice = std::make_shared<MockDevice>("device");
+ mockDevice->setOutputProducer(combiningOutputComponent);
+ EXPECT_FALSE(mockDevice->pullOutput());
+}
+
+TEST(CombiningOutputComponentTest, DuplicateOutputs)
+{
+ auto combiningOutputComponent = std::make_shared<SurgSim::Input::CombiningOutputComponent>("combiner");
+ auto mockDevice = std::make_shared<MockDevice>("device");
+ mockDevice->setOutputProducer(combiningOutputComponent);
+
+ std::vector<std::shared_ptr<SurgSim::Framework::Component>> outputs;
+ auto output1 = std::make_shared<SurgSim::Input::OutputComponent>("output1");
+ auto output2 = std::make_shared<SurgSim::Input::OutputComponent>("output2");
+ outputs.push_back(output1);
+ outputs.push_back(output2);
+ outputs.push_back(output1);
+ outputs.push_back(output2);
+ outputs.push_back(output2);
+ combiningOutputComponent->setOutputs(outputs);
+
+ auto storedOutputs = combiningOutputComponent->getOutputs();
+ ASSERT_EQ(2, storedOutputs.size());
+ EXPECT_EQ(output1, storedOutputs[0]);
+ EXPECT_EQ(output2, storedOutputs[1]);
+}
+
+TEST(CombiningOutputComponentTest, EmptyOutputs)
+{
+ auto combiningOutputComponent = std::make_shared<SurgSim::Input::CombiningOutputComponent>("combiner");
+ auto mockDevice = std::make_shared<MockDevice>("device");
+ mockDevice->setOutputProducer(combiningOutputComponent);
+
+ std::vector<std::shared_ptr<SurgSim::Framework::Component>> outputs;
+ outputs.push_back(std::make_shared<SurgSim::Input::OutputComponent>("output1"));
+ outputs.push_back(std::make_shared<SurgSim::Input::OutputComponent>("output2"));
+ combiningOutputComponent->setOutputs(outputs);
+
+ ASSERT_EQ(outputs, combiningOutputComponent->getOutputs());
+ EXPECT_FALSE(mockDevice->pullOutput());
+}
+
+TEST(CombiningOutputComponentTest, OneNonEmptyOutput)
+{
+ auto combiningOutputComponent = std::make_shared<SurgSim::Input::CombiningOutputComponent>("combiner");
+ auto mockDevice = std::make_shared<MockDevice>("device");
+ mockDevice->setOutputProducer(combiningOutputComponent);
+
+ std::vector<std::shared_ptr<SurgSim::Framework::Component>> outputs;
+ auto output = std::make_shared<SurgSim::Input::OutputComponent>("output1");
+ outputs.push_back(output);
+ combiningOutputComponent->setOutputs(outputs);
+
+ SurgSim::DataStructures::DataGroupBuilder builder;
+ builder.addVector(SurgSim::DataStructures::Names::FORCE);
+ builder.addBoolean("extraData");
+
+ SurgSim::DataStructures::DataGroup data = builder.createData();
+ auto initialForce = SurgSim::Math::Vector3d(0.89, 0.0, -324.67);
+ data.vectors().set(SurgSim::DataStructures::Names::FORCE, initialForce);
+ data.booleans().set("extraData", true);
+
+ output->setData(data);
+
+ ASSERT_TRUE(mockDevice->pullOutput());
+ SurgSim::DataStructures::DataGroup actualData = mockDevice->getOutputData();
+ ASSERT_FALSE(actualData.isEmpty());
+
+ SurgSim::Math::Vector3d actualForce;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::FORCE, &actualForce));
+ EXPECT_TRUE(actualForce.isApprox(initialForce));
+ EXPECT_FALSE(actualData.booleans().hasEntry("extraData"));
+
+ EXPECT_ANY_THROW(combiningOutputComponent->setData(data));
+}
+
+TEST(CombiningOutputComponentTest, SetCombiner)
+{
+ auto combiningOutputComponent = std::make_shared<SurgSim::Input::CombiningOutputComponent>("combiner");
+ combiningOutputComponent->setCombiner(DO_NOTHING_FUNCTOR);
+ auto mockDevice = std::make_shared<MockDevice>("device");
+ mockDevice->setOutputProducer(combiningOutputComponent);
+
+ std::vector<std::shared_ptr<SurgSim::Framework::Component>> outputs;
+ auto output1 = std::make_shared<SurgSim::Input::OutputComponent>("output1");
+ outputs.push_back(output1);
+ auto output2 = std::make_shared<SurgSim::Input::OutputComponent>("output2");
+ outputs.push_back(output2);
+ auto output3 = std::make_shared<SurgSim::Input::OutputComponent>("output3");
+ outputs.push_back(output3);
+ combiningOutputComponent->setOutputs(outputs);
+
+ SurgSim::DataStructures::DataGroupBuilder builder;
+ builder.addVector(SurgSim::DataStructures::Names::FORCE);
+
+ SurgSim::DataStructures::DataGroup data = builder.createData();
+ auto initialForce = SurgSim::Math::Vector3d(0.89, 0.0, -324.67);
+ data.vectors().set(SurgSim::DataStructures::Names::FORCE, initialForce);
+
+ output1->setData(data);
+ output3->setData(data);
+
+ EXPECT_FALSE(mockDevice->pullOutput());
+}
+
+
+TEST(CombiningOutputComponentTest, MultipleOutputs)
+{
+ auto combiningOutputComponent = std::make_shared<SurgSim::Input::CombiningOutputComponent>("combiner");
+ auto mockDevice = std::make_shared<MockDevice>("device");
+ mockDevice->setOutputProducer(combiningOutputComponent);
+
+ std::vector<std::shared_ptr<SurgSim::Framework::Component>> outputs;
+ auto output1 = std::make_shared<SurgSim::Input::OutputComponent>("output1");
+ outputs.push_back(output1);
+ auto output2 = std::make_shared<SurgSim::Input::OutputComponent>("output2");
+ outputs.push_back(output2);
+ auto output3 = std::make_shared<SurgSim::Input::OutputComponent>("output3");
+ outputs.push_back(output3);
+ combiningOutputComponent->setOutputs(outputs);
+
+ SurgSim::DataStructures::DataGroupBuilder builder;
+ builder.addVector(SurgSim::DataStructures::Names::FORCE);
+
+ SurgSim::DataStructures::DataGroup data = builder.createData();
+ auto initialForce = SurgSim::Math::Vector3d(0.89, 0.0, -324.67);
+ data.vectors().set(SurgSim::DataStructures::Names::FORCE, initialForce);
+
+ output1->setData(data);
+ output3->setData(data);
+
+ ASSERT_TRUE(mockDevice->pullOutput());
+ SurgSim::DataStructures::DataGroup actualData = mockDevice->getOutputData();
+ ASSERT_FALSE(actualData.isEmpty());
+
+ SurgSim::Math::Vector3d actualForce;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::FORCE, &actualForce));
+ EXPECT_TRUE(actualForce.isApprox(2.0 * initialForce));
+
+ // Check subsequent calls to pullOutput will correctly handle non-asserting DataGroup assignment.
+ EXPECT_NO_THROW(mockDevice->pullOutput());
+
+ // Check first OutputComponent going away.
+ outputs.clear();
+ output1.reset();
+ EXPECT_TRUE(mockDevice->pullOutput());
+ EXPECT_EQ(2, combiningOutputComponent->getOutputs().size());
+}
+
+
+TEST(CombiningOutputComponentTest, Serialization)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ auto inputManager = std::make_shared<SurgSim::Input::InputManager>();
+ runtime->addManager(inputManager);
+ runtime->addSceneElements("CombiningOutputComponent.yaml");
+
+ auto element = runtime->getScene()->getSceneElement("element");
+ ASSERT_NE(element, nullptr);
+ auto outputs = element->getComponents<SurgSim::Input::OutputComponent>();
+ ASSERT_EQ(outputs.size(), 4);
+ std::shared_ptr<SurgSim::Input::CombiningOutputComponent> combiningOutputComponent;
+ for (size_t i = 0; i < outputs.size(); ++i)
+ {
+ combiningOutputComponent = std::dynamic_pointer_cast<SurgSim::Input::CombiningOutputComponent>(outputs[i]);
+ if (combiningOutputComponent != nullptr)
+ {
+ outputs.erase(outputs.begin() + i);
+ break;
+ }
+ }
+ ASSERT_NE(combiningOutputComponent, nullptr);
+
+ std::set<std::shared_ptr<SurgSim::Framework::Component>> componentsFromElement;
+ componentsFromElement.insert(outputs.begin(), outputs.end());
+
+ std::set<std::shared_ptr<SurgSim::Framework::Component>> componentsFromCombiner;
+ auto actualOutputs = combiningOutputComponent->getOutputs();
+ componentsFromCombiner.insert(actualOutputs.begin(), actualOutputs.end());
+ ASSERT_EQ(componentsFromElement, componentsFromCombiner);
+ EXPECT_EQ("output1", actualOutputs[0]->getName());
+ EXPECT_EQ("output2", actualOutputs[1]->getName());
+ EXPECT_EQ("output3", actualOutputs[2]->getName());
+
+ auto mockDevice = std::make_shared<MockDevice>("device");
+ inputManager->addDevice(mockDevice);
+
+ SurgSim::DataStructures::DataGroupBuilder builder;
+ builder.addVector(SurgSim::DataStructures::Names::FORCE);
+
+ SurgSim::DataStructures::DataGroup data = builder.createData();
+ auto initialForce = SurgSim::Math::Vector3d(0.89, 0.0, -324.67);
+ data.vectors().set(SurgSim::DataStructures::Names::FORCE, initialForce);
+
+ std::static_pointer_cast<SurgSim::Input::OutputComponent>(actualOutputs[0])->setData(data);
+ std::static_pointer_cast<SurgSim::Input::OutputComponent>(actualOutputs[2])->setData(data);
+
+ runtime->start(true);
+ runtime->step();
+ runtime->step();
+ runtime->step();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(50));
+ runtime->stop();
+
+ ASSERT_TRUE(mockDevice->pullOutput());
+ SurgSim::DataStructures::DataGroup actualData = mockDevice->getOutputData();
+ ASSERT_FALSE(actualData.isEmpty());
+ SurgSim::Math::Vector3d actualForce;
+ ASSERT_TRUE(actualData.vectors().get(SurgSim::DataStructures::Names::FORCE, &actualForce));
+ EXPECT_TRUE(actualForce.isApprox(2.0 * initialForce));
+
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*combiningOutputComponent));
+ EXPECT_TRUE(node.IsMap());
+ std::shared_ptr<SurgSim::Input::CombiningOutputComponent> newComponent;
+ EXPECT_NO_THROW(newComponent = std::dynamic_pointer_cast<SurgSim::Input::CombiningOutputComponent>(
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+ ASSERT_NE(newComponent, nullptr);
+ auto newOutputs = newComponent->getValue<std::vector<std::shared_ptr<SurgSim::Framework::Component>>>("Outputs");
+ for (auto newOutput : newOutputs)
+ {
+ EXPECT_NE(std::dynamic_pointer_cast<SurgSim::Input::OutputComponent>(newOutput), nullptr);
+ }
+}
diff --git a/SurgSim/Input/UnitTests/CommonDeviceTests.cpp b/SurgSim/Input/UnitTests/CommonDeviceTests.cpp
index ef8679f..a2cbf02 100644
--- a/SurgSim/Input/UnitTests/CommonDeviceTests.cpp
+++ b/SurgSim/Input/UnitTests/CommonDeviceTests.cpp
@@ -204,6 +204,12 @@ TEST(CommonDeviceTests, RemoveInputConsumer)
EXPECT_EQ(3, consumer2->m_numTimesReceivedInput);
EXPECT_FALSE(consumer1->m_lastReceivedInput.strings().hasData("helloWorld"));
EXPECT_TRUE(consumer2->m_lastReceivedInput.strings().hasData("helloWorld"));
+
+ device.clearInputConsumers();
+ device.pushInput();
+ EXPECT_EQ(1, consumer1->m_numTimesReceivedInput);
+ EXPECT_EQ(3, consumer2->m_numTimesReceivedInput);
+ EXPECT_FALSE(device.removeInputConsumer(consumer2));
}
TEST(CommonDeviceTests, RemoveOutputProducer)
@@ -245,4 +251,9 @@ TEST(CommonDeviceTests, RemoveOutputProducer)
EXPECT_EQ(0, producer1->m_numTimesRequestedOutput);
EXPECT_EQ(2, producer2->m_numTimesRequestedOutput);
EXPECT_FALSE(device.getOutputData().integers().hasData("value"));
+
+ EXPECT_TRUE(device.setOutputProducer(producer1));
+ EXPECT_TRUE(device.hasOutputProducer());
+ device.clearOutputProducer();
+ EXPECT_FALSE(device.hasOutputProducer());
}
diff --git a/SurgSim/Input/UnitTests/Data/CombiningOutputComponent.yaml b/SurgSim/Input/UnitTests/Data/CombiningOutputComponent.yaml
new file mode 100644
index 0000000..9ecc00b
--- /dev/null
+++ b/SurgSim/Input/UnitTests/Data/CombiningOutputComponent.yaml
@@ -0,0 +1,25 @@
+- SurgSim::Framework::BasicSceneElement:
+ Name: element
+ Components:
+ - SurgSim::Input::CombiningOutputComponent:
+ Name: reducer
+ DeviceName: device
+ Outputs:
+ - SurgSim::Input::OutputComponent:
+ Name: output1
+ Id: output1
+ - SurgSim::Input::OutputComponent:
+ Name: output2
+ Id: output2
+ - SurgSim::Input::OutputComponent:
+ Name: output3
+ Id: output3
+ - SurgSim::Input::OutputComponent:
+ Name: output1
+ Id: output1
+ - SurgSim::Input::OutputComponent:
+ Name: output2
+ Id: output2
+ - SurgSim::Input::OutputComponent:
+ Name: output3
+ Id: output3
diff --git a/SurgSim/Input/UnitTests/InputComponentTest.cpp b/SurgSim/Input/UnitTests/InputComponentTest.cpp
index 2da5b97..75a181c 100644
--- a/SurgSim/Input/UnitTests/InputComponentTest.cpp
+++ b/SurgSim/Input/UnitTests/InputComponentTest.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -17,16 +17,18 @@
* Tests for the InputComponent class.
*/
+#include <gtest/gtest.h>
#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/DataStructures/DataGroup.h"
#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Input/InputComponent.h"
+#include "SurgSim/Input/UnitTests/TestDevice.h"
using SurgSim::Input::InputComponent;
-using SurgSim::DataStructures::DataGroup;
+
TEST(InputComponentTest, CanConstruct)
{
@@ -41,13 +43,17 @@ TEST(InputComponentTest, Accessors)
EXPECT_EQ("InputDevice", input.getDeviceName());
}
-TEST(InputComponentTest, NotConnected)
+TEST(InputComponentTest, GetData)
{
- InputComponent input("Input");
- input.setDeviceName("InputDevice");
- DataGroup dataGroup;
- EXPECT_THROW(input.getData(&dataGroup), SurgSim::Framework::AssertionFailure);
- EXPECT_FALSE(input.isDeviceConnected());
+ auto input = std::make_shared<InputComponent>("Input");
+ SurgSim::DataStructures::DataGroup data;
+ ASSERT_NO_THROW(input->getData(&data)) << "Getting data from an unconnected InputComponent should not assert.";
+ EXPECT_TRUE(data.isEmpty()) << "The data from an unconnected InputComponent should be empty.";
+
+ TestDevice device("MyTestDevice");
+ device.addInputConsumer(input);
+ ASSERT_NO_THROW(input->getData(&data));
+ EXPECT_FALSE(data.isEmpty());
}
TEST(InputComponentTest, Serialization)
diff --git a/SurgSim/Input/UnitTests/InputManagerTest.cpp b/SurgSim/Input/UnitTests/InputManagerTest.cpp
index 12b2078..d30cf96 100644
--- a/SurgSim/Input/UnitTests/InputManagerTest.cpp
+++ b/SurgSim/Input/UnitTests/InputManagerTest.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -55,8 +55,14 @@ public:
virtual ~MockComponent() {}
protected:
- virtual bool doInitialize() {return true;}
- virtual bool doWakeUp() {return true;}
+ virtual bool doInitialize()
+ {
+ return true;
+ }
+ virtual bool doWakeUp()
+ {
+ return true;
+ }
};
namespace SurgSim
{
@@ -108,9 +114,11 @@ TEST_F(InputManagerTest, DeviceAddRemove)
std::shared_ptr<DeviceInterface> testDevice3 = std::make_shared<TestDevice>("TestDevice3");
std::shared_ptr<DeviceInterface> testDevice4 = std::make_shared<TestDevice>("TestDevice4");
+ std::shared_ptr<DeviceInterface> testDevice5 = std::make_shared<TestDevice>("");
EXPECT_TRUE(inputManager->addDevice(testDevice3));
EXPECT_TRUE(inputManager->addDevice(testDevice4));
+ EXPECT_FALSE(inputManager->addDevice(testDevice5));
EXPECT_FALSE(inputManager->addDevice(testDevice3));
EXPECT_TRUE(inputManager->removeDevice(testDevice4));
EXPECT_FALSE(inputManager->removeDevice(testDevice4));
@@ -133,7 +141,7 @@ TEST_F(InputManagerTest, InputAddRemove)
EXPECT_TRUE(testDoAddComponent(listener1));
EXPECT_TRUE(testDoAddComponent(listener2));
EXPECT_TRUE(testDoAddComponent(listener3));
- EXPECT_FALSE(testDoAddComponent(notvalid));
+ EXPECT_TRUE(testDoAddComponent(notvalid)); // InputManager will add InputComponents that don't connect to a device.
// Excercise adds and removes
@@ -157,18 +165,17 @@ TEST_F(InputManagerTest, InputfromDevice)
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);
+ 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);
+ EXPECT_TRUE(dataGroup.strings().get("helloWorld", &data));
+ EXPECT_EQ("bvalue", data);
}
TEST_F(InputManagerTest, OutputAddRemove)
@@ -176,18 +183,20 @@ 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");
+ std::shared_ptr<OutputComponent> invalid = std::make_shared<OutputComponent>("Component5");
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_TRUE(testDoAddComponent(output2)); // InputManager will add even if same device already connected.
EXPECT_FALSE(testDoAddComponent(output2));
EXPECT_TRUE(testDoAddComponent(output3));
- EXPECT_FALSE(testDoAddComponent(invalid));
+ EXPECT_TRUE(testDoAddComponent(invalid)); // InputManager will add even if does not connect to a device.
+
EXPECT_TRUE(testDoRemoveComponent(output1));
EXPECT_FALSE(testDoRemoveComponent(output1));
+ EXPECT_TRUE(testDoRemoveComponent(invalid));
}
TEST_F(InputManagerTest, OutputPush)
diff --git a/SurgSim/Input/UnitTests/OutputComponentTest.cpp b/SurgSim/Input/UnitTests/OutputComponentTest.cpp
index a6c90f2..e08425c 100644
--- a/SurgSim/Input/UnitTests/OutputComponentTest.cpp
+++ b/SurgSim/Input/UnitTests/OutputComponentTest.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -14,16 +14,18 @@
// limitations under the License.
/** @file
- * Tests for the InputComponent class.
+ * Tests for the OutputComponent class.
*/
+#include <gtest/gtest.h>
#include <memory>
#include <string>
-#include <gtest/gtest.h>
-#include "SurgSim/Input/OutputComponent.h"
+#include <yaml-cpp/yaml.h>
+
#include "SurgSim/DataStructures/DataGroup.h"
-#include "yaml-cpp/yaml.h"
+#include "SurgSim/DataStructures/DataGroupBuilder.h"
#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Input/OutputComponent.h"
using SurgSim::Input::OutputComponent;
using SurgSim::DataStructures::DataGroup;
@@ -39,15 +41,20 @@ TEST(OutputComponentTest, Accessors)
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());
+ SurgSim::DataStructures::DataGroup actualData;
+ ASSERT_FALSE(output.requestOutput("", &actualData));
+
+ SurgSim::DataStructures::DataGroupBuilder builder;
+ std::string name = "aBool";
+ builder.addBoolean(name);
+ DataGroup dataGroup = builder.createData();
+ dataGroup.booleans().set(name, true);
+ output.setData(dataGroup);
+ bool result = false;
+ ASSERT_TRUE(output.requestOutput("", &actualData));
+ ASSERT_TRUE(actualData.booleans().get(name, &result));
+ EXPECT_TRUE(result);
}
TEST(OutputComponentTest, Serialization)
diff --git a/SurgSim/Input/UnitTests/TestDevice.cpp b/SurgSim/Input/UnitTests/TestDevice.cpp
index 60c65ee..4cc0e67 100644
--- a/SurgSim/Input/UnitTests/TestDevice.cpp
+++ b/SurgSim/Input/UnitTests/TestDevice.cpp
@@ -16,23 +16,32 @@
#include "SurgSim/Input/UnitTests/TestDevice.h"
TestDevice::TestDevice(const std::string& uniqueName) :
- CommonDevice(uniqueName, buildInputData())
+ CommonDevice(uniqueName, buildInputData()),
+ m_initialized(false)
{
-
}
// required by the DeviceInterface API
bool TestDevice::initialize()
{
+ SURGSIM_ASSERT(!isInitialized()) << getName() << " already initialized.";
+ m_initialized = true;
return true;
}
// required by the DeviceInterface API
bool TestDevice::finalize()
{
+ SURGSIM_ASSERT(isInitialized()) << getName() << " is not initialized, cannot finalize.";
+ m_initialized = false;
return true;
}
+bool TestDevice::isInitialized() const
+{
+ return m_initialized;
+}
+
// expose the pushInput method to the world
void TestDevice::pushInput()
{
diff --git a/SurgSim/Input/UnitTests/TestDevice.h b/SurgSim/Input/UnitTests/TestDevice.h
index 644b890..8ea6689 100644
--- a/SurgSim/Input/UnitTests/TestDevice.h
+++ b/SurgSim/Input/UnitTests/TestDevice.h
@@ -34,16 +34,16 @@ class TestDevice : public CommonDevice
public:
explicit TestDevice(const std::string& uniqueName);
- virtual bool initialize();
+ bool initialize() override;
- virtual bool finalize();
+ bool isInitialized() const override;
- virtual void pushInput();
+ void pushInput() override;
// Send some data down the stream
void pushInput(const std::string& data);
- virtual bool pullOutput();
+ bool pullOutput() override;
const DataGroup& getOutputData() const;
@@ -52,6 +52,12 @@ public:
DataGroup buildOutputData();
std::string lastPulledData;
+
+ /// true if initialized and not finalized.
+ bool m_initialized;
+
+private:
+ bool finalize() override;
};
diff --git a/SurgSim/Blocks/UnitTests/config.txt.in b/SurgSim/Input/UnitTests/config.txt.in
similarity index 100%
copy from SurgSim/Blocks/UnitTests/config.txt.in
copy to SurgSim/Input/UnitTests/config.txt.in
diff --git a/SurgSim/Math/Aabb.h b/SurgSim/Math/Aabb.h
index 8f034b4..4b4034e 100644
--- a/SurgSim/Math/Aabb.h
+++ b/SurgSim/Math/Aabb.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -62,6 +62,7 @@ bool doAabbIntersect(const Eigen::AlignedBox<Scalar, Dim>& a,
{
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
@@ -76,9 +77,40 @@ Eigen::AlignedBox<Scalar, Dim> makeAabb(
Eigen::AlignedBox<Scalar, Dim> result(vector0);
result.extend(vector1);
result.extend(vector2);
- return std::move(result);
+ return result;
}
+
+/// Rotate the extrema of the aabb, note that that will extend the size of the box
+/// \tparam Scalar numeric type
+/// \tparam Dim dimension of the space to be used
+/// \param transform The Rigidtransform to use
+/// \param aabb the aabb to transform
+/// \return the transformed aabb
+template <class Scalar, int Dim>
+Eigen::AlignedBox<Scalar, Dim> transformAabb(const Eigen::Transform<Scalar, Dim, Eigen::Isometry>& transform,
+ const Eigen::AlignedBox<Scalar, Dim>& aabb)
+{
+ static std::array<typename Eigen::AlignedBox<Scalar, Dim>::CornerType, 8> corners =
+ {
+ Eigen::AlignedBox<Scalar, Dim>::BottomLeftFloor, Eigen::AlignedBox<Scalar, Dim>::BottomRightFloor,
+ Eigen::AlignedBox<Scalar, Dim>::TopLeftFloor, Eigen::AlignedBox<Scalar, Dim>::TopRightFloor,
+ Eigen::AlignedBox<Scalar, Dim>::BottomLeftCeil, Eigen::AlignedBox<Scalar, Dim>::BottomRightCeil,
+ Eigen::AlignedBox<Scalar, Dim>::TopLeftCeil, Eigen::AlignedBox<Scalar, Dim>::TopRightCeil,
+ };
+ if (aabb.isEmpty())
+ {
+ return aabb;
+ }
+
+ Eigen::AlignedBox<Scalar, Dim> result;
+ std::for_each(corners.cbegin(), corners.cend(),
+ [&result, &aabb, &transform](typename Eigen::AlignedBox<Scalar, Dim>::CornerType c)
+ {
+ result.extend(transform * aabb.corner(c));
+ });
+ return result;
}
}
+}
#endif
diff --git a/SurgSim/Math/BoxShape.cpp b/SurgSim/Math/BoxShape.cpp
index 6fccf1e..4bb8e3b 100644
--- a/SurgSim/Math/BoxShape.cpp
+++ b/SurgSim/Math/BoxShape.cpp
@@ -25,13 +25,14 @@ BoxShape::BoxShape(double sizeX, double sizeY, double sizeZ) :
m_size(Vector3d(sizeX, sizeY, sizeZ))
{
calculateVertices();
+ updateAabb();
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()
+int BoxShape::getType() const
{
return SHAPE_TYPE_BOX;
}
@@ -59,16 +60,19 @@ double BoxShape::getSizeZ() const
void BoxShape::setSizeX(double sizeX)
{
m_size[0] = sizeX;
+ updateAabb();
}
void BoxShape::setSizeY(double sizeY)
{
m_size[1] = sizeY;
+ updateAabb();
}
void BoxShape::setSizeZ(double sizeZ)
{
m_size[2] = sizeZ;
+ updateAabb();
}
double BoxShape::getVolume() const
@@ -89,8 +93,8 @@ SurgSim::Math::Matrix33d BoxShape::getSecondMomentOfVolume() const
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]);
+ sizeSquared[0] + sizeSquared[2],
+ sizeSquared[0] + sizeSquared[1]);
return inertia;
}
@@ -106,20 +110,30 @@ const std::array<Vector3d, 8>& BoxShape::getVertices() const
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)
+ 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();
}
}
+void BoxShape::updateAabb()
+{
+ m_aabb.setEmpty();
+ m_aabb.extend(-(m_size / 2.0));
+ m_aabb.extend(m_size / 2.0);
+}
+
bool BoxShape::isValid() const
{
return (m_size.minCoeff() >= 0);
diff --git a/SurgSim/Math/BoxShape.h b/SurgSim/Math/BoxShape.h
index ca6bb3c..152a01c 100644
--- a/SurgSim/Math/BoxShape.h
+++ b/SurgSim/Math/BoxShape.h
@@ -35,12 +35,12 @@ 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);
+ explicit 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;
+ int getType() const override;
/// Get size of the box
/// \return the size of the box (in m)
@@ -60,16 +60,16 @@ public:
/// Get the volume of the shape
/// \return The volume of the shape (in m-3)
- virtual double getVolume() const override;
+ double getVolume() const override;
/// Get the volumetric center of the shape
/// \return The center of the shape
- virtual Vector3d getCenter() const override;
+ 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;
+ Matrix33d getSecondMomentOfVolume() const override;
/// Function that returns the local vertex location, given an index.
/// \param i The vertex index.
@@ -81,7 +81,7 @@ public:
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;
+ bool isValid() const override;
protected:
// Setters in 'protected' sections are for serialization purpose only.
@@ -102,11 +102,14 @@ private:
/// Function that calculates the box vertices.
void calculateVertices();
+ /// Update the local aabb for this box
+ void updateAabb();
+
/// The box sizes along the 3 axis respectively {X,Y,Z}
Vector3d m_size;
/// The box vertices.
- std::array<Vector3d,8> m_vertices;
+ std::array<Vector3d, 8> m_vertices;
};
}; // Math
diff --git a/SurgSim/Math/CMakeLists.txt b/SurgSim/Math/CMakeLists.txt
index 510e3de..4e046fa 100644
--- a/SurgSim/Math/CMakeLists.txt
+++ b/SurgSim/Math/CMakeLists.txt
@@ -18,10 +18,13 @@
set(SURGSIM_MATH_SOURCES
BoxShape.cpp
CapsuleShape.cpp
+ CardinalSplines.cpp
+ CompoundShape.cpp
CylinderShape.cpp
DoubleSidedPlaneShape.cpp
GaussLegendreQuadrature.cpp
LinearSolveAndInverse.cpp
+ LinearSparseSolveAndInverse.cpp
MathConvert.cpp
MeshShape.cpp
MlcpGaussSeidelSolver.cpp
@@ -40,7 +43,10 @@ set(SURGSIM_MATH_SOURCES
OdeSolverRungeKutta4.cpp
OdeSolverStatic.cpp
OdeState.cpp
+ ParticlesShape.cpp
PlaneShape.cpp
+ SegmentMeshShape.cpp
+ SegmentMeshShapePlyReaderDelegate.cpp
Shape.cpp
SphereShape.cpp
SurfaceMeshShape.cpp
@@ -50,17 +56,30 @@ set(SURGSIM_MATH_HEADERS
Aabb.h
BoxShape.h
CapsuleShape.h
+ CardinalSplines.h
+ CompoundShape.h
+ CubicSolver.h
+ CubicSolver-inl.h
CylinderShape.h
DoubleSidedPlaneShape.h
GaussLegendreQuadrature.h
Geometry.h
+ IntervalArithmetic.h
+ IntervalArithmetic-inl.h
+ KalmanFilter.h
+ KalmanFilter-inl.h
+ LinearMotionArithmetic.h
+ LinearMotionArithmetic-inl.h
LinearSolveAndInverse.h
LinearSolveAndInverse-inl.h
+ LinearSparseSolveAndInverse.h
MathConvert.h
MathConvert-inl.h
Matrix.h
MeshShape.h
MeshShape-inl.h
+ MinMax.h
+ MinMax-inl.h
MlcpConstraintType.h
MlcpConstraintTypeName.h
MlcpGaussSeidelSolver.h
@@ -82,26 +101,43 @@ set(SURGSIM_MATH_HEADERS
OdeSolverRungeKutta4.h
OdeSolverStatic.h
OdeState.h
+ ParticlesShape.h
+ ParticlesShape-inl.h
PlaneShape.h
+ PointTriangleCcdContactCalculation-inl.h
+ Polynomial.h
+ Polynomial-inl.h
+ PolynomialRoots.h
+ PolynomialRoots-inl.h
+ PolynomialValues.h
+ PolynomialValues-inl.h
Quaternion.h
RigidTransform.h
+ Scalar.h
+ Scalar-inl.h
+ SegmentMeshShape.h
+ SegmentMeshShape-inl.h
+ SegmentMeshShapePlyReaderDelegate.h
+ SegmentSegmentCcdContactCalculation-inl.h
Shape.h
Shapes.h
+ SparseMatrix.h
SphereShape.h
SurfaceMeshShape.h
SurfaceMeshShape-inl.h
+ TriangleCapsuleContactCalculation-inl.h
TriangleTriangleContactCalculation-inl.h
TriangleTriangleIntersection-inl.h
Valid.h
Valid-inl.h
Vector.h
)
+surgsim_create_library_header(Math.h "${SURGSIM_MATH_HEADERS}")
surgsim_add_library(
SurgSimMath
"${SURGSIM_MATH_SOURCES}"
"${SURGSIM_MATH_HEADERS}"
- "SurgSim/Math"
)
set(LIBS
@@ -110,10 +146,7 @@ set(LIBS
${YAML_CPP_LIBRARIES}
)
-target_link_libraries(SurgSimMath ${LIBS}
-)
-add_dependencies(SurgSimMath yaml-cpp)
-
+target_link_libraries(SurgSimMath ${LIBS})
if(BUILD_TESTING)
add_subdirectory(UnitTests)
diff --git a/SurgSim/Math/CapsuleShape.cpp b/SurgSim/Math/CapsuleShape.cpp
index 0911cbf..73af8a5 100644
--- a/SurgSim/Math/CapsuleShape.cpp
+++ b/SurgSim/Math/CapsuleShape.cpp
@@ -25,9 +25,10 @@ CapsuleShape::CapsuleShape(double length, double radius) : m_length(length), m_r
{
SURGSIM_ADD_SERIALIZABLE_PROPERTY(CapsuleShape, double, Radius, getRadius, setRadius);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(CapsuleShape, double, Length, getLength, setLength);
+ updateAabb();
}
-int CapsuleShape::getType()
+int CapsuleShape::getType() const
{
return SHAPE_TYPE_CAPSULE;
}
@@ -45,11 +46,13 @@ double CapsuleShape::getRadius() const
void CapsuleShape::setLength(double length)
{
m_length = length;
+ updateAabb();
}
void CapsuleShape::setRadius(double radius)
{
m_radius = radius;
+ updateAabb();
}
double CapsuleShape::getVolume() const
@@ -78,8 +81,8 @@ SurgSim::Math::Vector3d CapsuleShape::bottomCenter() const
SurgSim::Math::Matrix33d CapsuleShape::getSecondMomentOfVolume() const
{
- const double &r = m_radius;
- const double &l = m_length;
+ 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;
@@ -121,5 +124,12 @@ bool CapsuleShape::isValid() const
return (m_length >= 0) && (m_radius >= 0);
}
+void CapsuleShape::updateAabb()
+{
+ m_aabb.setEmpty();
+ m_aabb.extend(Vector3d(-m_radius, -m_length / 2.0 - m_radius, -m_radius));
+ m_aabb.extend(Vector3d(m_radius, m_length / 2.0 + m_radius, m_radius));
+}
+
}; // namespace Math
}; // namespace SurgSim
diff --git a/SurgSim/Math/CapsuleShape.h b/SurgSim/Math/CapsuleShape.h
index 745e3e1..d2e62ed 100644
--- a/SurgSim/Math/CapsuleShape.h
+++ b/SurgSim/Math/CapsuleShape.h
@@ -34,12 +34,12 @@ 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);
+ explicit CapsuleShape(double length = 0.0, double radius = 0.0);
SURGSIM_CLASSNAME(SurgSim::Math::CapsuleShape);
/// \return the type of the shape
- virtual int getType() override;
+ int getType() const override;
/// Get the capsule length (i.e. cylinder length)
/// \return The capsule length (in m)
@@ -51,11 +51,11 @@ public:
/// Get the volume of the shape
/// \return The volume of the shape (in m-3)
- virtual double getVolume() const override;
+ double getVolume() const override;
/// Get the volumetric center of the shape
/// \return The center of the shape
- virtual Vector3d getCenter() const override;
+ 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
@@ -68,10 +68,10 @@ public:
/// 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;
+ Matrix33d getSecondMomentOfVolume() const override;
/// \return True if length and radius are bigger than or equal to 0; Otherwise, false.
- virtual bool isValid() const override;
+ bool isValid() const override;
protected:
// Setters in 'protected' sections are for serialization purpose only.
@@ -83,7 +83,7 @@ protected:
/// Set the capsule radius (i.e. cylinder/spheres radius)
/// \param radius The capsule radius (in m)
void setRadius(double radius);
-
+ void updateAabb();
private:
/// Capsule length
double m_length;
diff --git a/SurgSim/Math/CardinalSplines.cpp b/SurgSim/Math/CardinalSplines.cpp
new file mode 100644
index 0000000..284d416
--- /dev/null
+++ b/SurgSim/Math/CardinalSplines.cpp
@@ -0,0 +1,104 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/CardinalSplines.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+namespace CardinalSplines
+{
+
+void extendControlPoints(const SurgSim::DataStructures::VerticesPlain& points,
+ std::vector<SurgSim::Math::Vector3d>* result)
+{
+ SURGSIM_ASSERT(points.getNumVertices() >= 2) << "Cannot extend control points less than 2.";
+ SURGSIM_ASSERT(result != nullptr) << "'result' can not be nullptr.";
+ result->clear();
+ result->reserve(points.getNumVertices() + 2);
+
+ // Interpolate the 1st point (ghost) as the symmetric of P1 from P0: P-1 = P0 - (P1 - P0)
+ result->push_back(2.0 * points.getVertexPosition(0) - points.getVertexPosition(1));
+ for (size_t i = 0; i < points.getNumVertices(); ++i)
+ {
+ result->push_back(points.getVertexPosition(i));
+ }
+ // Interpolate the last point (ghost) as the symmetric of Pn-1 from Pn: Pn+1 = Pn - (Pn-1 - Pn)
+ result->push_back(2.0 * points.getVertexPosition(points.getNumVertices() - 1) -
+ points.getVertexPosition(points.getNumVertices() - 2));
+}
+
+void interpolate(size_t subdivisions,
+ const std::vector<SurgSim::Math::Vector3d>& controlPoints,
+ std::vector<SurgSim::Math::Vector3d>* points,
+ double tau)
+{
+ SURGSIM_ASSERT(subdivisions >= 0) << "'subdivision' must be at least 0.";
+ SURGSIM_ASSERT(controlPoints.size() >= 4) << "Cannot apply Cardinal Splines interpolation with less than 4 points";
+ SURGSIM_ASSERT(points != nullptr) << "'points' is nullptr";
+ SURGSIM_ASSERT(0 <= tau && tau <= 1) << "Tension parameter 'tau' must be in the range [0,1].";
+
+ size_t numPoints = controlPoints.size();
+
+ // Allow for no subdivisions for debug scenarios.
+ if (subdivisions == 0)
+ {
+ for (size_t pointIndex = 0; pointIndex < numPoints; ++pointIndex)
+ {
+ points->push_back(controlPoints[pointIndex]);
+ }
+ return;
+ }
+
+ double stepsize = 1.0 / static_cast<double>(subdivisions);
+ for (size_t pointIndex = 0; pointIndex < numPoints - 3; ++pointIndex)
+ {
+ std::array<SurgSim::Math::Vector3d, 4> p =
+ {
+ controlPoints[pointIndex],
+ controlPoints[pointIndex + 1],
+ controlPoints[pointIndex + 2],
+ controlPoints[pointIndex + 3]
+ };
+
+ double abscissa = 0.0;
+ /*
+ Note that 'controlPoints' are NOT included in the final result, 'points'.
+ However, when 'abscissa' is 0.0, the interpolated point will happen to have the same value with one of the
+ control points and thus being included in the result.
+ */
+ for (size_t i = 0; i < subdivisions; ++i)
+ {
+ double abcissaSquared = abscissa * abscissa;
+ double abcissaCubed = abcissaSquared * abscissa;
+
+ SurgSim::Math::Vector3d result =
+ p[1] +
+ abscissa * (tau * (p[2] - p[0])) +
+ abcissaSquared * (2.0 * tau * p[0] + (tau - 3.0) * p[1] + (3.0 - 2.0 * tau) * p[2] - tau * p[3]) +
+ abcissaCubed * (-tau * p[0] + (2.0 - tau) * p[1] + (tau - 2.0) * p[2] + tau * p[3]);
+
+ points->push_back(std::move(result));
+
+ abscissa += stepsize;
+ }
+ }
+}
+
+}; // namespace CardinalSplines
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/CardinalSplines.h b/SurgSim/Math/CardinalSplines.h
new file mode 100644
index 0000000..d863091
--- /dev/null
+++ b/SurgSim/Math/CardinalSplines.h
@@ -0,0 +1,58 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_CARDINALSPLINES_H
+#define SURGSIM_MATH_CARDINALSPLINES_H
+
+#include <vector>
+
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Math/Vector.h"
+
+/// \file CardinalSplines.h collects the utilities to do Cardinal Splines interpolation.
+namespace SurgSim
+{
+namespace Math
+{
+namespace CardinalSplines
+{
+
+/// Function to add two 'ghost' points to 'points' at the beginning and the end,
+/// prepare 'points' for Cardinal Splines interpolation.
+/// \param points List of points to be interpolated.
+/// \param[out] result List of points with two ghost points added,
+/// one at the beginning and another at the end of 'points'.
+void extendControlPoints(const SurgSim::DataStructures::VerticesPlain& points,
+ std::vector<SurgSim::Math::Vector3d>* result);
+
+/// Run Cardinal Splines interpolation on 'controlPoints'.
+/// See https://en.wikipedia.org/wiki/Centripetal_Catmull-Rom_spline
+/// https://en.wikipedia.org/wiki/Cubic_Hermite_spline
+/// https://people.cs.clemson.edu/~dhouse/courses/405/notes/splines.pdf
+/// https://www.cs.utexas.edu/~fussell/courses/cs384g/lectures/lecture16-Interpolating_curves.pdf for more details.
+/// \param subdivisions Number of interpolated points between each pair of control points.
+/// \param controlPoints List of points to be interpolated.
+/// \param[out] points List of interpolated points, not including the control points.
+/// \param tau Defines the tension, affects how sharply the curve bends at the control points.
+void interpolate(size_t subdivisions,
+ const std::vector<Math::Vector3d>& controlPoints,
+ std::vector<Math::Vector3d>* points,
+ double tau = 0.4);
+
+}; // namespace CardinalSplines
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_CARDINALSPLINES_H
diff --git a/SurgSim/Math/CompoundShape.cpp b/SurgSim/Math/CompoundShape.cpp
new file mode 100644
index 0000000..6bcbb9c
--- /dev/null
+++ b/SurgSim/Math/CompoundShape.cpp
@@ -0,0 +1,279 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/CompoundShape.h"
+#include "SurgSim/Math/MathConvert.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::CompoundShape, CompoundShape);
+
+CompoundShape::CompoundShape()
+{
+ {
+ typedef std::vector<SubShape> PropertyType;
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SurgSim::Math::CompoundShape, PropertyType, Shapes, getShapes, setShapes);
+ }
+}
+
+CompoundShape::~CompoundShape()
+{
+}
+
+int CompoundShape::getType() const
+{
+ return SHAPE_TYPE_COMPOUNDSHAPE;
+}
+
+double CompoundShape::getVolume() const
+{
+ {
+ ReadLock lock(m_mutex);
+ if (m_volume.hasValue())
+ {
+ return *m_volume;
+ }
+ }
+
+ {
+ WriteLock lock(m_mutex);
+ if (!m_volume.hasValue())
+ {
+ double volume = 0.0;
+
+ for (const auto& shape : m_shapes)
+ {
+ volume += (shape.first)->getVolume();
+ }
+ m_volume = volume;
+ }
+ return *m_volume;
+ }
+}
+
+
+Vector3d CompoundShape::getCenter() const
+{
+ {
+ ReadLock lock(m_mutex);
+ if (m_center.hasValue())
+ {
+ return *m_center;
+ }
+ }
+
+ {
+ WriteLock lock(m_mutex);
+ if (!m_center.hasValue())
+ {
+ Vector3d result = Vector3d::Zero();
+ if (m_shapes.size() > 0)
+ {
+ double total = 0.0;
+ for (const auto& shape : m_shapes)
+ {
+ double volume = shape.first->getVolume();
+ result += shape.second * (shape.first->getCenter()) * volume;
+ total += volume;
+ }
+ result /= total;
+
+ // We have it, write the total volume as well ...
+ m_volume = total;
+ }
+ m_center = result;
+ }
+ return *m_center;
+ }
+}
+
+Matrix33d CompoundShape::getSecondMomentOfVolume() const
+{
+ // Calculate the compound values, this needs to be done outside of the ReadLock, otherwise
+ // this might freeze up
+ auto center = getCenter();
+
+ {
+ ReadLock lock(m_mutex);
+ if (m_secondMoment.hasValue())
+ {
+ return *m_secondMoment;
+ }
+ }
+
+ {
+ WriteLock lock(m_mutex);
+ if (!m_secondMoment.hasValue())
+ {
+ Matrix33d result = Matrix33d::Zero();
+ if (m_shapes.size() > 0)
+ {
+ for (const auto& subShape : m_shapes)
+ {
+ const auto& shape = subShape.first;
+ const auto& pose = subShape.second;
+ const auto& r = pose.linear();
+ Matrix33d skew = makeSkewSymmetricMatrix((center - pose * shape->getCenter()).eval());
+ Matrix33d inertia =
+ r * shape->getSecondMomentOfVolume() * r.transpose() - shape->getVolume() * skew * skew;
+
+ result += inertia;
+ }
+ }
+ m_secondMoment = result;
+ }
+ return *m_secondMoment;
+ }
+}
+
+
+bool CompoundShape::isValid() const
+{
+ return true;
+}
+
+const Math::Aabbd& CompoundShape::getBoundingBox() const
+{
+ {
+ ReadLock lock(m_mutex);
+ if (m_localAabb.hasValue())
+ {
+ return *m_localAabb;
+ }
+ }
+
+ {
+ WriteLock lock(m_mutex);
+ if (!m_localAabb.hasValue())
+ {
+ Math::Aabbd result;
+ for (const auto& subShape : m_shapes)
+ {
+ result.extend(Math::transformAabb(subShape.second, subShape.first->getBoundingBox()));
+ }
+ m_localAabb.setValue(result);
+ }
+ return *m_localAabb;
+ }
+}
+
+void CompoundShape::invalidateData()
+{
+ m_volume.invalidate();
+ m_center.invalidate();
+ m_secondMoment.invalidate();
+ m_localAabb.invalidate();
+}
+
+size_t CompoundShape::addShape(const std::shared_ptr<Shape>& shape, const RigidTransform3d& pose)
+{
+ WriteLock lock(m_mutex);
+ m_shapes.emplace_back(shape, pose);
+ invalidateData();
+ return m_shapes.size() - 1;
+}
+
+void CompoundShape::setShapes(const std::vector<SubShape>& shapes)
+{
+ WriteLock lock(m_mutex);
+ m_shapes = shapes;
+ invalidateData();
+}
+
+const std::vector<CompoundShape::SubShape>& CompoundShape::getShapes() const
+{
+ ReadLock lock(m_mutex);
+ return m_shapes;
+}
+
+const std::shared_ptr<Shape>& CompoundShape::getShape(size_t index) const
+{
+ ReadLock lock(m_mutex);
+ SURGSIM_ASSERT(index < m_shapes.size()) << "Shape index out of range.";
+ return m_shapes[index].first;
+}
+
+RigidTransform3d CompoundShape::getPose(size_t index) const
+{
+ ReadLock lock(m_mutex);
+ SURGSIM_ASSERT(index < m_shapes.size()) << "Shape index out of range.";
+ return m_shapes[index].second;
+}
+
+void CompoundShape::setPoses(const std::vector<RigidTransform3d>& poses)
+{
+ WriteLock lock(m_mutex);
+ SURGSIM_ASSERT(poses.size() == m_shapes.size()) << "New poses and number of shapes differ in size";
+ size_t i = 0;
+ for (auto& shape : m_shapes)
+ {
+ shape.second = poses[i++];
+ }
+ invalidateData();
+}
+
+
+void CompoundShape::setPose(size_t index, const RigidTransform3d& pose)
+{
+ WriteLock(m_mutex);
+ SURGSIM_ASSERT(index < m_shapes.size()) << "Shape index out of range.";
+ m_shapes[index].second = pose;
+ invalidateData();
+}
+
+size_t CompoundShape::getNumShapes() const
+{
+ ReadLock lock(m_mutex);
+ return m_shapes.size();
+}
+
+void CompoundShape::clearShapes()
+{
+ WriteLock lock(m_mutex);
+ m_shapes.clear();
+ invalidateData();
+}
+
+bool CompoundShape::isTransformable() const
+{
+ return true;
+}
+
+std::shared_ptr<Shape> CompoundShape::getTransformed(const RigidTransform3d& pose) const
+{
+ auto transformed = std::make_shared<CompoundShape>();
+ for (const auto& shape : m_shapes)
+ {
+ std::shared_ptr<Shape> newShape;
+ RigidTransform3d newPose = pose * shape.second;
+ if (shape.first->isTransformable())
+ {
+ newShape = shape.first->getTransformed(newPose);
+ }
+ else
+ {
+ newShape = shape.first;
+ }
+ transformed->addShape(newShape, newPose);
+ }
+ return transformed;
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Math/CompoundShape.h b/SurgSim/Math/CompoundShape.h
new file mode 100644
index 0000000..d3723d3
--- /dev/null
+++ b/SurgSim/Math/CompoundShape.h
@@ -0,0 +1,135 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_COMPOUNDSHAPE_H
+#define SURGSIM_MATH_COMPOUNDSHAPE_H
+
+#include <memory>
+#include <utility>
+
+#include <boost/thread.hpp>
+
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/DataStructures/OptionalValue.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+SURGSIM_STATIC_REGISTRATION(CompoundShape);
+
+class CompoundShape : public Shape
+{
+public:
+ /// Constructor
+ CompoundShape();
+
+ /// Destructor
+ ~CompoundShape();
+
+ SURGSIM_CLASSNAME(SurgSim::Math::CompoundShape);
+
+ typedef std::pair<std::shared_ptr<Shape>, RigidTransform3d> SubShape;
+
+ /// Add a shape to this shape, you can optionally supply a pose for the added shape
+ /// \param shape to be added
+ /// \param pose for the newly added shape
+ /// \return the index of the newly added shape
+ size_t addShape(const std::shared_ptr<Shape>& shape, const RigidTransform3d& pose = RigidTransform3d::Identity());
+
+ /// Sets the shapes for this object, the shapes should be a list of shapes together with their respective poses,
+ /// this will clear all the previously contained shapes
+ /// \param shapes list of shapes
+ void setShapes(const std::vector<SubShape>& shapes);
+
+ /// \return all the contained shapes and their respective poses
+ const std::vector<SubShape>& getShapes() const;
+
+ /// \return a specific shape
+ /// \throws SurgSim::AssertionFailure if the index exceeds the current number of shapes
+ const std::shared_ptr<Shape>& getShape(size_t index) const;
+
+ /// \return the pose of a specific shape
+ /// \throws SurgSim::AssertionFailure if the index exceeds the current number of shapes
+ RigidTransform3d getPose(size_t index) const;
+
+ /// Sets the poses for all shapes
+ /// \param poses array of poses to be copied to each shape
+ /// \throws SurgSimm::AssertialFailure if the size of poses.size() != getNumShapes()
+ void setPoses(const std::vector<RigidTransform3d>& poses);
+
+ /// Set the pose for the specified shape
+ /// \param index index of the target shape
+ /// \param pose new pose for the indicated shape
+ /// \throws SurgSim::AssertionFailure if the index exceeds the current number of shapes
+ void setPose(size_t index, const RigidTransform3d& pose);
+
+ /// \return the number of shapes in this shape
+ size_t getNumShapes() const;
+
+ /// clears all the enclosed shapes
+ void clearShapes();
+
+ int getType() const override;
+
+ /// \return the volume of the shape
+ /// \note that currently this is a very simple sum of all the volumes of the enclosed shapes it will
+ /// disregard any overlapping shapes.
+ double getVolume() const override;
+
+ Vector3d getCenter() const override;
+
+ Matrix33d getSecondMomentOfVolume() const override;
+
+ bool isValid() const override;
+
+ const Math::Aabbd& getBoundingBox() const override;
+
+ bool isTransformable() const override;
+
+ std::shared_ptr<Shape> getTransformed(const RigidTransform3d& pose) const override;
+
+private:
+
+ /// Clears the data for the volume, center and secondMoment and aabb
+ /// so it can be recalculated when needed again
+ void invalidateData();
+
+ std::vector<SubShape> m_shapes;
+
+ typedef boost::shared_lock<boost::shared_mutex> ReadLock;
+ typedef boost::unique_lock<boost::shared_mutex> WriteLock;
+
+ mutable boost::shared_mutex m_mutex;
+
+ ///@{
+ /// Storage for the physical properties of this shape
+ /// mutable so they can be recalculated inside the
+ /// const functions getVolume(), getCenter(), getSecondMommentOfVolume()
+ mutable DataStructures::OptionalValue<Vector3d> m_center;
+ mutable DataStructures::OptionalValue<double> m_volume;
+ mutable DataStructures::OptionalValue<Matrix33d> m_secondMoment;
+ mutable DataStructures::OptionalValue<Math::Aabbd> m_localAabb;
+ ///@}
+
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Math/CubicSolver-inl.h b/SurgSim/Math/CubicSolver-inl.h
new file mode 100644
index 0000000..5cdad2c
--- /dev/null
+++ b/SurgSim/Math/CubicSolver-inl.h
@@ -0,0 +1,132 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_CUBICSOLVER_INL_H
+#define SURGSIM_MATH_CUBICSOLVER_INL_H
+
+#include <boost/math/tools/roots.hpp>
+#include <limits>
+#include <vector>
+
+#include "SurgSim/Math/IntervalArithmetic.h"
+#include "SurgSim/Math/PolynomialRoots.h"
+
+using boost::math::tools::bisect;
+
+
+namespace SurgSim
+{
+namespace Math
+{
+
+template <class T>
+int findRootsInRange01(const Polynomial<T, 3>& p, std::array<T, 3>* roots)
+{
+ int numberOfRoots = 0;
+ boost::math::tools::eps_tolerance<T> tolerance(std::numeric_limits<T>::digits - 3);
+ const T epsilon = 4 * std::numeric_limits<T>::epsilon();
+
+ // Is degenerate?
+ if (isNearZero(p.getCoefficient(3), epsilon))
+ {
+ Polynomial<T, 2> quadratic(p.getCoefficient(0), p.getCoefficient(1), p.getCoefficient(2));
+ PolynomialRoots<T, 2> quadraticRoots(quadratic, std::numeric_limits<T>::epsilon());
+
+ for (int i = 0; i < quadraticRoots.getNumRoots(); ++i)
+ {
+ if (quadraticRoots[i] >= 0.0 && quadraticRoots[i] <= 1.0)
+ {
+ (*roots)[numberOfRoots++] = quadraticRoots[i];
+ }
+ }
+
+ return numberOfRoots;
+ }
+
+ PolynomialRoots<T, 2> stationaryPoints(p.derivative(), std::numeric_limits<T>::epsilon());
+ if (stationaryPoints.getNumRoots() < 2 ||
+ !Interval<T>(0, 1).overlapsWith(Interval<T>(stationaryPoints[0], stationaryPoints[1])))
+ {
+ T p0 = p.getCoefficient(0); // p.evaluate(static_cast<T>(0));
+ if (isNearZero(p0, epsilon))
+ {
+ (*roots)[0] = 0.0;
+ return 1;
+ }
+
+ T p1 = p.evaluate(static_cast<T>(1));
+ if (isNearZero(p1, epsilon))
+ {
+ (*roots)[0] = static_cast<T>(1);
+ return 1;
+ }
+ if (p0 * p1 < 0)
+ {
+ auto bracket = bisect(p, static_cast<T>(0), static_cast<T>(1), tolerance);
+ (*roots)[0] = (bracket.first + bracket.second) * 0.5;
+ return 1;
+ }
+
+ }
+ else
+ {
+ // Build the monotonic intervals partitioning [0..1] to be analyzed one by one
+ // #performance HS-2016-feb-17 Test with boost::static_vector as this gets used by the CCD
+ std::vector<Interval<T>> intervals;
+
+ T lastValue = static_cast<T>(0);
+ if (stationaryPoints[0] > static_cast<T>(0))
+ {
+ intervals.emplace_back(lastValue, stationaryPoints[0]);
+ lastValue = stationaryPoints[0];
+ }
+ if (stationaryPoints[1] < static_cast<T>(1))
+ {
+ intervals.emplace_back(lastValue, stationaryPoints[1]);
+ lastValue = stationaryPoints[1];
+ }
+ intervals.emplace_back(lastValue, static_cast<T>(1));
+
+ for (auto interval : intervals)
+ {
+ // On each interval, only 1 root can be found
+ T pMin = p.evaluate(interval.getMin());
+ if (isNearZero(pMin, epsilon))
+ {
+ (*roots)[numberOfRoots++] = interval.getMin();
+ }
+ else
+ {
+ T pMax = p.evaluate(interval.getMax());
+ if (isNearZero(pMax, epsilon))
+ {
+ (*roots)[numberOfRoots++] = interval.getMax();
+ }
+ else if (pMin * pMax < 0)
+ {
+ auto bracket = bisect(p, interval.getMin(), interval.getMax(), tolerance);
+ (*roots)[numberOfRoots++] = (bracket.first + bracket.second) * 0.5;
+ }
+ }
+ }
+ }
+
+ return numberOfRoots;
+}
+
+}; // Math
+}; // SurgSim
+
+#endif // SURGSIM_MATH_CUBICSOLVER_INL_H
diff --git a/SurgSim/Math/CubicSolver.h b/SurgSim/Math/CubicSolver.h
new file mode 100644
index 0000000..aaa5905
--- /dev/null
+++ b/SurgSim/Math/CubicSolver.h
@@ -0,0 +1,54 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_CUBICSOLVER_H
+#define SURGSIM_MATH_CUBICSOLVER_H
+
+#include "SurgSim/Math/Polynomial.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// Find all roots in range \f$[0 \ldotp\ldotp 1]\f$ of a cubic equation
+/// \tparam T The equation coefficient type
+/// \param polynomial The cubic polynomial \f$ax^3 + bx^2 + cx + d\f$
+/// \param[out] roots All roots ordered ascendingly in \f$[0 \ldotp\ldotp 1]\f$ if any (3 max)
+/// \return The number of roots found in \f$[0 \ldotp\ldotp 1]\f$ and saved in roots.
+/// \f[
+/// \begin{array}{lll}
+/// P(x) &=& ax^3 + bx^2 + cx + d \\ \text{}
+/// P'(x) &=& 3ax^2 + 2bx + c \Rightarrow \Delta = (2b)^2 - 4(3a)(c) = 4(b^2 - 3ac)
+/// \end{array}
+/// \\ \text{}
+/// \left\{
+/// \begin{array}{ll}
+/// \Delta < 0 & \text{P is monotonic, P' is always the same sign, the sign of P'(0) = sign(c)} \\ \text{}
+/// \Delta = 0 & \text{P is monotonic with an inflection point, P' is always the same sign, except at P'(root) = 0}
+/// \\ \text{}
+/// \Delta > 0 & \text{P is monotonic on 3 separate intervals}
+/// \end{array}
+/// \right.
+/// \f]
+template <class T>
+int findRootsInRange01(const Polynomial<T, 3>& polynomial, std::array<T, 3>* roots);
+
+}; // Math
+}; // SurgSim
+
+#include "SurgSim/Math/CubicSolver-inl.h"
+
+#endif // SURGSIM_MATH_CUBICSOLVER_H
diff --git a/SurgSim/Math/CylinderShape.cpp b/SurgSim/Math/CylinderShape.cpp
index 056a15f..5a136c5 100644
--- a/SurgSim/Math/CylinderShape.cpp
+++ b/SurgSim/Math/CylinderShape.cpp
@@ -25,9 +25,10 @@ CylinderShape::CylinderShape(double length, double radius) : m_length(length), m
{
SURGSIM_ADD_SERIALIZABLE_PROPERTY(CylinderShape, double, Radius, getRadius, setRadius);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(CylinderShape, double, Length, getLength, setLength);
+ updateAabb();
}
-int CylinderShape::getType()
+int CylinderShape::getType() const
{
return SHAPE_TYPE_CYLINDER;
}
@@ -46,11 +47,20 @@ double CylinderShape::getRadius() const
void CylinderShape::setLength(double length)
{
m_length = length;
+ updateAabb();
}
void CylinderShape::setRadius(double radius)
{
m_radius = radius;
+ updateAabb();
+}
+
+void CylinderShape::updateAabb()
+{
+ m_aabb.setEmpty();
+ m_aabb.extend(Vector3d(-m_radius, -m_length / 2.0, -m_radius));
+ m_aabb.extend(Vector3d(m_radius, m_length / 2.0, m_radius));
}
double CylinderShape::getVolume() const
diff --git a/SurgSim/Math/CylinderShape.h b/SurgSim/Math/CylinderShape.h
index 296b130..e919035 100644
--- a/SurgSim/Math/CylinderShape.h
+++ b/SurgSim/Math/CylinderShape.h
@@ -34,12 +34,12 @@ 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);
+ explicit CylinderShape(double length = 0.0, double radius = 0.0);
SURGSIM_CLASSNAME(SurgSim::Math::CylinderShape);
/// \return the type of the shape
- virtual int getType() override;
+ int getType() const override;
/// Get the cylinder length
/// \return The cylinder length (in m)
@@ -51,19 +51,19 @@ public:
/// Get the volume of the shape
/// \return The volume of the shape (in m-3)
- virtual double getVolume() const override;
+ double getVolume() const override;
/// Get the volumetric center of the shape
/// \return The center of the shape
- virtual Vector3d getCenter() const override;
+ 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;
+ Matrix33d getSecondMomentOfVolume() const override;
/// \return True if length and radius are bigger than or equal to 0; Otherwise, false.
- virtual bool isValid() const override;
+ bool isValid() const override;
protected:
// Setters in 'protected' sections are for serialization purpose only.
@@ -77,6 +77,9 @@ protected:
void setRadius(double radius);
private:
+
+ void updateAabb();
+
/// The cylinder length
double m_length;
diff --git a/SurgSim/Math/DoubleSidedPlaneShape.cpp b/SurgSim/Math/DoubleSidedPlaneShape.cpp
index 1e75520..3f4b603 100644
--- a/SurgSim/Math/DoubleSidedPlaneShape.cpp
+++ b/SurgSim/Math/DoubleSidedPlaneShape.cpp
@@ -25,7 +25,7 @@ DoubleSidedPlaneShape::DoubleSidedPlaneShape()
{
}
-int DoubleSidedPlaneShape::getType()
+int DoubleSidedPlaneShape::getType() const
{
return SHAPE_TYPE_DOUBLESIDEDPLANE;
}
diff --git a/SurgSim/Math/DoubleSidedPlaneShape.h b/SurgSim/Math/DoubleSidedPlaneShape.h
index fde853d..cf6d576 100644
--- a/SurgSim/Math/DoubleSidedPlaneShape.h
+++ b/SurgSim/Math/DoubleSidedPlaneShape.h
@@ -37,20 +37,20 @@ public:
SURGSIM_CLASSNAME(SurgSim::Math::DoubleSidedPlaneShape);
/// \return the type of the shape
- virtual int getType() override;
+ int getType() const override;
/// Get the volume of the shape
/// \return The volume of the shape (in m-3)
- virtual double getVolume() const override;
+ double getVolume() const override;
/// Get the volumetric center of the shape
/// \return The center of the shape
- virtual Vector3d getCenter() const override;
+ 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;
+ Matrix33d getSecondMomentOfVolume() const override;
/// Gets the d of the plane equation.
/// \return The value of d (always 0).
@@ -62,7 +62,7 @@ public:
/// A DoubleSidedPlaneShape is always valid.
/// \return True.
- virtual bool isValid() const override;
+ bool isValid() const override;
};
}; // Math
diff --git a/SurgSim/Math/GaussLegendreQuadrature.cpp b/SurgSim/Math/GaussLegendreQuadrature.cpp
index 1c05251..36539ad 100644
--- a/SurgSim/Math/GaussLegendreQuadrature.cpp
+++ b/SurgSim/Math/GaussLegendreQuadrature.cpp
@@ -22,25 +22,25 @@ namespace SurgSim
namespace Math
{
-std::array<gaussQuadraturePoint, 1> gaussQuadrature1Point =
+const std::array<gaussQuadraturePoint, 1> gaussQuadrature1Point =
{{
gaussQuadraturePoint(0.0, 2.0)
}};
-std::array<gaussQuadraturePoint, 2> gaussQuadrature2Points =
+const 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 =
+const 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 =
+const 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),
@@ -48,7 +48,7 @@ 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)
}};
-std::array<gaussQuadraturePoint, 5> gaussQuadrature5Points =
+const 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),
@@ -57,7 +57,7 @@ std::array<gaussQuadraturePoint, 5> gaussQuadrature5Points =
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 =
+const std::array<gaussQuadraturePoint, 100> gaussQuadrature100Points =
{{
gaussQuadraturePoint( 0.0156289844215430828722167, 0.0312554234538633569476425),
gaussQuadraturePoint(-0.0156289844215430828722167, 0.0312554234538633569476425),
@@ -161,5 +161,38 @@ std::array<gaussQuadraturePoint, 100> gaussQuadrature100Points =
gaussQuadraturePoint(-0.9997137267734412336782285, 0.0007346344905056717304063)
}};
+const std::array<gaussQuadratureTrianglePoint, 3> gaussQuadrature2DTriangle3Points =
+{{
+ gaussQuadratureTrianglePoint(1.0 / 2.0, 1.0 / 2.0, 1.0 / 3.0),
+ gaussQuadratureTrianglePoint(1.0 / 2.0, 0.0, 1.0 / 3.0),
+ gaussQuadratureTrianglePoint(0.0 , 1.0 / 2.0, 1.0 / 3.0)
+}};
+
+const std::array<gaussQuadratureTrianglePoint, 6> gaussQuadrature2DTriangle6Points =
+{{
+ gaussQuadratureTrianglePoint(0.44594849091597, 0.44594849091597, 0.22338158967801),
+ gaussQuadratureTrianglePoint(0.44594849091597, 0.10810301816807, 0.22338158967801),
+ gaussQuadratureTrianglePoint(0.10810301816807, 0.44594849091597, 0.22338158967801),
+ gaussQuadratureTrianglePoint(0.09157621350977, 0.09157621350977, 0.10995174365532),
+ gaussQuadratureTrianglePoint(0.09157621350977, 0.81684757298046, 0.10995174365532),
+ gaussQuadratureTrianglePoint(0.81684757298046, 0.09157621350977, 0.10995174365532)
+}};
+
+const std::array<gaussQuadratureTrianglePoint, 12> gaussQuadrature2DTriangle12Points =
+{{
+ gaussQuadratureTrianglePoint(0.24928674517091, 0.24928674517091, 0.11678627572638),
+ gaussQuadratureTrianglePoint(0.24928674517091, 0.50142650965818, 0.11678627572638),
+ gaussQuadratureTrianglePoint(0.50142650965818, 0.24928674517091, 0.11678627572638),
+ gaussQuadratureTrianglePoint(0.06308901449150, 0.06308901449150, 0.05084490637021),
+ gaussQuadratureTrianglePoint(0.06308901449150, 0.87382197101700, 0.05084490637021),
+ gaussQuadratureTrianglePoint(0.87382197101700, 0.06308901449150, 0.05084490637021),
+ gaussQuadratureTrianglePoint(0.31035245103378, 0.63650249912140, 0.08285107561837),
+ gaussQuadratureTrianglePoint(0.63650249912140, 0.05314504984482, 0.08285107561837),
+ gaussQuadratureTrianglePoint(0.05314504984482, 0.31035245103378, 0.08285107561837),
+ gaussQuadratureTrianglePoint(0.63650249912140, 0.31035245103378, 0.08285107561837),
+ gaussQuadratureTrianglePoint(0.31035245103378, 0.05314504984482, 0.08285107561837),
+ gaussQuadratureTrianglePoint(0.05314504984482, 0.63650249912140, 0.08285107561837)
+}};
+
}; // namespace Math
}; // namespace SurgSim
diff --git a/SurgSim/Math/GaussLegendreQuadrature.h b/SurgSim/Math/GaussLegendreQuadrature.h
index 2199316..24cb8ad 100644
--- a/SurgSim/Math/GaussLegendreQuadrature.h
+++ b/SurgSim/Math/GaussLegendreQuadrature.h
@@ -28,6 +28,7 @@ namespace SurgSim
namespace Math
{
+/// 1D Gauss-Legendre quadrature
struct gaussQuadraturePoint
{
gaussQuadraturePoint(double p, double w) : point(p), weight(w){}
@@ -36,59 +37,105 @@ struct gaussQuadraturePoint
const double weight;
};
-/// 1-point Gauss-Legendre quadrature {<x_1, w_1>}
+/// 2D Gauss-Legendre quadrature on a triangle
+/// \note In a triangle ABC, a point \f$P\f$ is defined by its parametrized coordinate \f$(\xi, \eta)\f$ as
+/// \note \f$P = A + \xi.AB + \eta.AC\f$
+struct gaussQuadratureTrianglePoint
+{
+ gaussQuadratureTrianglePoint(double xi, double eta, double w) : coordinateXi(xi), coordinateEta(eta), weight(w){}
+
+ const double coordinateXi; ///< \f$\xi \in [0, 1]\f$, must verify \f$\xi + \eta \leq 1.0\f$
+ const double coordinateEta; ///< \f$\eta \in [0, 1]\f$, must verify \f$\xi + \eta \leq 1.0\f$
+ const double weight;
+};
+
+/// 1D 1-point Gauss-Legendre quadrature \f${<x_1, w_1>}\f$
/// \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;
+extern const std::array<gaussQuadraturePoint, 1> gaussQuadrature1Point;
-/// 2-points Gauss-Legendre quadrature {<x_1, w_1>, <x_2, w_2>}
+/// 1D 2-points Gauss-Legendre quadrature \f${<x_1, w_1>, <x_2, w_2>}\f$
/// \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;
+extern const std::array<gaussQuadraturePoint, 2> gaussQuadrature2Points;
-/// 3-points Gauss-Legendre quadrature {<x_1, w_1>, <x_2, w_2>, <x_3, w_3>}
+/// 1D 3-points Gauss-Legendre quadrature \f${<x_1, w_1>, <x_2, w_2>, <x_3, w_3>}\f$
/// \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;
+extern const 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>}
+/// 1D 4-points Gauss-Legendre quadrature \f${<x_1, w_1>, <x_2, w_2>, <x_3, w_3>, <x_4, w_4>}\f$
/// \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;
+extern const 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>}
+/// 1D 5-points Gauss-Legendre quadrature \f${<x_1, w_1>, <x_2, w_2>, <x_3, w_3>, <x_4, w_4>, <x_5, w_5>}\f$
/// \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;
+extern const 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>}
+/// 1D 100-points Gauss-Legendre quadrature \f${<x_1, w_1>, <x_2, w_2>, <x_3, w_3>, ..., <x_{100}, w_{100}>}\f$
/// \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;
+extern const std::array<gaussQuadraturePoint, 100> gaussQuadrature100Points;
+
+// http://math2.uncc.edu/~shaodeng/TEACHING/math5172/Lectures/Lect_15.PDF
+// "Quadrature Formulas in Two Dimensions"
+// \int_0^1 \int_0^{1-eta} f(xi, eta) dxi deta = 1/2 sum_i w[i] f(xi[i], eta[i])
+
+/// 2D triangle Gauss-Legendre quadrature 3-points \f${<\xi_1, \eta_1, w_1>, ..., <\xi_3, \eta_3, w_3>}\f$
+/// \note Gauss-Legendre quadrature numerically evaluates the integral of a function \f$f(\xi, \eta)\f$ with a
+/// finite sum using some weights and specific points on the triangle.
+/// \note \f$\int_{0}^{1} \int_{0}^{1-\eta} f(\xi, \eta) d\xi d\eta = \sum_{i=1}^n w_i f(\xi_i, \eta_i)\f$
+/// \note n is the number of points used to discretized the integral
+/// \note \f$(\xi_i, \eta_i)\f$ is the parametrized location of the triangle point to evaluate the function with
+/// \note \f$w_i\f$ is the weight to assign to the function evaluation at the given point
+/// \note A 3-points Gauss-Legendre quadrature on the triangle is exact for polynomial functions of degree 2 or less.
+extern const std::array<gaussQuadratureTrianglePoint, 3> gaussQuadrature2DTriangle3Points;
+
+/// 2D triangle Gauss-Legendre quadrature 6-points \f${<\xi_1, \eta_1, w_1>, ..., <\xi_6, \eta_6, w_6>}\f$
+/// \note Gauss-Legendre quadrature numerically evaluates the integral of a function \f$f(\xi, \eta)\f$ with a
+/// finite sum using some weights and specific points on the triangle.
+/// \note \f$\int_{0}^{1} \int_{0}^{1-\eta} f(\xi, \eta) d\xi d\eta = \sum_{i=1}^n w_i f(\xi_i, \eta_i)\f$
+/// \note n is the number of points used to discretized the integral
+/// \note \f$(\xi_i, \eta_i)\f$ is the parametrized location of the triangle point to evaluate the function with
+/// \note \f$w_i\f$ is the weight to assign to the function evaluation at the given point
+/// \note A 6-points Gauss-Legendre quadrature on the triangle is exact for polynomial functions of degree 4 or less.
+extern const std::array<gaussQuadratureTrianglePoint, 6> gaussQuadrature2DTriangle6Points;
+
+/// 2D triangle Gauss-Legendre quadrature 12-points \f${<\xi_1, \eta_1, w_1>, ..., <\xi_{12}, \eta_{12}, w_{12}>}\f$
+/// \note Gauss-Legendre quadrature numerically evaluates the integral of a function \f$f(\xi, \eta)\f$ with a
+/// finite sum using some weights and specific points on the triangle.
+/// \note \f$\int_{0}^{1} \int_{0}^{1-\eta} f(\xi, \eta) d\xi d\eta = \sum_{i=1}^n w_i f(\xi_i, \eta_i)\f$
+/// \note n is the number of points used to discretized the integral
+/// \note \f$(\xi_i, \eta_i)\f$ is the parametrized location of the triangle point to evaluate the function with
+/// \note \f$w_i\f$ is the weight to assign to the function evaluation at the given point
+/// \note A 12-points Gauss-Legendre quadrature on the triangle is exact for polynomial functions of degree 6 or less.
+extern const std::array<gaussQuadratureTrianglePoint, 12> gaussQuadrature2DTriangle12Points;
}; // namespace Math
}; // namespace SurgSim
diff --git a/SurgSim/Math/Geometry.h b/SurgSim/Math/Geometry.h
index 88313db..096b66e 100644
--- a/SurgSim/Math/Geometry.h
+++ b/SurgSim/Math/Geometry.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -21,6 +21,7 @@
#include <Eigen/Geometry>
#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/Polynomial.h"
#include "SurgSim/Math/Vector.h"
/// \file Geometry.h a collection of functions that calculation geometric properties of various basic geometric shapes.
@@ -57,6 +58,33 @@ static const double ScalarEpsilon = 1e-10;
}
+/// Calculate the barycentric coordinates of a point with respect to a line segment.
+/// \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 sv0, sv1 Vertices of the line segment.
+/// \param [out] coordinates Barycentric coordinates.
+/// \return bool true on success, false if two or more if the line segment is considered degenerate
+/// \note The point need not be on the line segment, in which case, the barycentric coordinate of the projection
+/// is calculated.
+template <class T, int MOpt> inline
+bool barycentricCoordinates(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, 2, 1, MOpt>* coordinates)
+{
+ const Eigen::Matrix<T, 3, 1, MOpt> line = sv1 - sv0;
+ const T length2 = line.squaredNorm();
+ if (length2 < Geometry::SquaredDistanceEpsilon)
+ {
+ coordinates->setConstant((std::numeric_limits<double>::quiet_NaN()));
+ return false;
+ }
+ (*coordinates)[1] = (pt - sv0).dot(line) / length2;
+ (*coordinates)[0] = static_cast<T>(1) - (*coordinates)[1];
+ return true;
+}
+
/// 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
@@ -67,7 +95,7 @@ static const double ScalarEpsilon = 1e-10;
/// \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
+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,
@@ -75,15 +103,15 @@ bool barycentricCoordinates(const Eigen::Matrix<T, 3, 1, MOpt>& pt,
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);
+ 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)[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;
}
@@ -97,7 +125,7 @@ bool barycentricCoordinates(const Eigen::Matrix<T, 3, 1, MOpt>& pt,
/// \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
+/// \return bool true on success, false if the triangle is considered degenerate
template <class T, int MOpt> inline
bool barycentricCoordinates(
const Eigen::Matrix<T, 3, 1, MOpt>& pt,
@@ -106,7 +134,14 @@ bool barycentricCoordinates(
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);
+ Eigen::Matrix<T, 3, 1, MOpt> tn = (tv1 - tv0).cross(tv2 - tv0);
+ double norm = tn.norm();
+ if (norm < Geometry::DistanceEpsilon)
+ {
+ coordinates->setConstant((std::numeric_limits<double>::quiet_NaN()));
+ return false;
+ }
+ tn /= norm;
return barycentricCoordinates(pt, tv0, tv1, tv2, tn, coordinates);
}
@@ -159,6 +194,54 @@ bool isPointInsideTriangle(
baryCoords[2] >= -Geometry::ScalarEpsilon);
}
+/// Check if a point is on the edge of a triangle.
+/// \note Use barycentricCoordinates() if you need the coordinates.
+/// \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 (must be of norm 1 and a,b,c CCW).
+/// \return true if pt lies on the edge of the triangle tv0, tv1, tv2, false otherwise.
+template <class T, int MOpt> inline
+bool isPointOnTriangleEdge(
+ 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 &&
+ baryCoords.minCoeff() <= Geometry::ScalarEpsilon);
+}
+
+/// Check if a point is on the edge of 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 on the edge of the triangle tv0, tv1, tv2, false otherwise.
+template <class T, int MOpt> inline
+bool isPointOnTriangleEdge(
+ 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 &&
+ baryCoords.minCoeff() <= 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.
@@ -189,17 +272,17 @@ T distancePointLine(
// 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;
+ 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();
+ 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();
+ 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
@@ -217,14 +300,14 @@ T distancePointSegment(
const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
Eigen::Matrix<T, 3, 1, MOpt>* result)
{
- Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1-sv0;
+ 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();
+ return (pt - sv0).norm();
}
- T lambda = v01.dot(pt-sv0);
+ T lambda = v01.dot(pt - sv0);
if (lambda <= 0)
{
*result = sv0;
@@ -235,9 +318,9 @@ T distancePointSegment(
}
else
{
- *result = sv0 + lambda*v01/v01Norm2;
+ *result = sv0 + lambda * v01 / v01Norm2;
}
- return (*result-pt).norm();
+ return (*result - pt).norm();
}
/// Determine the distance between two lines
@@ -266,7 +349,7 @@ T distanceLineLine(
// 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;
+ Eigen::Matrix<T, 3, 1, MOpt> l0v01 = l0v1 - l0v0;
T a = l0v01.squaredNorm();
if (a <= Geometry::SquaredDistanceEpsilon)
{
@@ -274,7 +357,7 @@ T distanceLineLine(
*pt0 = l0v0;
return distancePointSegment(l0v0, l1v0, l1v1, pt1);
}
- Eigen::Matrix<T, 3, 1, MOpt> l1v01 = l1v1-l1v0;
+ Eigen::Matrix<T, 3, 1, MOpt> l1v01 = l1v1 - l1v0;
T b = -l0v01.dot(l1v01);
T c = l1v01.squaredNorm();
if (c <= Geometry::SquaredDistanceEpsilon)
@@ -283,10 +366,10 @@ T distanceLineLine(
*pt1 = l1v0;
return distancePointSegment(l1v0, l0v0, l0v1, pt0);
}
- Eigen::Matrix<T, 3, 1, MOpt> l0v0_l1v0 = l0v0-l1v0;
+ 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;
+ T ratio = a * c - b * b;
if (std::abs(ratio) <= Geometry::ScalarEpsilon)
{
// parallel case
@@ -297,12 +380,12 @@ T distanceLineLine(
{
// non-parallel case
T inv_ratio = T(1) / ratio;
- lambda0 = (b*e - c*d) * inv_ratio;
- lambda1 = (b*d - a*e) * inv_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();
+ return ((*pt0) - (*pt1)).norm();
}
@@ -325,8 +408,8 @@ T distanceSegmentSegment(
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)
+ 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
@@ -334,7 +417,7 @@ T distanceSegmentSegment(
// 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;
+ Eigen::Matrix<T, 3, 1, MOpt> s0v01 = s0v1 - s0v0;
T a = s0v01.squaredNorm();
if (a <= Geometry::SquaredDistanceEpsilon)
{
@@ -342,7 +425,7 @@ T distanceSegmentSegment(
*pt0 = s0v0;
return distancePointSegment<T>(s0v0, s1v0, s1v1, pt1);
}
- Eigen::Matrix<T, 3, 1, MOpt> s1v01 = s1v1-s1v0;
+ Eigen::Matrix<T, 3, 1, MOpt> s1v01 = s1v1 - s1v0;
T b = -s0v01.dot(s1v01);
T c = s1v01.squaredNorm();
if (c <= Geometry::SquaredDistanceEpsilon)
@@ -351,11 +434,11 @@ T distanceSegmentSegment(
*pt1 = s1v1;
return distancePointSegment<T>(s1v0, s0v0, s0v1, pt0);
}
- Eigen::Matrix<T, 3, 1, MOpt> tempLine = s0v0-s1v0;
+ 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)
+ T ratio = a * c - b * b;
+ T s, t; // parametrization variables (do not initialize)
int region = -1;
T tmp;
// Non-parallel case
@@ -375,8 +458,8 @@ T distanceSegmentSegment(
// 6 | 7 | 8
// | |
//
- s = b*e-c*d;
- t = b*d-a*e;
+ s = b * e - c * d;
+ t = b * d - a * e;
if (s >= 0)
{
if (s <= ratio)
@@ -438,149 +521,149 @@ T distanceSegmentSegment(
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
- {
+ case 0:
+ // Global minimum inside [0,1] [0,1]
+ s /= ratio;
+ t /= ratio;
+ edge = edge_skip;
+ break;
+ case 1:
edge = s1;
- }
- break;
- case 3:
- edge = t1;
- break;
- case 4:
- // Q_s(0,1)/2 = b+d
- if (b+d > 0)
- {
- edge = s0;
- }
- else
- {
+ 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 5:
- edge = s0;
- break;
- case 6:
- // Q_s(0,0)/2 = d
- if (d > 0)
- {
+ break;
+ case 4:
+ // Q_s(0,1)/2 = b+d
+ if (b + d > 0)
+ {
+ edge = s0;
+ }
+ else
+ {
+ edge = t1;
+ }
+ break;
+ case 5:
edge = s0;
- }
- else
- {
- edge = t0;
- }
- break;
- case 7:
- edge = t0;
- break;
- case 8:
- // Q_s(1,0)/2 = a+d
- if (a+d > 0)
- {
+ break;
+ case 6:
+ // Q_s(0,0)/2 = d
+ if (d > 0)
+ {
+ edge = s0;
+ }
+ else
+ {
+ edge = t0;
+ }
+ break;
+ case 7:
edge = t0;
- }
- else
- {
- edge = s1;
- }
- break;
- default:
- break;
+ 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)
- {
+ 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;
- }
- else if (-tmp > a)
- {
+ 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;
- }
- else
- {
- s = -tmp/a;
- }
- break;
- case edge_skip:
- break;
- default:
- break;
+ 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
@@ -598,21 +681,21 @@ T distanceSegmentSegment(
else if (-d <= a)
{
// s-segment 0 end-point in the middle of the t 0-1 segment, get distance
- s = -d/a;
+ s = -d / a;
t = 0;
}
else
{
// s-segment 1 is definitely closer
s = 1;
- tmp = a+d;
+ tmp = a + d;
if (-tmp >= b)
{
t = 1;
}
else
{
- t = -tmp/b;
+ t = -tmp / b;
}
}
}
@@ -628,7 +711,7 @@ T distanceSegmentSegment(
else if (d <= 0)
{
// mid-0
- s = -d/a;
+ s = -d / a;
t = 0;
}
else
@@ -641,7 +724,7 @@ T distanceSegmentSegment(
}
else
{
- t = -d/b;
+ t = -d / b;
}
}
}
@@ -653,7 +736,7 @@ T distanceSegmentSegment(
*s0t = s;
*s1t = t;
}
- return ((*pt1)-(*pt0)).norm();
+ 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
@@ -678,8 +761,8 @@ T distancePointTriangle(
// 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;
+ 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)
{
@@ -699,15 +782,15 @@ T distancePointTriangle(
// Degenerate edge 3
return distancePointSegment<T>(pt, tv0, tv1, result);
}
- Eigen::Matrix<T, 3, 1, MOpt> tv0pv0 = tv0-pt;
+ 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;
+ 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 + t <= ratio)
{
if (s < 0)
{
@@ -764,112 +847,112 @@ T distancePointTriangle(
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)
- {
+ 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;
- }
- 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
- {
+ 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 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)
- {
+ 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;
- }
- else
- {
- edge = s1t1;
- }
- break;
- default:
- break;
+ 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)
- {
+ case s0:
+ // F(t) = Q(0, t), F'(t)=0 when -e/c = 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;
+ 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();
+ return ((*result) - pt).norm();
}
/// Calculate the intersection of a line segment with a triangle
@@ -897,12 +980,12 @@ bool doesCollideSegmentTriangle(
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;
+ 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;
+ 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);
@@ -911,16 +994,16 @@ bool doesCollideSegmentTriangle(
// Ray is parallel to triangle plane
if (std::abs(b) <= Geometry::AngularEpsilon)
{
- if (a == 0)
+ if (std::abs(a) <= Geometry::AngularEpsilon)
{
// Ray lies in triangle plane
Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
- for (int i=0; i<2; ++i)
+ for (int i = 0; i < 2; ++i)
{
- barycentricCoordinates((i==0?sv0:sv1), tv0, tv1, tv2, tn, &baryCoords);
+ 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;
+ *result = (i == 0) ? sv0 : sv1;
return true;
}
}
@@ -942,7 +1025,7 @@ bool doesCollideSegmentTriangle(
return false;
}
//Ray comes toward triangle but isn't long enough to reach it
- if (r > 1+Geometry::DistanceEpsilon)
+ if (r > 1 + Geometry::DistanceEpsilon)
{
return false;
}
@@ -960,13 +1043,13 @@ bool doesCollideSegmentTriangle(
// Get and test parametric coords
T s = (uv * wv - vv * wu) / D;
// I is outside T
- if (s < 0 || s > 1)
+ if (s < -Geometry::DistanceEpsilon || s > 1 + Geometry::DistanceEpsilon)
{
return false;
}
T t = (uv * wu - uu * wv) / D;
// I is outside T
- if (t < 0 || (s + t) > 1)
+ if (t < -Geometry::DistanceEpsilon || (s + t) > 1 + Geometry::DistanceEpsilon)
{
return false;
}
@@ -993,7 +1076,7 @@ T distancePointPlane(
Eigen::Matrix<T, 3, 1, MOpt>* result)
{
T dist = n.dot(pt) + d;
- *result = pt - n*dist;
+ *result = pt - n * dist;
return dist;
}
@@ -1027,9 +1110,9 @@ T distanceSegmentPlane(
Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
if (std::abs(n.dot(v01)) <= Geometry::AngularEpsilon)
{
- *closestPointSegment = (sv0 + sv1)*T(0.5);
+ *closestPointSegment = (sv0 + sv1) * T(0.5);
dist0 = n.dot(*closestPointSegment) + d;
- *planeIntersectionPoint = *closestPointSegment - dist0*n;
+ *planeIntersectionPoint = *closestPointSegment - dist0 * n;
return (std::abs(dist0) < Geometry::DistanceEpsilon ? 0 : dist0);
}
// Both on the same side
@@ -1038,21 +1121,21 @@ T distanceSegmentPlane(
if (std::abs(dist0) < std::abs(dist1))
{
*closestPointSegment = sv0;
- *planeIntersectionPoint = sv0 - dist0*n;
+ *planeIntersectionPoint = sv0 - dist0 * n;
return dist0;
}
else
{
*closestPointSegment = sv1;
- *planeIntersectionPoint = sv1 - dist1*n;
+ *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);
+ 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;
@@ -1084,9 +1167,9 @@ T distanceTrianglePlane(
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;
+ 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()));
@@ -1114,7 +1197,7 @@ T distanceTrianglePlane(
}
else
{
- Eigen::Matrix<T, 3, 1, MOpt> t12 = tv2-tv1;
+ Eigen::Matrix<T, 3, 1, MOpt> t12 = tv2 - tv1;
*planeProjectionPoint = tv1 + (-d - n.dot(tv1)) / n.dot(t12) * t12;
}
}
@@ -1133,18 +1216,18 @@ T distanceTrianglePlane(
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];
+ 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();
@@ -1177,7 +1260,7 @@ bool doesIntersectPlanePlane(
return false; // planes disjoint
}
// Compute common point
- *pt0 = (pd1*pn0-pd0*pn1).cross(lineDir) / lineDirNorm2;
+ *pt0 = (pd1 * pn0 - pd0 * pn1).cross(lineDir) / lineDirNorm2;
*pt1 = *pt0 + lineDir;
return true;
}
@@ -1237,7 +1320,7 @@ T distanceSegmentTriangle(
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 Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
const T v01DotTn = n.dot(v01);
if (std::abs(v01DotTn) <= Geometry::AngularEpsilon)
{
@@ -1262,7 +1345,7 @@ T distanceSegmentTriangle(
// 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;
+ T lambda = -n.dot(sv0 - tv0) / v01DotTn;
if (lambda >= 0 && lambda <= 1)
{
*segmentPoint = *trianglePoint = sv0 + lambda * v01;
@@ -1298,26 +1381,26 @@ T distanceSegmentTriangle(
(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;
+ 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 ...
@@ -1351,9 +1434,9 @@ T distanceTriangleTriangle(
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);
+ 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);
+ 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)
@@ -1412,7 +1495,7 @@ 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)
+ 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);
@@ -1507,6 +1590,26 @@ bool doesIntersectBoxCapsule(
return doesIntersect;
}
+/// Helper method to determine the nearest point between a point and a line.
+/// \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 point is the point under consideration.
+/// \param segment0 one point on the line
+/// \param segment1 second point on the line
+/// \return the closest point on the line through the segment to the point under test
+template <class T, int VOpt>
+Eigen::Matrix<T, 3, 1, VOpt> nearestPointOnLine(const Eigen::Matrix<T, 3, 1, VOpt>& point,
+ const Eigen::Matrix<T, 3, 1, VOpt>& segment0, const Eigen::Matrix<T, 3, 1, VOpt>& segment1)
+{
+ auto pointToSegmentStart = segment0 - point;
+ auto segmentDirection = segment1 - segment0;
+ auto squaredNorm = segmentDirection.squaredNorm();
+ SURGSIM_ASSERT(squaredNorm != 0.0) << "Line is defined by two collocated points.";
+ auto distance = -pointToSegmentStart.dot(segmentDirection) / squaredNorm;
+ auto p0Proj = segment0 + distance * segmentDirection;
+ return p0Proj;
+}
+
/// 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
///
@@ -1607,13 +1710,179 @@ bool calculateContactTriangleTriangle(
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
+/// Calculate the contact between two triangles.
+/// 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.
+/// \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 calculateContactTriangleTriangleSeparatingAxis(
+ 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.
+/// \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 [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 calculateContactTriangleTriangleSeparatingAxis(
+ 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);
+
+/// Calculate the contact between a capsule and a triangle.
+/// If the shapes intersect, the deepest penetration of the capsule along the triangle normal is calculated.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param tv0,tv1,tv2 Vertices of the triangle.
+/// \param tn Normal of the triangle, should be normalized.
+/// \param cv0,cv1 Ends of the capsule axis.
+/// \param cr Capsule radius.
+/// \param [out] penetrationDepth The depth of penetration.
+/// \param [out] penetrationPointTriangle The contact point on triangle.
+/// \param [out] penetrationPointCapsule The contact point on capsule.
+/// \param [out] contactNormal The contact normal that points from capsule to triangle.
+/// \param [out] penetrationPointCapsuleAxis The point on the capsule axis closest to the triangle.
+/// \return True, if intersection is detected.
+/// \note The [out] params are not modified if there is no intersection.
+/// \note If penetrationPointTriangle is moved by (contactNormal*penetrationDepth*0.5) and penetrationPointCapsule
+/// is moved by -(contactNormal*penetrationDepth*0.5), the shapes will no longer be intersecting.
+template <class T, int MOpt> inline
+bool calculateContactTriangleCapsule(
+ 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,
+ const Eigen::Matrix<T, 3, 1, MOpt>& cv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& cv1,
+ double cr,
+ T* penetrationDepth,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointTriangle,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsule,
+ Eigen::Matrix<T, 3, 1, MOpt>* contactNormal,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsuleAxis);
+
+/// Calculate the contact between a capsule and a triangle.
+/// If the shapes intersect, the deepest penetration of the capsule along the triangle normal is calculated.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param tv0,tv1,tv2 Vertices of the triangle.
+/// \param cv0,cv1 Ends of the capsule axis.
+/// \param cr Capsule radius.
+/// \param [out] penetrationDepth The depth of penetration.
+/// \param [out] penetrationPointTriangle The contact point on triangle.
+/// \param [out] penetrationPointCapsule The contact point on capsule.
+/// \param [out] contactNormal The contact normal that points from capsule to triangle.
+/// \param [out] penetrationPointCapsuleAxis The point on the capsule axis closest to the triangle.
+/// \return True, if intersection is detected.
+/// \note The [out] params are not modified if there is no intersection.
+/// \note If penetrationPointTriangle is moved by (contactNormal*penetrationDepth*0.5) and penetrationPointCapsule
+/// is moved by -(contactNormal*penetrationDepth*0.5), the shapes will no longer be intersecting.
+template <class T, int MOpt> inline
+bool calculateContactTriangleCapsule(
+ 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>& cv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& cv1,
+ double cr,
+ T* penetrationDepth,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointTriangle,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsule,
+ Eigen::Matrix<T, 3, 1, MOpt>* contactNormal,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsuleAxis);
+
+/// Test when 4 points are coplanar in the range [0..1] given their linear motion
+/// \tparam T The scalar type
+/// \tparam MOpt The matrix options
+/// \param A, B, C, D the 4 point' motion (each has a pair from -> to)
+/// \param[out] timesOfCoplanarity The normalized times (in [0..1]) at which the 4 points are coplanar
+/// \return The number of times the 4 points are coplanar throughout their motion in [0..1]
+template <class T, int MOpt>
+int timesOfCoplanarityInRange01(
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& A,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& B,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& C,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& D,
+ std::array<T, 3>* timesOfCoplanarity)
+{
+ /// Let's define the following:
+ /// A(t) = A0 + t * VA with VA = A1 - A0
+ /// Similarily for B(t), C(t) and D(t)
+ /// Therefore we have AB(t) = B(t) - A(t) = B(0) + t * VB - A(0) - t * VA
+ /// = AB(0) + t * [VB - VA] = AB(0) + t * VAB
+ ///
+ /// The 4 points ABCD are coplanar are time t if they verify:
+ /// [AB(t).cross(CD(t))].AC(t) = 0
+ /// We develop this equation to clearly formulate the resulting cubic equation:
+ ///
+ /// [AB(0).cross(CD(0)) + t*AB(0).cross(VCD) + t*VAB.cross(CD(0)) + t^2*VAB.cross(VCD)] . [AC(0) + t * VAC] = 0
+ /// t^0 * [[AB(0).cross(CD(0))].AC(0)] +
+ /// t^1 * [[AB(0).cross(CD(0))].VAC + [AB(0).cross(VCD)].AC(0) + [VAB.cross(CD(0))].AC(0)] +
+ /// t^2 * [[AB(0).cross(VCD)].VAC + [VAB.cross(CD(0))].VAC + [VAB.cross(VCD)].AC(0)] +
+ /// t^3 * [[VAB.cross(VCD)].VAC] = 0
+ Eigen::Matrix<T, 3, 1, MOpt> AB0 = B.first - A.first;
+ Eigen::Matrix<T, 3, 1, MOpt> AC0 = C.first - A.first;
+ Eigen::Matrix<T, 3, 1, MOpt> CD0 = D.first - C.first;
+ Eigen::Matrix<T, 3, 1, MOpt> VA = (A.second - A.first);
+ Eigen::Matrix<T, 3, 1, MOpt> VC = (C.second - C.first);
+ Eigen::Matrix<T, 3, 1, MOpt> VAB = (B.second - B.first) - VA;
+ Eigen::Matrix<T, 3, 1, MOpt> VAC = VC - VA;
+ Eigen::Matrix<T, 3, 1, MOpt> VCD = (D.second - D.first) - VC;
+ T a0 = AB0.cross(CD0).dot(AC0);
+ T a1 = AB0.cross(CD0).dot(VAC) + (AB0.cross(VCD) + VAB.cross(CD0)).dot(AC0);
+ T a2 = (AB0.cross(VCD) + VAB.cross(CD0)).dot(VAC) + VAB.cross(VCD).dot(AC0);
+ T a3 = VAB.cross(VCD).dot(VAC);
+
+ return findRootsInRange01(Polynomial<T, 3>(a0, a1, a2, a3), timesOfCoplanarity);
+}
}; // namespace Math
}; // namespace SurgSim
+#include "SurgSim/Math/PointTriangleCcdContactCalculation-inl.h"
+#include "SurgSim/Math/SegmentSegmentCcdContactCalculation-inl.h"
+#include "SurgSim/Math/TriangleCapsuleContactCalculation-inl.h"
#include "SurgSim/Math/TriangleTriangleIntersection-inl.h"
#include "SurgSim/Math/TriangleTriangleContactCalculation-inl.h"
-
#endif
diff --git a/SurgSim/Math/IntervalArithmetic-inl.h b/SurgSim/Math/IntervalArithmetic-inl.h
new file mode 100644
index 0000000..bfa8d8d
--- /dev/null
+++ b/SurgSim/Math/IntervalArithmetic-inl.h
@@ -0,0 +1,899 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_INTERVALARITHMETIC_INL_H
+#define SURGSIM_MATH_INTERVALARITHMETIC_INL_H
+
+#include "SurgSim/Math/MinMax.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+template <class T>
+Interval<T>::Interval(): m_min(static_cast<T>(0)), m_max(static_cast<T>(0)) {}
+
+template <class T>
+Interval<T>::Interval(const T& min, const T& max): m_min(min), m_max(max)
+{
+ SURGSIM_ASSERT(min <= max) << "Incorrect order of interval bounds";
+}
+
+template <class T>
+Interval<T>::Interval(const Interval<T>& i): m_min(i.m_min), m_max(i.m_max) {}
+
+template <class T>
+Interval<T>::Interval(Interval<T>&& i): m_min(static_cast<T>(0)), m_max(static_cast<T>(0))
+{
+ m_min = i.m_min;
+ m_max = i.m_max;
+}
+
+template <class T>
+Interval<T>& Interval<T>::operator=(const Interval<T>& i)
+{
+ m_min = i.m_min;
+ m_max = i.m_max;
+ return *this;
+}
+
+template <class T>
+Interval<T>& Interval<T>::operator=(Interval<T>&& i)
+{
+ m_min = i.m_min;
+ m_max = i.m_max;
+ return *this;
+}
+
+template <class T>
+Interval<T> Interval<T>::minToMax(const T& a1, const T& a2)
+{
+ T min, max;
+ minMax(a1, a2, &min, &max);
+ return Interval<T>(min, max);
+}
+
+template <class T>
+Interval<T> Interval<T>::minToMax(const T& a1, const T& a2, const T& a3)
+{
+ T min, max;
+ minMax(a1, a2, a3, &min, &max);
+ return Interval<T>(min, max);
+}
+
+template <class T>
+Interval<T> Interval<T>::minToMax(const T& a1, const T& a2, const T& a3, const T& a4)
+{
+ T min, max;
+ minMax(a1, a2, a3, a4, &min, &max);
+ return Interval<T>(min, max);
+}
+
+template <class T>
+bool Interval<T>::overlapsWith(const Interval<T>& i) const
+{
+ return (m_min <= i.m_max && i.m_min <= m_max);
+}
+
+template <class T>
+bool Interval<T>::contains(const T& val) const
+{
+ return (m_min <= val && m_max >= val);
+}
+
+template <class T>
+bool Interval<T>::containsZero() const
+{
+ return contains(static_cast<T>(0));
+}
+
+template <class T>
+bool Interval<T>::isApprox(const Interval<T>& i, const T& epsilon) const
+{
+ return (std::abs(m_min - i.m_min) <= epsilon) && (std::abs(m_max - i.m_max) <= epsilon);
+}
+
+template <class T>
+bool Interval<T>::operator ==(const Interval<T>& i) const
+{
+ return (m_min == i.m_min && m_max == i.m_max);
+}
+
+template <class T>
+bool Interval<T>::operator !=(const Interval<T>& i) const
+{
+ return !(this->operator==(i));
+}
+
+template <class T>
+Interval<T>& Interval<T>::addThickness(const T& thickness)
+{
+ m_min -= thickness;
+ m_max += thickness;
+ return *this;
+}
+
+template <class T>
+Interval<T>& Interval<T>::extendToInclude(const T& x)
+{
+ if (x < m_min)
+ {
+ m_min = x;
+ }
+ else if (x > m_max)
+ {
+ m_max = x;
+ }
+ return *this;
+}
+
+template <class T>
+Interval<T>& Interval<T>::extendToInclude(const Interval<T>& i)
+{
+ if (i.m_min < m_min)
+ {
+ m_min = i.m_min;
+ }
+ if (i.m_max > m_max)
+ {
+ m_max = i.m_max;
+ }
+ return *this;
+}
+
+template <class T>
+Interval<T> Interval<T>::operator +(const Interval<T>& i) const
+{
+ return Interval<T>(m_min + i.m_min, m_max + i.m_max);
+}
+
+template <class T>
+Interval<T> Interval<T>::operator +(const T& v) const
+{
+ return Interval<T>(m_min + v, m_max + v);
+}
+
+template <class T>
+Interval<T>& Interval<T>::operator +=(const Interval<T>& i)
+{
+ m_min += i.m_min;
+ m_max += i.m_max;
+ return *this;
+}
+
+template <class T>
+Interval<T>& Interval<T>::operator +=(const T& v)
+{
+ m_min += v;
+ m_max += v;
+ return *this;
+}
+
+template <class T>
+Interval<T> Interval<T>::operator -() const
+{
+ return Interval<T>(-m_max, -m_min);
+}
+
+template <class T>
+Interval<T> Interval<T>::operator -(const Interval<T>& i) const
+{
+ return Interval<T>(m_min - i.m_max, m_max - i.m_min);
+}
+
+template <class T>
+Interval<T> Interval<T>::operator -(const T& v) const
+{
+ return Interval<T>(m_min - v, m_max - v);
+}
+
+template <class T>
+Interval<T>& Interval<T>::operator -=(const Interval<T>& i)
+{
+ m_min -= i.m_max;
+ m_max -= i.m_min;
+ return *this;
+}
+
+template <class T>
+Interval<T>& Interval<T>::operator -=(const T& v)
+{
+ m_min -= v;
+ m_max -= v;
+ return *this;
+}
+
+template <class T>
+Interval<T> Interval<T>::operator *(const Interval<T>& i) const
+{
+ return minToMax(m_min * i.m_min, m_min * i.m_max, m_max * i.m_min, m_max * i.m_max);
+}
+
+template <class T>
+Interval<T> Interval<T>::operator *(const T& v) const
+{
+ if (v >= 0)
+ {
+ return Interval<T>(m_min * v, m_max * v);
+ }
+ else
+ {
+ return Interval<T>(m_max * v, m_min * v);
+ }
+}
+
+template <class T>
+Interval<T>& Interval<T>::operator *=(const Interval<T>& i)
+{
+ *this = minToMax(m_min * i.m_min, m_min * i.m_max, m_max * i.m_min, m_max * i.m_max);
+ return *this;
+}
+
+template <class T>
+Interval<T>& Interval<T>::operator *=(const T& v)
+{
+ *this = minToMax(v * m_min, v * m_max);
+ return *this;
+}
+
+template <class T>
+Interval<T> Interval<T>::inverse() const
+{
+ SURGSIM_ASSERT(!containsZero()) << "Cannot invert or divide by an interval containing 0. Interval: [" <<
+ getMin() << ", " << getMax() << "]" << std::endl;
+ return Interval<T>(static_cast<T>(1) / m_max, static_cast<T>(1) / m_min);
+}
+
+template <class T>
+Interval<T> Interval<T>::operator /(const Interval<T>& i) const
+{
+ return (*this) * i.inverse();
+}
+
+template <class T>
+Interval<T>& Interval<T>::operator /=(const Interval<T>& i)
+{
+ return (*this) = (*this) * i.inverse();
+}
+
+template <class T>
+Interval<T> Interval<T>::square() const
+{
+ T lowerBoundSquared = m_min * m_min;
+ T upperBoundSquared = m_max * m_max;
+ T minSquare, maxSquare;
+ minMax(lowerBoundSquared, upperBoundSquared, &minSquare, &maxSquare);
+ return Interval<T>((m_min < 0 && m_max > 0) ? 0 : minSquare, maxSquare);
+}
+
+template <class T>
+T Interval<T>::getMin() const
+{
+ return m_min;
+}
+
+template <class T>
+T Interval<T>::getMax() const
+{
+ return m_max;
+}
+
+template <class T>
+Interval<T> Interval<T>::lowerHalf() const
+{
+ return Interval<T>(m_min, (m_min + m_max) * static_cast<T>(0.5));
+}
+
+template <class T>
+Interval<T> Interval<T>::upperHalf() const
+{
+ return Interval<T>((m_min + m_max) * static_cast<T>(0.5), m_max);
+}
+
+// Class ND
+template <class T, int N>
+IntervalND<T, N>::IntervalND()
+{
+ for (int i = 0; i < N; i++)
+ {
+ m_interval[i] = Interval<T>();
+ }
+}
+
+template <class T, int N>
+IntervalND<T, N>::IntervalND(const std::array<Interval<T>, N>& x)
+{
+ for (int i = 0; i < N; i++)
+ {
+ m_interval[i] = x[i];
+ }
+}
+
+template <class T, int N>
+IntervalND<T, N>::IntervalND(const IntervalND<T, N>& interval)
+{
+ for (int i = 0; i < N; i++)
+ {
+ m_interval[i] = interval.m_interval[i];
+ }
+}
+
+template <class T, int N>
+IntervalND<T, N>::IntervalND(IntervalND<T, N>&& i)
+{
+ for (int j = 0; j < N; j++)
+ {
+ m_interval[j] = i.m_interval[j];
+ }
+}
+
+template <class T, int N>
+IntervalND<T, N>::IntervalND(const std::array<T, N>& a, const std::array<T, N>& b)
+{
+ for (int i = 0; i < N; ++i)
+ {
+ m_interval[i] = Interval<T>::minToMax(a[i], b[i]);
+ }
+}
+
+template <class T, int N>
+IntervalND<T, N>& IntervalND<T, N>::operator =(const IntervalND<T, N>& interval)
+{
+ for (int i = 0; i < N; i++)
+ {
+ m_interval[i] = interval.m_interval[i];
+ }
+ return *this;
+}
+
+template <class T, int N>
+IntervalND<T, N>& IntervalND<T, N>::operator=(IntervalND<T, N>&& i)
+{
+ if (this != &i)
+ {
+ for (int j = 0; j < N; j++)
+ {
+ m_interval[j] = i.m_interval[j];
+ }
+ }
+
+ return *this;
+}
+
+template <class T, int N>
+bool IntervalND<T, N>::overlapsWith(const IntervalND<T, N>& interval) const
+{
+ // For the rectangular [hyper]prisms to overlap, they must overlap in all axes.
+ for (int i = 0; i < N; ++i)
+ {
+ if (!m_interval[i].overlapsWith(interval.m_interval[i]))
+ {
+ return false;
+ }
+ }
+ return true;
+}
+
+template <class T, int N>
+bool IntervalND<T, N>::isApprox(const IntervalND<T, N>& interval, const T& epsilon) const
+{
+ for (int i = 0; i < N; i++)
+ {
+ if (!m_interval[i].isApprox(interval.m_interval[i], epsilon))
+ {
+ return false;
+ }
+ }
+ return true;
+}
+
+template <class T, int N>
+bool IntervalND<T, N>::operator ==(const IntervalND<T, N>& interval) const
+{
+ for (int i = 0; i < N; i++)
+ {
+ if (m_interval[i] != interval.m_interval[i])
+ {
+ return false;
+ }
+ }
+ return true;
+}
+
+template <class T, int N>
+bool IntervalND<T, N>::operator !=(const IntervalND<T, N>& interval) const
+{
+ return !(this->operator==(interval));
+}
+
+template <class T, int N>
+IntervalND<T, N>& IntervalND<T, N>::addThickness(const T& thickness)
+{
+ for (int i = 0; i < N; i++)
+ {
+ m_interval[i].addThickness(thickness);
+ }
+ return *this;
+}
+
+template <class T, int N>
+IntervalND<T, N> IntervalND<T, N>::operator +(const IntervalND<T, N>& interval) const
+{
+ IntervalND<T, N> ret;
+ for (int i = 0; i < N; i++)
+ {
+ ret.m_interval[i] = m_interval[i] + interval.m_interval[i];
+ }
+ return ret;
+}
+
+template <class T, int N>
+IntervalND<T, N>& IntervalND<T, N>::operator +=(const IntervalND<T, N>& interval)
+{
+ for (int i = 0; i < N; i++)
+ {
+ m_interval[i] += interval.m_interval[i];
+ }
+ return *this;
+}
+
+template <class T, int N>
+IntervalND<T, N> IntervalND<T, N>::operator -(const IntervalND<T, N>& interval) const
+{
+ IntervalND<T, N> ret;
+ for (int i = 0; i < N; i++)
+ {
+ ret.m_interval[i] = m_interval[i] - interval.m_interval[i];
+ }
+ return ret;
+}
+
+template <class T, int N>
+IntervalND<T, N>& IntervalND<T, N>::operator -=(const IntervalND<T, N>& interval)
+{
+ for (int i = 0; i < N; i++)
+ {
+ m_interval[i] -= interval.m_interval[i];
+ }
+ return *this;
+}
+
+template <class T, int N>
+IntervalND<T, N> IntervalND<T, N>::operator *(const IntervalND<T, N>& interval) const
+{
+ IntervalND<T, N> ret;
+ for (int i = 0; i < N; i++)
+ {
+ ret.m_interval[i] = m_interval[i] * interval.m_interval[i];
+ }
+ return ret;
+}
+
+template <class T, int N>
+IntervalND<T, N>& IntervalND<T, N>::operator *=(const IntervalND<T, N>& interval)
+{
+ for (int i = 0; i < N; i++)
+ {
+ m_interval[i] *= interval.m_interval[i];
+ }
+ return *this;
+}
+
+template <class T, int N>
+IntervalND<T, N> IntervalND<T, N>::inverse() const
+{
+ IntervalND<T, N> ret;
+ for (int i = 0; i < N; i++)
+ {
+ ret.m_interval[i] = m_interval[i].inverse();
+ }
+ return ret;
+}
+
+template <class T, int N>
+IntervalND<T, N> IntervalND<T, N>::operator /(const IntervalND<T, N>& interval) const
+{
+ IntervalND<T, N> ret;
+ for (int i = 0; i < N; i++)
+ {
+ ret.m_interval[i] = m_interval[i] / interval.m_interval[i];
+ }
+ return ret;
+}
+
+template <class T, int N>
+IntervalND<T, N>& IntervalND<T, N>::operator /=(const IntervalND<T, N>& interval)
+{
+ for (int i = 0; i < N; i++)
+ {
+ m_interval[i] /= interval.m_interval[i];
+ }
+ return *this;
+}
+
+template <class T, int N>
+Interval<T> IntervalND<T, N>::dotProduct(const IntervalND<T, N>& interval) const
+{
+ Interval<T> ret(static_cast<T>(0), static_cast<T>(0));
+ for (int i = 0; i < N; i++)
+ {
+ ret += m_interval[i] * interval.m_interval[i];
+ }
+ return ret;
+}
+
+template <class T, int N>
+Interval<T> IntervalND<T, N>::magnitudeSquared() const
+{
+ Interval<T> result = m_interval[0].square();
+ for (int i = 1; i < N; ++i)
+ {
+ result += m_interval[i].square();
+ }
+ return result;
+}
+
+template <class T, int N>
+Interval<T> IntervalND<T, N>::magnitude() const
+{
+ Interval<T> magnitudeSq = magnitudeSquared();
+ // Both minimum and maximum are guaranteed to be non-negative.
+ return Interval<T>(sqrt(magnitudeSq.getMin()), sqrt(magnitudeSq.getMax()));
+}
+
+template <class T, int N>
+const Interval<T>& IntervalND<T, N>::getAxis(size_t i) const
+{
+ return m_interval[i];
+}
+
+template <class T>
+IntervalND<T, 3>::IntervalND()
+{
+ m_interval[0] = Interval<T>();
+ m_interval[1] = Interval<T>();
+ m_interval[2] = Interval<T>();
+}
+
+template <class T>
+IntervalND<T, 3>::IntervalND(const std::array<Interval<T>, 3>& x)
+{
+ m_interval[0] = x[0];
+ m_interval[1] = x[1];
+ m_interval[2] = x[2];
+}
+
+template <class T>
+IntervalND<T, 3>::IntervalND(const Interval<T>& x, const Interval<T>& y, const Interval<T>& z)
+{
+ m_interval[0] = x;
+ m_interval[1] = y;
+ m_interval[2] = z;
+}
+
+template <class T>
+IntervalND<T, 3>::IntervalND(const IntervalND<T, 3>& i)
+{
+ m_interval[0] = i.m_interval[0];
+ m_interval[1] = i.m_interval[1];
+ m_interval[2] = i.m_interval[2];
+}
+
+template <class T>
+IntervalND<T, 3>::IntervalND(IntervalND<T, 3>&& i)
+{
+ m_interval[0] = i.m_interval[0];
+ m_interval[1] = i.m_interval[1];
+ m_interval[2] = i.m_interval[2];
+}
+
+template <class T>
+IntervalND<T, 3>::IntervalND(const std::array<T, 3>& a, const std::array<T, 3>& b)
+{
+ m_interval[0] = Interval<T>::minToMax(a[0], b[0]);
+ m_interval[1] = Interval<T>::minToMax(a[1], b[1]);
+ m_interval[2] = Interval<T>::minToMax(a[2], b[2]);
+}
+
+template <class T>
+IntervalND<T, 3>& IntervalND<T, 3>::operator =(const IntervalND<T, 3>& i)
+{
+ m_interval[0] = i.m_interval[0];
+ m_interval[1] = i.m_interval[1];
+ m_interval[2] = i.m_interval[2];
+ return *this;
+}
+
+template <class T>
+IntervalND<T, 3>& IntervalND<T, 3>::operator=(IntervalND<T, 3>&& i)
+{
+ m_interval[0] = i.m_interval[0];
+ m_interval[1] = i.m_interval[1];
+ m_interval[2] = i.m_interval[2];
+ return *this;
+}
+
+template <class T>
+bool IntervalND<T, 3>::overlapsWith(const IntervalND<T, 3>& interval) const
+{
+ // For the rectangular prisms to overlap, they must overlap in all axes.
+ return (m_interval[0].overlapsWith(interval.m_interval[0]) && m_interval[1].overlapsWith(interval.m_interval[1])
+ && m_interval[2].overlapsWith(interval.m_interval[2]));
+}
+
+template <class T>
+bool IntervalND<T, 3>::isApprox(const IntervalND<T, 3>& i, const T& epsilon) const
+{
+ return (m_interval[0].isApprox(i.m_interval[0], epsilon) && m_interval[1].isApprox(i.m_interval[1], epsilon) &&
+ m_interval[2].isApprox(i.m_interval[2], epsilon));
+}
+
+template <class T>
+bool IntervalND<T, 3>::operator ==(const IntervalND<T, 3>& i) const
+{
+ return (m_interval[0] == i.m_interval[0] && m_interval[1] == i.m_interval[1] && m_interval[2] == i.m_interval[2]);
+}
+
+template <class T>
+bool IntervalND<T, 3>::operator !=(const IntervalND<T, 3>& i) const
+{
+ return !(this->operator==(i));
+}
+
+template <class T>
+IntervalND<T, 3>& IntervalND<T, 3>::addThickness(const T& thickness)
+{
+ m_interval[0].addThickness(thickness);
+ m_interval[1].addThickness(thickness);
+ m_interval[2].addThickness(thickness);
+ return *this;
+}
+
+template <class T>
+IntervalND<T, 3> IntervalND<T, 3>::operator +(const IntervalND<T, 3>& i) const
+{
+ return IntervalND<T, 3>(m_interval[0] + i.m_interval[0], m_interval[1] + i.m_interval[1],
+ m_interval[2] + i.m_interval[2]);
+}
+
+template <class T>
+IntervalND<T, 3>& IntervalND<T, 3>::operator +=(const IntervalND<T, 3>& i)
+{
+ m_interval[0] += i.m_interval[0];
+ m_interval[1] += i.m_interval[1];
+ m_interval[2] += i.m_interval[2];
+ return *this;
+}
+
+template <class T>
+IntervalND<T, 3> IntervalND<T, 3>::operator -(const IntervalND<T, 3>& i) const
+{
+ return IntervalND<T, 3>(m_interval[0] - i.m_interval[0], m_interval[1] - i.m_interval[1],
+ m_interval[2] - i.m_interval[2]);
+}
+
+template <class T>
+IntervalND<T, 3>& IntervalND<T, 3>::operator -=(const IntervalND<T, 3>& i)
+{
+ m_interval[0] -= i.m_interval[0];
+ m_interval[1] -= i.m_interval[1];
+ m_interval[2] -= i.m_interval[2];
+ return *this;
+}
+
+template <class T>
+IntervalND<T, 3> IntervalND<T, 3>::operator *(const IntervalND<T, 3>& i) const
+{
+ return IntervalND<T, 3>(m_interval[0] * i.m_interval[0], m_interval[1] * i.m_interval[1],
+ m_interval[2] * i.m_interval[2]);
+}
+
+template <class T>
+IntervalND<T, 3>& IntervalND<T, 3>::operator *=(const IntervalND<T, 3>& i)
+{
+ m_interval[0] *= i.m_interval[0];
+ m_interval[1] *= i.m_interval[1];
+ m_interval[2] *= i.m_interval[2];
+ return *this;
+}
+
+template <class T>
+IntervalND<T, 3> IntervalND<T, 3>::inverse() const
+{
+ return IntervalND<T, 3>(m_interval[0].inverse(), m_interval[1].inverse(), m_interval[2].inverse());
+}
+
+template <class T>
+IntervalND<T, 3> IntervalND<T, 3>::operator /(const IntervalND<T, 3>& i) const
+{
+ return IntervalND<T, 3>(m_interval[0] / i.m_interval[0], m_interval[1] / i.m_interval[1],
+ m_interval[2] / i.m_interval[2]);
+}
+
+template <class T>
+IntervalND<T, 3>& IntervalND<T, 3>::operator /=(const IntervalND<T, 3>& i)
+{
+ m_interval[0] /= i.m_interval[0];
+ m_interval[1] /= i.m_interval[1];
+ m_interval[2] /= i.m_interval[2];
+ return *this;
+}
+
+template <class T>
+Interval<T> IntervalND<T, 3>::dotProduct(const IntervalND<T, 3>& i) const
+{
+ return (m_interval[0] * i.m_interval[0] + m_interval[1] * i.m_interval[1] + m_interval[2] * i.m_interval[2]);
+}
+
+template <class T>
+IntervalND<T, 3> IntervalND<T, 3>::crossProduct(const IntervalND<T, 3>& i) const
+{
+ return IntervalND<T, 3>(m_interval[1] * i.m_interval[2] - m_interval[2] * i.m_interval[1],
+ m_interval[2] * i.m_interval[0] - m_interval[0] * i.m_interval[2],
+ m_interval[0] * i.m_interval[1] - m_interval[1] * i.m_interval[0]);
+}
+
+template <class T>
+Interval<T> IntervalND<T, 3>::magnitudeSquared() const
+{
+ return m_interval[0].square() + m_interval[1].square() + m_interval[2].square();
+}
+
+template <class T>
+Interval<T> IntervalND<T, 3>::magnitude() const
+{
+ Interval<T> magnitudeSq = magnitudeSquared();
+ // Both minimum and maximum are guaranteed to be non-negative.
+ return Interval<T>(sqrt(magnitudeSq.getMin()), sqrt(magnitudeSq.getMax()));
+}
+
+template <class T>
+const Interval<T>& IntervalND<T, 3>::getAxis(size_t i) const
+{
+ return m_interval[i];
+}
+
+// Utility functions not part of any class
+template <typename T>
+Interval<T> operator+ (T v, const Interval<T>& i)
+{
+ return i + v;
+}
+
+template <typename T>
+Interval<T> operator* (T v, const Interval<T>& i)
+{
+ return i * v;
+}
+
+template <class T>
+void IntervalArithmetic_add(const Interval<T>& a, const Interval<T>& b, Interval<T>* res)
+{
+ res->m_min = a.m_min + b.m_min;
+ res->m_max = a.m_max + b.m_max;
+}
+
+template <class T>
+void IntervalArithmetic_addadd(const Interval<T>& a, const Interval<T>& b, Interval<T>* res)
+{
+ res->m_min += a.m_min + b.m_min;
+ res->m_max += a.m_max + b.m_max;
+}
+
+template <class T>
+void IntervalArithmetic_sub(const Interval<T>& a, const Interval<T>& b, Interval<T>* res)
+{
+ res->m_min = a.m_min - b.m_max;
+ res->m_max = a.m_max - b.m_min;
+}
+
+template <class T>
+void IntervalArithmetic_addsub(const Interval<T>& a, const Interval<T>& b, Interval<T>* res)
+{
+ res->m_min += a.m_min - b.m_max;
+ res->m_max += a.m_max - b.m_min;
+}
+
+template <class T>
+void IntervalArithmetic_mul(const Interval<T>& a, const Interval<T>& b, Interval<T>* res)
+{
+ T min, max;
+ minMax(a.m_min * b.m_min, a.m_min * b.m_max, a.m_max * b.m_min, a.m_max * b.m_max, &min, &max);
+ res->m_min = min;
+ res->m_max = max;
+}
+
+template <class T>
+void IntervalArithmetic_addmul(const Interval<T>& a, const Interval<T>& b, Interval<T>* res)
+{
+ T min, max;
+ minMax(a.m_min * b.m_min, a.m_min * b.m_max, a.m_max * b.m_min, a.m_max * b.m_max, &min, &max);
+ res->m_min += min;
+ res->m_max += max;
+}
+
+template <class T>
+void IntervalArithmetic_submul(const Interval<T>& a, const Interval<T>& b, Interval<T>* res)
+{
+ T min, max;
+ minMax(a.m_min * b.m_min, a.m_min * b.m_max, a.m_max * b.m_min, a.m_max * b.m_max, &min, &max);
+ res->m_min -= max;
+ res->m_max -= min;
+}
+
+// Interval functions
+template <typename T>
+std::ostream& operator<< (std::ostream& o, const Interval<T>& interval)
+{
+ o << "[" << interval.getMin() << "," << interval.getMax() << "]";
+ return o;
+}
+
+// Interval ND functions
+template <typename T, int N>
+std::ostream& operator<< (std::ostream& o, const IntervalND<T, N>& interval)
+{
+ o << "(" << interval.getAxis(0);
+ for (int i = 1; i < N; ++i)
+ {
+ o << ";" << interval.getAxis(i);
+ }
+ o << ")";
+ return o;
+}
+
+// Interval 3D functions
+template <class T>
+void IntervalArithmetic_add(const IntervalND<T, 3>& a, const IntervalND<T, 3>& b, IntervalND<T, 3>* res)
+{
+ IntervalArithmetic_add(a.m_interval[0], b.m_interval[0], &(res->m_interval[0]));
+ IntervalArithmetic_add(a.m_interval[1], b.m_interval[1], &(res->m_interval[1]));
+ IntervalArithmetic_add(a.m_interval[2], b.m_interval[2], &(res->m_interval[2]));
+}
+
+template <class T>
+void IntervalArithmetic_sub(const IntervalND<T, 3>& a, const IntervalND<T, 3>& b, IntervalND<T, 3>* res)
+{
+ IntervalArithmetic_sub(a.m_interval[0], b.m_interval[0], &(res->m_interval[0]));
+ IntervalArithmetic_sub(a.m_interval[1], b.m_interval[1], &(res->m_interval[1]));
+ IntervalArithmetic_sub(a.m_interval[2], b.m_interval[2], &(res->m_interval[2]));
+}
+
+template <class T>
+void IntervalArithmetic_crossProduct(const IntervalND<T, 3>& a, const IntervalND<T, 3>& b, IntervalND<T, 3>* res)
+{
+ IntervalArithmetic_mul(a.m_interval[1], b.m_interval[2], &(res->m_interval[0]));
+ IntervalArithmetic_submul(a.m_interval[2], b.m_interval[1], &(res->m_interval[0]));
+
+ IntervalArithmetic_mul(a.m_interval[2], b.m_interval[0], &(res->m_interval[1]));
+ IntervalArithmetic_submul(a.m_interval[0], b.m_interval[2], &(res->m_interval[1]));
+
+ IntervalArithmetic_mul(a.m_interval[0], b.m_interval[1], &(res->m_interval[2]));
+ IntervalArithmetic_submul(a.m_interval[1], b.m_interval[0], &(res->m_interval[2]));
+}
+
+template <class T>
+void IntervalArithmetic_dotProduct(const IntervalND<T, 3>& a, const IntervalND<T, 3>& b, Interval<T>* res)
+{
+ IntervalArithmetic_mul(a.m_interval[0], b.m_interval[0], res);
+ IntervalArithmetic_addmul(a.m_interval[1], b.m_interval[1], res);
+ IntervalArithmetic_addmul(a.m_interval[2], b.m_interval[2], res);
+}
+
+}; // Math
+}; // SurgSim
+
+#endif // SURGSIM_MATH_INTERVALARITHMETIC_INL_H
diff --git a/SurgSim/Math/IntervalArithmetic.h b/SurgSim/Math/IntervalArithmetic.h
new file mode 100644
index 0000000..24ef29a
--- /dev/null
+++ b/SurgSim/Math/IntervalArithmetic.h
@@ -0,0 +1,568 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_INTERVALARITHMETIC_H
+#define SURGSIM_MATH_INTERVALARITHMETIC_H
+
+#include <array>
+#include <ostream>
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// Interval defines the concept of a mathematical interval and provides operations on it
+/// including arithmetic operations, construction, and IO.
+///
+/// \tparam T underlying data type over which the interval is defined.
+///
+/// \sa IntervalND<T, N> and IntervalND<T, 3>
+template <class T>
+class Interval
+{
+ template <class P>
+ friend void IntervalArithmetic_add(const Interval<P>& a, const Interval<P>& b, Interval<P>* res); // +
+ template <class P>
+ friend void IntervalArithmetic_addadd(const Interval<P>& a, const Interval<P>& b, Interval<P>* res); // +=( + )
+ template <class P>
+ friend void IntervalArithmetic_sub(const Interval<P>& a, const Interval<P>& b, Interval<P>* res); // -
+ template <class P>
+ friend void IntervalArithmetic_addsub(const Interval<P>& a, const Interval<P>& b, Interval<P>* res); // +=( - )
+ template <class P>
+ friend void IntervalArithmetic_mul(const Interval<P>& a, const Interval<P>& b, Interval<P>* res); // *
+ template <class P>
+ friend void IntervalArithmetic_addmul(const Interval<P>& a, const Interval<P>& b, Interval<P>* res); // += ( * )
+ template <class P>
+ friend void IntervalArithmetic_submul(const Interval<P>& a, const Interval<P>& b, Interval<P>* res); // -= ( * )
+
+public:
+ /// Constructor
+ Interval();
+
+ /// Constructor
+ /// \param min Lower bound of the constructed interval
+ /// \param max Upper bound of the constructed interval
+ /// \exception if max is less than min
+ Interval(const T& min, const T& max);
+
+ /// Copy constructor
+ /// \param i Interval to be copied
+ Interval(const Interval<T>& i);
+
+ /// Move constructor
+ /// \param i Interval to be copied
+ Interval(Interval<T>&& i);
+
+ /// Assignment operator
+ /// \param i Interval to be copied
+ Interval<T>& operator =(const Interval<T>& i);
+
+ /// Move assignment operator
+ /// \param i Interval to be moved
+ Interval<T>& operator=(Interval<T>&& i);
+
+ /// Generate an interval from min to max based on the inputs
+ /// \param a1 first input value
+ /// \param a2 second input value
+ /// \return an interval spanning the minimum input to the maximum input.
+ static Interval<T> minToMax(const T& a1, const T& a2);
+
+ /// Generate an interval from min to max based on the inputs
+ /// \param a1 first input value
+ /// \param a2 second input value
+ /// \param a3 third input value
+ /// \return an interval spanning the minimum input to the maximum input.
+ static Interval<T> minToMax(const T& a1, const T& a2, const T& a3);
+
+ /// Generate an interval from min to max based on the inputs
+ /// \param a1 first input value
+ /// \param a2 second input value
+ /// \param a3 third input value
+ /// \param a4 fourth input value
+ /// \return an interval spanning the minimum input to the maximum input.
+ static Interval<T> minToMax(const T& a1, const T& a2, const T& a3, const T& a4);
+
+ /// \param i the interval the current interval will be tested against
+ /// \return true if the input interval overlaps the current interval
+ bool overlapsWith(const Interval<T>& i) const;
+
+ /// \param val the value to test for inclusion in the interval
+ /// \return true if the current interval contains val
+ bool contains(const T& val) const;
+
+ /// \return true if the current interval contains 0
+ bool containsZero() const;
+
+ /// \param i the interval to be tested
+ /// \param epsilon the nearness parameter
+ /// \return true if the current interval is within epsilon of the input interval
+ bool isApprox(const Interval<T>& i, const T& epsilon) const;
+
+ /// \param i the interval to be tested
+ /// \return true if the current interval is identical to the input interval
+ bool operator ==(const Interval<T>& i) const;
+
+ /// \param i the interval to be tested
+ /// \return true if the current interval is not identical to the input interval
+ bool operator !=(const Interval<T>& i) const;
+
+ /// Widens the current interval by thickness on both sides
+ /// \param thickness the amount to widen the current interval on both sides
+ /// \return the current interval after modification
+ Interval<T>& addThickness(const T& thickness);
+
+ /// Widens the current interval on one end to include x
+ /// \param x the value to be included in the interval
+ /// \return the current interval extended to include x
+ Interval<T>& extendToInclude(const T& x);
+
+ /// Widens the current interval on both ends to include i
+ /// \param i the interval to be wholly contained in the current interval
+ /// \return the current interval extended to include the entirety of i
+ Interval<T>& extendToInclude(const Interval<T>& i);
+
+ /// @{
+ /// Standard arithmetic operators extended to intervals
+ Interval<T> operator +(const Interval<T>& i) const;
+ Interval<T> operator +(const T& v) const;
+ Interval<T>& operator +=(const Interval<T>& i);
+ Interval<T>& operator +=(const T& v);
+ Interval<T> operator -() const;
+ Interval<T> operator -(const Interval<T>& i) const;
+ Interval<T> operator -(const T& v) const;
+ Interval<T>& operator -=(const Interval<T>& i);
+ Interval<T>& operator -=(const T& v);
+ Interval<T> operator *(const Interval<T>& i) const;
+ Interval<T> operator *(const T& v) const;
+ Interval<T>& operator *=(const Interval<T>& i);
+ Interval<T>& operator *=(const T& v);
+ /// @}
+
+ /// \return the inverse of the current interval
+ /// \exception if the interval includes 0
+ Interval<T> inverse() const;
+
+ /// \param i the interval to be divided by
+ /// \return the current interval multiplied by the inverse of i
+ /// \exception if i includes 0
+ Interval<T> operator /(const Interval<T>& i) const;
+
+ /// \param i the interval to be divided by
+ /// \return the current interval multiplied by the inverse of i
+ /// \exception if i includes 0
+ /// \note the current interval is modified by this operation
+ Interval<T>& operator /=(const Interval<T>& i);
+
+ /// \return the square of the current interval
+ /// \note if the original interval contains 0, then the result will have the minimum identically set to 0
+ Interval<T> square() const;
+
+ /// \return the lower limit of the interval
+ T getMin() const;
+
+ /// \return the upper limit of the interval
+ T getMax() const;
+
+ /// \return the interval from the lower limit to the midpoint
+ Interval<T> lowerHalf() const;
+
+ /// \return the interval from the midpoint to the upper limit
+ Interval<T> upperHalf() const;
+
+private:
+ /// The lower (m_min) and upper (m_max) limits of the interval
+ T m_min, m_max;
+};
+
+
+/// IntervalND defines the concept of a group of mathematical intervals and provides operations on them
+/// including arithmetic operations, construction, and IO.
+///
+/// \tparam T underlying data type over which the interval is defined.
+/// \tparam N number of intervals in the group.
+///
+/// \sa Interval<T> and IntervalND<T, 3>
+template <class T, int N>
+class IntervalND
+{
+public:
+ static_assert(N >= 1, "IntervalND<T, N> cannot be instantiated with N<=0.");
+
+ /// Constructor
+ IntervalND();
+
+ /// Constructor
+ /// \param x array of N intervals to be copied into the group
+ explicit IntervalND(const std::array<Interval<T>, N>& x);
+
+ /// Copy constructor
+ /// \param interval interval group to copied
+ IntervalND(const IntervalND<T, N>& interval);
+
+ /// Move constructor
+ /// \param i Interval to be copied
+ IntervalND(IntervalND<T, N>&& i);
+
+ /// Constructor
+ /// \param a array of N values to be used as the respective minimums for the interval entries.
+ /// \param b array of N values to be used as the respective maximums for the interval entries.
+ IntervalND(const std::array<T, N>& a, const std::array<T, N>& b);
+
+ /// Assignment operator
+ /// \param interval Interval group to be copied
+ IntervalND<T, N>& operator =(const IntervalND<T, N>& interval);
+
+ /// Move assignment operator
+ /// \param i Interval to be moved
+ IntervalND<T, N>& operator=(IntervalND<T, N>&& i);
+
+ /// \param interval the interval group the current group will be tested against
+ /// \return true if the input group interval overlaps the current group
+ bool overlapsWith(const IntervalND<T, N>& interval) const;
+
+ /// \param interval the interval group to be tested
+ /// \param epsilon the nearness parameter
+ /// \return true if each interval in the input group is approximately equal to its correspondent
+ /// element in interval.
+ bool isApprox(const IntervalND<T, N>& interval, const T& epsilon) const;
+
+ /// \param interval the interval group to be tested
+ /// \return true if the current interval group is identical to the input group
+ bool operator ==(const IntervalND<T, N>& interval) const;
+
+ /// \param interval the interval group to be tested
+ /// \return true if the current interval group is not identical to the input group
+ bool operator !=(const IntervalND<T, N>& interval) const;
+
+ /// Widens every interval in the current interval group by thickness on both sides
+ /// \param thickness the amount to widen on both sides
+ /// \return the current interval group after modification
+ IntervalND<T, N>& addThickness(const T& thickness);
+
+ /// @{
+ /// Standard arithmetic operators extended to interval groups
+ IntervalND<T, N> operator +(const IntervalND<T, N>& interval) const;
+ IntervalND<T, N>& operator +=(const IntervalND<T, N>& interval);
+ IntervalND<T, N> operator -(const IntervalND<T, N>& interval) const;
+ IntervalND<T, N>& operator -=(const IntervalND<T, N>& interval);
+ IntervalND<T, N> operator *(const IntervalND<T, N>& interval) const;
+ IntervalND<T, N>& operator *=(const IntervalND<T, N>& interval);
+ /// @}
+
+ /// \return the inverse of each interval in the interval group
+ /// \exception if any interval includes 0
+ IntervalND<T, N> inverse() const;
+
+ /// \param interval the interval to be divided by
+ /// \return the product of each interval in the group multiplied by the inverse of
+ /// its correspondent in interval
+ /// \exception if any component of interval includes 0
+ IntervalND<T, N> operator /(const IntervalND<T, N>& interval) const;
+
+ /// \param interval the interval to be divided by
+ /// \return the product of each interval in the group multiplied by the inverse of
+ /// its correspondent in interval
+ /// \note the current interval is modified by this operation
+ IntervalND<T, N>& operator /=(const IntervalND<T, N>& interval);
+
+ /// \param interval the input interval group
+ /// \return the interval dot product of the current group and interval
+ Interval<T> dotProduct(const IntervalND<T, N>& interval) const;
+
+ /// \return the square of the interval magnitude for the current group
+ Interval<T> magnitudeSquared() const;
+
+ /// \return the interval magnitude for the current group
+ Interval<T> magnitude() const;
+
+ /// \param i the selector for the interval to be returned
+ /// \return the ith interval in the current group
+ const Interval<T>& getAxis(size_t i) const;
+
+private:
+ /// The N dimensional group of intervals
+ std::array<Interval<T>, N> m_interval;
+};
+
+/// IntervalND<T,3> defines the concept of a group of mathematical intervals specialized to 3 intervals and provides
+/// operations on them including arithmetic operations, construction, and IO.
+///
+/// \tparam T underlying data type over which the interval is defined.
+///
+/// \sa Interval<T> and IntervalND<T, N>
+template <class T>
+class IntervalND<T, 3>
+{
+ template <class P>
+ friend void IntervalArithmetic_add(const IntervalND<P, 3>& a, const IntervalND<P, 3>& b, IntervalND<P, 3>* res);
+ template <class P>
+ friend void IntervalArithmetic_sub(const IntervalND<P, 3>& a, const IntervalND<P, 3>& b, IntervalND<P, 3>* res);
+ template <class P>
+ friend void IntervalArithmetic_crossProduct(const IntervalND<P, 3>& a, const IntervalND<P, 3>& b,
+ IntervalND<P, 3>* res);
+ template <class P>
+ friend void IntervalArithmetic_dotProduct(const IntervalND<P, 3>& a, const IntervalND<P, 3>& b,
+ Interval<P>* res);
+
+public:
+ /// Constructor
+ IntervalND();
+
+ /// Constructor
+ /// \param x array of 3 intervals to be copied into the group
+ explicit IntervalND(const std::array<Interval<T>, 3>& x);
+
+ /// Constructor
+ /// \param x first interval to be added to the 3 group
+ /// \param y second interval to be added to the 3 group
+ /// \param z third interval to be added to the 3 group
+ IntervalND(const Interval<T>& x, const Interval<T>& y, const Interval<T>& z);
+
+ /// Copy constructor
+ /// \param i interval 3 group to copied
+ IntervalND(const IntervalND<T, 3>& i);
+
+ /// Move constructor
+ /// \param i Interval to be copied
+ IntervalND(IntervalND<T, 3>&& i);
+
+ /// Constructor
+ /// \param a array of 3 values to be used as the respective minimums for the interval entries.
+ /// \param b array of 3 values to be used as the respective maximums for the interval entries.
+ IntervalND(const std::array<T, 3>& a, const std::array<T, 3>& b);
+
+ /// Assignment operator
+ /// \param i Interval 3 group to be copied
+ IntervalND<T, 3>& operator =(const IntervalND<T, 3>& i);
+
+ /// Move assignment operator
+ /// \param i Interval to be moved
+ IntervalND<T, 3>& operator=(IntervalND<T, 3>&& i);
+
+ /// \param interval the interval group the current group will be tested against
+ /// \return true if the input 3 group interval overlaps the current 3 group
+ bool overlapsWith(const IntervalND<T, 3>& interval) const;
+
+ /// \param interval the interval group to be tested
+ /// \param epsilon the nearness parameter
+ /// \return true if each interval in the input group is approximately equal to its correspondent
+ /// element in interval.
+ bool isApprox(const IntervalND<T, 3>& interval, const T& epsilon) const;
+
+ /// \param i the interval group to be tested
+ /// \return true if the current interval 3 group is identical to the input 3 group i
+ bool operator ==(const IntervalND<T, 3>& i) const;
+
+ /// \param i the interval group to be tested
+ /// \return true if the current interval 3 group is not identical to the input 3 group i
+ bool operator !=(const IntervalND<T, 3>& i) const;
+
+ /// Widens every interval in the current interval group by thickness on both sides
+ /// \param thickness the amount to widen on both sides
+ /// \return the current interval group after modification
+ IntervalND<T, 3>& addThickness(const T& thickness);
+
+ /// @{
+ /// Standard arithmetic operators extended to 3 interval groups
+ IntervalND<T, 3> operator +(const IntervalND<T, 3>& i) const;
+ IntervalND<T, 3>& operator +=(const IntervalND<T, 3>& i);
+ IntervalND<T, 3> operator -(const IntervalND<T, 3>& i) const;
+ IntervalND<T, 3>& operator -=(const IntervalND<T, 3>& i);
+ IntervalND<T, 3> operator *(const IntervalND<T, 3>& i) const;
+ IntervalND<T, 3>& operator *=(const IntervalND<T, 3>& i);
+ /// @}
+
+ /// \return the inverse of each interval in the 3 interval group
+ /// \exception if any interval includes 0
+ IntervalND<T, 3> inverse() const;
+
+ /// \param i the interval to be divided by
+ /// \return the product of each interval in the 3 group multiplied by the inverse of
+ /// its correspondent in i
+ /// \exception if any component of interval includes 0
+ IntervalND<T, 3> operator /(const IntervalND<T, 3>& i) const;
+
+ /// \param i the interval to be divided by
+ /// \return the product of each interval in the 3 group multiplied by the inverse of
+ /// its correspondent in i
+ /// \note the current interval is modified by this operation
+ IntervalND<T, 3>& operator /=(const IntervalND<T, 3>& i);
+
+ /// \param i the input interval group
+ /// \return the interval dot product of the current 3 group and interval
+ Interval<T> dotProduct(const IntervalND<T, 3>& i) const;
+
+ /// \param i the input interval group
+ /// \return the interval cross product of the current 3 group and interval
+ IntervalND<T, 3> crossProduct(const IntervalND<T, 3>& i) const;
+
+ /// \return the square of the interval magnitude for the current 3 group
+ Interval<T> magnitudeSquared() const;
+
+ /// \return the interval magnitude for the current 3 group
+ Interval<T> magnitude() const;
+
+ /// \param i the selector for the interval to be returned
+ /// \return the ith interval in the current 3 group
+ const Interval<T>& getAxis(size_t i) const;
+
+private:
+ /// The 3 dimensional group of intervals
+ std::array<Interval<T>, 3> m_interval;
+};
+
+// Interval utilities
+
+/// \tparam T underlying type of the interval
+/// \param v the scalar
+/// \param i the interval
+/// \return the sum of the scalar v and the interval i
+template <typename T>
+Interval<T> operator+ (T v, const Interval<T>& i);
+
+/// \tparam T underlying type of the interval
+/// \param v the scalar
+/// \param i the interval
+/// \return the product of the scalar v and the interval i
+template <typename T>
+Interval<T> operator* (T v, const Interval<T>& i);
+
+/// Write a textual version of the interval to an output stream
+/// \tparam T underlying type of the interval
+/// \param o the ostream to be written to
+/// \param interval the interval to write
+/// \return the active ostream
+template <typename T>
+std::ostream& operator<< (std::ostream& o, const Interval<T>& interval);
+
+/// Calculate the sum of two intervals
+/// \tparam P underlying type of the interval
+/// \param a the first interval
+/// \param b the second interval
+/// \param res [out] the result of a + b
+template <class P>
+void IntervalArithmetic_add(const Interval<P>& a, const Interval<P>& b,
+ Interval<P>* res); // +
+
+/// Calculate the sum of three intervals res + a + b
+/// \tparam P underlying type of the interval
+/// \param a the first interval
+/// \param b the second interval
+/// \param res [in/out] the result of res + a + b
+template <class P>
+void IntervalArithmetic_addadd(const Interval<P>& a, const Interval<P>& b,
+ Interval<P>* res); // +=( + )
+
+/// Calculate the difference of two intervals
+/// \tparam P underlying type of the interval
+/// \param a the first interval
+/// \param b the second interval
+/// \param res [out] the result of a - b
+template <class P>
+void IntervalArithmetic_sub(const Interval<P>& a, const Interval<P>& b,
+ Interval<P>* res); // -
+
+/// Add the difference of two intervals to an existing value
+/// \tparam P underlying type of the interval
+/// \param a the first interval
+/// \param b the second interval
+/// \param res [in/out] the result of res + (a - b)
+template <class P>
+void IntervalArithmetic_addsub(const Interval<P>& a, const Interval<P>& b,
+ Interval<P>* res); // +=( - )
+
+/// Calculate the product of two intervals
+/// \tparam P underlying type of the interval
+/// \param a the first interval
+/// \param b the second interval
+/// \param res [out] the result of a * b
+template <class P>
+void IntervalArithmetic_mul(const Interval<P>& a, const Interval<P>& b,
+ Interval<P>* res); // *
+
+/// Add the product of two intervals to an existing value
+/// \tparam P underlying type of the interval
+/// \param a the first interval
+/// \param b the second interval
+/// \param res [in/out] the result of res + (a * b)
+template <class P>
+void IntervalArithmetic_addmul(const Interval<P>& a, const Interval<P>& b,
+ Interval<P>* res); // += ( * )
+
+/// Subtract the product of two intervals from an existing value
+/// \tparam P underlying type of the interval
+/// \param a the first interval
+/// \param b the second interval
+/// \param res [in/out] the result of res - (a * b)
+template <class P>
+void IntervalArithmetic_submul(const Interval<P>& a, const Interval<P>& b,
+ Interval<P>* res); // -= ( * )
+
+// Interval ND utilities
+
+/// Write a textual version of an interval group to an output stream
+/// \tparam T underlying type of the interval
+/// \tparam N number of intervals in the group
+/// \param o the ostream to be written to
+/// \param interval the interval group to write
+/// \return the active ostream
+template <typename T, int N>
+std::ostream& operator<< (std::ostream& o, const IntervalND<T, N>& interval);
+
+// Interval 3D utilities
+
+/// Calculate the sum of two interval groups
+/// \tparam P underlying type of the interval
+/// \param a the first interval group
+/// \param b the second interval group
+/// \param res [out] the result of a + b
+template <class P>
+void IntervalArithmetic_add(const IntervalND<P, 3>& a, const IntervalND<P, 3>& b,
+ IntervalND<P, 3>* res);
+
+/// Calculate the difference of two interval groups
+/// \tparam P underlying type of the interval
+/// \param a the first interval group
+/// \param b the second interval group
+/// \param res [out] the result of a - b
+template <class P>
+void IntervalArithmetic_sub(const IntervalND<P, 3>& a, const IntervalND<P, 3>& b,
+ IntervalND<P, 3>* res);
+
+/// Calculate the dot product of two interval groups
+/// \tparam P underlying type of the interval
+/// \param a the first interval group
+/// \param b the second interval group
+/// \param res [out] the dot product of a and b
+template <class P>
+void IntervalArithmetic_dotProduct(const IntervalND<P, 3>& a, const IntervalND<P, 3>& b,
+ Interval<P>* res);
+
+/// Calculate the cross product of two interval groups
+/// \tparam P underlying type of the interval
+/// \param a the first interval group
+/// \param b the second interval group
+/// \param res [out] the cross product of a and b
+template <class P>
+void IntervalArithmetic_crossProduct(const IntervalND<P, 3>& a, const IntervalND<P, 3>& b,
+ IntervalND<P, 3>* res);
+
+}; // Math
+}; // SurgSim
+
+#include "SurgSim/Math/IntervalArithmetic-inl.h"
+
+#endif // SURGSIM_MATH_INTERVALARITHMETIC_H
diff --git a/SurgSim/Math/KalmanFilter-inl.h b/SurgSim/Math/KalmanFilter-inl.h
new file mode 100644
index 0000000..d10e7c5
--- /dev/null
+++ b/SurgSim/Math/KalmanFilter-inl.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_MATH_KALMANFILTER_INL_H
+#define SURGSIM_MATH_KALMANFILTER_INL_H
+
+namespace SurgSim
+{
+namespace Math
+{
+
+template <size_t M, size_t N>
+KalmanFilter<M, N>::KalmanFilter() :
+ m_stateTransition(Eigen::Matrix<double, M, M, Eigen::RowMajor>::Constant(std::numeric_limits<double>::quiet_NaN())),
+ m_observationMatrix(Eigen::Matrix<double, N, M, Eigen::RowMajor>::Constant(
+ std::numeric_limits<double>::quiet_NaN())),
+ m_processNoiseCovariance(Eigen::Matrix<double, M, M, Eigen::RowMajor>::Constant(
+ std::numeric_limits<double>::quiet_NaN())),
+ m_measurementNoiseCovariance(Eigen::Matrix<double, N, N, Eigen::RowMajor>::Constant(
+ std::numeric_limits<double>::quiet_NaN())),
+ m_state(Eigen::Matrix<double, M, 1>::Constant(std::numeric_limits<double>::quiet_NaN())),
+ m_stateCovariance(Eigen::Matrix<double, M, M, Eigen::RowMajor>::Constant(std::numeric_limits<double>::quiet_NaN()))
+{
+}
+
+template <size_t M, size_t N>
+KalmanFilter<M, N>::~KalmanFilter()
+{
+}
+
+template <size_t M, size_t N>
+void KalmanFilter<M, N>::setInitialState(const Eigen::Ref<const Eigen::Matrix<double, M, 1>>& x)
+{
+ m_state = x;
+}
+
+template <size_t M, size_t N>
+void KalmanFilter<M, N>::setInitialStateCovariance(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& p)
+{
+ m_stateCovariance = p;
+}
+
+template <size_t M, size_t N>
+void KalmanFilter<M, N>::setStateTransition(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& a)
+{
+ m_stateTransition = a;
+}
+
+template <size_t M, size_t N>
+void KalmanFilter<M, N>::setObservationMatrix(const Eigen::Ref<const Eigen::Matrix<double, N, M>>& h)
+{
+ m_observationMatrix = h;
+}
+
+template <size_t M, size_t N>
+void KalmanFilter<M, N>::setProcessNoiseCovariance(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& q)
+{
+ m_processNoiseCovariance = q;
+}
+
+template <size_t M, size_t N>
+void KalmanFilter<M, N>::setMeasurementNoiseCovariance(const Eigen::Ref<const Eigen::Matrix<double, N, N>>& r)
+{
+ m_measurementNoiseCovariance = r;
+}
+
+template <size_t M, size_t N>
+const Eigen::Matrix<double, M, 1>& KalmanFilter<M, N>::update()
+{
+ updatePrediction();
+ return m_state;
+}
+
+template <size_t M, size_t N>
+const Eigen::Matrix<double, M, 1>&
+ KalmanFilter<M, N>::update(const Eigen::Ref<const Eigen::Matrix<double, N, 1>>& measurement)
+{
+ updatePrediction();
+ updateMeasurement(measurement);
+ return m_state;
+}
+
+template <size_t M, size_t N>
+const Eigen::Matrix<double, M, 1>& KalmanFilter<M, N>::getState() const
+{
+ return m_state;
+}
+
+template <size_t M, size_t N>
+void KalmanFilter<M, N>::updatePrediction()
+{
+ m_state = m_stateTransition * m_state;
+ m_stateCovariance = m_stateTransition * m_stateCovariance * m_stateTransition.transpose() +
+ m_processNoiseCovariance;
+}
+
+template <size_t M, size_t N>
+void KalmanFilter<M, N>::updateMeasurement(const Eigen::Ref<const Eigen::Matrix<double, N, 1>>& measurement)
+{
+ const Matrix gain = m_stateCovariance * m_observationMatrix.transpose() *
+ (m_observationMatrix * m_stateCovariance * m_observationMatrix.transpose() +
+ m_measurementNoiseCovariance).inverse();
+ m_state += gain * (measurement - m_observationMatrix * m_state);
+ m_stateCovariance -= gain * m_observationMatrix * m_stateCovariance;
+}
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_KALMANFILTER_INL_H
\ No newline at end of file
diff --git a/SurgSim/Math/KalmanFilter.h b/SurgSim/Math/KalmanFilter.h
new file mode 100644
index 0000000..dc3bb31
--- /dev/null
+++ b/SurgSim/Math/KalmanFilter.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_MATH_KALMANFILTER_H
+#define SURGSIM_MATH_KALMANFILTER_H
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// Implements a linear Kalman filter, a recursive estimator.
+/// Does not support control inputs.
+/// \tparam M The length of the state vector.
+/// \tparam N The length of the measurement vector.
+template <size_t M, size_t N>
+class KalmanFilter
+{
+public:
+ /// Constructor.
+ KalmanFilter();
+
+ /// Destructor.
+ virtual ~KalmanFilter();
+
+ /// Set the initial state vector, x(0), length m.
+ /// \param x The initial state.
+ void setInitialState(const Eigen::Ref<const Eigen::Matrix<double, M, 1>>& x);
+
+ /// Set the initial covariance of the state, P(0), size m x m.
+ /// \param p The initial covariance.
+ void setInitialStateCovariance(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& p);
+
+ /// Set the state transition, A, such that x(t+1) = A.x(t), size m x m.
+ /// \param a The state transition matrix.
+ void setStateTransition(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& a);
+
+ /// Set the observation matrix, H, such that z(t) = H.x(t), size n x m.
+ /// \param h The observation matrix.
+ void setObservationMatrix(const Eigen::Ref<const Eigen::Matrix<double, N, M>>& h);
+
+ /// Set the process noise covariance, size m x m.
+ /// \param q The process noise covariance.
+ void setProcessNoiseCovariance(const Eigen::Ref<const Eigen::Matrix<double, M, M>>& q);
+
+ /// Set the measurement noise covariance, size n x n.
+ /// \param r The measurement noise covariance.
+ void setMeasurementNoiseCovariance(const Eigen::Ref<const Eigen::Matrix<double, N, N>>& r);
+
+ /// Advance one step without a measurement.
+ /// \return The estimate for the new state, x(t+1), length m.
+ const Eigen::Matrix<double, M, 1>& update();
+
+ /// Advance one step with measurement.
+ /// \param measurement The measurement, z(t), length n.
+ /// \return The estimate for the new state, x(t+1), length m.
+ const Eigen::Matrix<double, M, 1>& update(const Eigen::Ref<const Eigen::Matrix<double, N, 1>>& measurement);
+
+ /// Get the current state. Does not advance the state.
+ /// \return The estimate for the current state, x(t), length m.
+ const Eigen::Matrix<double, M, 1>& getState() const;
+
+private:
+ /// Use the current estimated state, x(t), and matrices to predict the new state, x(t+1), and
+ /// state covariance, P(t+1).
+ void updatePrediction();
+
+ /// Correct the current estimated state, x(t), and state covariance, P(t), based on a measurement, z(t).
+ /// \param measurement The measurement, length n.
+ void updateMeasurement(const Eigen::Ref<const Eigen::Matrix<double, N, 1>>& measurement);
+
+ /// The state transition matrix.
+ Eigen::Matrix<double, M, M, Eigen::RowMajor> m_stateTransition;
+
+ /// The observation matrix.
+ Eigen::Matrix<double, N, M, Eigen::RowMajor> m_observationMatrix;
+
+ /// The process noise covariance.
+ Eigen::Matrix<double, M, M, Eigen::RowMajor> m_processNoiseCovariance;
+
+ /// The measurement noise covariance.
+ Eigen::Matrix<double, N, N, Eigen::RowMajor> m_measurementNoiseCovariance;
+
+ /// The state.
+ Eigen::Matrix<double, M, 1> m_state;
+
+ /// The covariance of the state.
+ Eigen::Matrix<double, M, M, Eigen::RowMajor> m_stateCovariance;
+};
+
+}; // Math
+}; // SurgSim
+
+#include "SurgSim/Math/KalmanFilter-inl.h"
+
+#endif // SURGSIM_MATH_KALMANFILTER_H
diff --git a/SurgSim/Math/LinearMotionArithmetic-inl.h b/SurgSim/Math/LinearMotionArithmetic-inl.h
new file mode 100644
index 0000000..fd5a1af
--- /dev/null
+++ b/SurgSim/Math/LinearMotionArithmetic-inl.h
@@ -0,0 +1,705 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_LINEARMOTIONARITHMETIC_INL_H
+#define SURGSIM_MATH_LINEARMOTIONARITHMETIC_INL_H
+
+namespace SurgSim
+{
+namespace Math
+{
+
+template <typename T>
+LinearMotion<T>::LinearMotion() : m_start(static_cast<T>(0)), m_end(static_cast<T>(0)) {}
+
+template <typename T>
+LinearMotion<T>::LinearMotion(const T& start, const T& end) : m_start(start), m_end(end) {}
+
+template <typename T>
+LinearMotion<T>::LinearMotion(const LinearMotion<T>& m) : m_start(m.m_start), m_end(m.m_end) {}
+
+template <typename T>
+LinearMotion<T>::LinearMotion(LinearMotion<T>&& m)
+{
+ *this = std::move(m);
+}
+
+template <typename T>
+LinearMotion<T>& LinearMotion<T>::operator=(const LinearMotion<T>& m)
+{
+ m_start = m.m_start;
+ m_end = m.m_end;
+ return *this;
+}
+
+template <typename T>
+LinearMotion<T>& LinearMotion<T>::operator=(LinearMotion<T>&& m)
+{
+ m_start = std::move(m.m_start);
+ m_end = std::move(m.m_end);
+ return *this;
+}
+
+template <typename T>
+Interval<T> LinearMotion<T>::toInterval() const
+{
+ return Interval<T>::minToMax(m_start, m_end);
+}
+
+template <typename T>
+Polynomial<T, 1> LinearMotion<T>::toPolynomial() const
+{
+ return Polynomial<T, 1>(m_start, m_end - m_start);
+}
+
+template <typename T>
+bool LinearMotion<T>::containsZero() const
+{
+ return toInterval().containsZero();
+}
+
+template <typename T>
+bool LinearMotion<T>::isApprox(const LinearMotion<T>& m, const T& epsilon) const
+{
+ return (std::abs(m_start - m.m_start) <= epsilon) && (std::abs(m_end - m.m_end) <= epsilon);
+}
+
+template <typename T>
+bool LinearMotion<T>::operator==(const LinearMotion<T>& m) const
+{
+ return ((m_start == m.m_start) && (m_end == m.m_end));
+}
+
+template <typename T>
+bool LinearMotion<T>::operator!=(const LinearMotion<T>& m) const
+{
+ return !(*this == m);
+}
+
+template <typename T>
+LinearMotion<T> LinearMotion<T>::operator+(const LinearMotion<T>& m) const
+{
+ return LinearMotion<T>(m_start + m.m_start, m_end + m.m_end);
+}
+
+template <typename T>
+LinearMotion<T>& LinearMotion<T>::operator+=(const LinearMotion<T>& m)
+{
+ m_start += m.m_start;
+ m_end += m.m_end;
+ return *this;
+}
+
+template <typename T>
+LinearMotion<T> LinearMotion<T>::operator-(const LinearMotion<T>& m) const
+{
+ return LinearMotion<T>(m_start - m.m_start, m_end - m.m_end);
+}
+
+template <typename T>
+LinearMotion<T>& LinearMotion<T>::operator-=(const LinearMotion<T>& m)
+{
+ m_start -= m.m_start;
+ m_end -= m.m_end;
+ return *this;
+}
+
+template <typename T>
+Interval<T> LinearMotion<T>::operator*(const LinearMotion<T>& m) const
+{
+ return this->toInterval() * m.toInterval();
+}
+
+template <typename T>
+Interval<T> LinearMotion<T>::operator/(const LinearMotion<T>& m) const
+{
+ return this->toInterval() / m.toInterval();
+}
+
+template <typename T>
+T LinearMotion<T>::getStart() const
+{
+ return m_start;
+}
+
+template <typename T>
+T LinearMotion<T>::getEnd() const
+{
+ return m_end;
+}
+
+template <typename T>
+T LinearMotion<T>::atTime(const T& t) const
+{
+ return ((static_cast<T>(1) - t) * m_start + t * m_end);
+}
+
+template <typename T>
+LinearMotion<T> LinearMotion<T>::firstHalf() const
+{
+ return LinearMotion<T>(m_start, (m_start + m_end) * static_cast<T>(0.5));
+}
+
+template <typename T>
+LinearMotion<T> LinearMotion<T>::secondHalf() const
+{
+ return LinearMotion<T>((m_start + m_end) * static_cast<T>(0.5), m_end);
+}
+
+// Class ND
+template <typename T, int N>
+LinearMotionND<T, N>::LinearMotionND()
+{
+}
+
+template <typename T, int N>
+LinearMotionND<T, N>::LinearMotionND(const std::array<LinearMotion<T>, N>& x)
+{
+ m_motion = x;
+}
+
+template <typename T, int N>
+LinearMotionND<T, N>::LinearMotionND(const LinearMotionND<T, N>& motion)
+{
+ m_motion = motion.m_motion;
+}
+
+template <typename T, int N>
+LinearMotionND<T, N>::LinearMotionND(LinearMotionND<T, N>&& motion)
+{
+ *this = std::move(motion);
+}
+
+template <typename T, int N>
+LinearMotionND<T, N>::LinearMotionND(const std::array<T, N>& a, const std::array<T, N>& b)
+{
+ for (int i = 0; i < N; ++i)
+ {
+ m_motion[i] = LinearMotion<T>(a[i], b[i]);
+ }
+}
+
+template <typename T, int N>
+LinearMotionND<T, N>& LinearMotionND<T, N>::operator=(const LinearMotionND<T, N>& motion)
+{
+ m_motion = motion.m_motion;
+ return *this;
+}
+
+template <typename T, int N>
+LinearMotionND<T, N>& LinearMotionND<T, N>::operator=(LinearMotionND<T, N>&& motion)
+{
+ if (this != &motion)
+ {
+ m_motion = std::move(motion.m_motion);
+ }
+
+ return *this;
+}
+
+template <typename T, int N>
+IntervalND<T, N> LinearMotionND<T, N>::toInterval() const
+{
+ std::array<Interval<T>, N> motions;
+ for (int i = 0; i < N; ++i)
+ {
+ motions[i] = m_motion[i].toInterval();
+ }
+
+ return IntervalND<T, N>(motions);
+}
+
+template <typename T, int N>
+bool LinearMotionND<T, N>::isApprox(const LinearMotionND<T, N>& motion, const T& epsilon) const
+{
+ for (int i = 0; i < N; i++)
+ {
+ if (!m_motion[i].isApprox(motion.m_interval[i], epsilon))
+ {
+ return false;
+ }
+ }
+ return true;
+}
+
+template <typename T, int N>
+bool LinearMotionND<T, N>::operator==(const LinearMotionND<T, N>& motion) const
+{
+ return (m_motion == motion.m_motion);
+}
+
+template <typename T, int N>
+bool LinearMotionND<T, N>::operator!=(const LinearMotionND<T, N>& motion) const
+{
+ return !(this->operator==(motion));
+}
+
+template <typename T, int N>
+LinearMotionND<T, N> LinearMotionND<T, N>::operator+(const LinearMotionND<T, N>& m) const
+{
+ LinearMotionND<T, N> ret(*this);
+ ret += m;
+ return ret;
+}
+
+template <typename T, int N>
+LinearMotionND<T, N>& LinearMotionND<T, N>::operator+=(const LinearMotionND<T, N>& m)
+{
+ for (int i = 0; i < N; ++i)
+ {
+ m_motion[i] += m.m_motion[i];
+ }
+ return *this;
+}
+
+template <typename T, int N>
+LinearMotionND<T, N> LinearMotionND<T, N>::operator-(const LinearMotionND<T, N>& m) const
+{
+ LinearMotionND<T, N> ret(*this);
+ ret -= m;
+ return ret;
+}
+
+template <typename T, int N>
+LinearMotionND<T, N>& LinearMotionND<T, N>::operator-=(const LinearMotionND<T, N>& m)
+{
+ for (int i = 0; i < N; ++i)
+ {
+ m_motion[i] -= m.m_motion[i];
+ }
+ return *this;
+}
+
+template <typename T, int N>
+IntervalND<T, N> LinearMotionND<T, N>::operator*(const LinearMotionND<T, N>& m) const
+{
+ return this->toInterval() * m.toInterval();
+}
+
+template <typename T, int N>
+IntervalND<T, N> LinearMotionND<T, N>::operator/(const LinearMotionND<T, N>& m) const
+{
+ return this->toInterval() / m.toInterval();
+}
+
+template <typename T, int N>
+Interval<T> LinearMotionND<T, N>::dotProduct(const LinearMotionND<T, N>& motion) const
+{
+ Interval<T> ret(static_cast<T>(0), static_cast<T>(0));
+ for (int i = 0 ; i < N ; i++)
+ {
+ ret += m_motion[i] * motion.m_motion[i];
+ }
+ return ret;
+}
+
+template <typename T, int N>
+const LinearMotion<T>& LinearMotionND<T, N>::getAxis(int i) const
+{
+ SURGSIM_ASSERT((i >= 0) && (i < N)) << "Asking for an axis greater than the dimensionality of the linear motion";
+ return m_motion[i];
+}
+
+template <typename T, int N>
+void LinearMotionND<T, N>::getStart(std::array<T, N>* start) const
+{
+ for (int i = 0; i < N; ++i)
+ {
+ (*start)[i] = m_motion[i].getStart();
+ }
+}
+
+template <typename T, int N>
+void LinearMotionND<T, N>::getEnd(std::array<T, N>* end) const
+{
+ for (int i = 0; i < N; ++i)
+ {
+ (*end)[i] = m_motion[i].getEnd();
+ }
+}
+
+template <typename T, int N>
+LinearMotionND<T, N> LinearMotionND<T, N>::firstHalf() const
+{
+ LinearMotionND<T, N> ret;
+ for (int i = 0; i < N; ++i)
+ {
+ ret.m_motion[i] = m_motion[i].firstHalf();
+ }
+ return ret;
+}
+
+template <typename T, int N>
+LinearMotionND<T, N> LinearMotionND<T, N>::secondHalf() const
+{
+ LinearMotionND<T, N> ret;
+ for (int i = 0; i < N; ++i)
+ {
+ ret.m_motion[i] = m_motion[i].secondHalf();
+ }
+ return ret;
+}
+
+// Special case for dimension 3
+template <typename T>
+LinearMotionND<T, 3>::LinearMotionND()
+{
+}
+
+template <typename T>
+LinearMotionND<T, 3>::LinearMotionND(const std::array<LinearMotion<T>, 3>& x)
+{
+ m_motion[0] = x[0];
+ m_motion[1] = x[1];
+ m_motion[2] = x[2];
+}
+
+template <typename T>
+LinearMotionND<T, 3>::LinearMotionND(const LinearMotion<T>& a, const LinearMotion<T>& b, const LinearMotion<T>& c)
+{
+ m_motion[0] = a;
+ m_motion[1] = b;
+ m_motion[2] = c;
+}
+
+template <typename T>
+LinearMotionND<T, 3>::LinearMotionND(const LinearMotionND<T, 3>& motion)
+{
+ m_motion[0] = motion.m_motion[0];
+ m_motion[1] = motion.m_motion[1];
+ m_motion[2] = motion.m_motion[2];
+}
+
+template <typename T>
+LinearMotionND<T, 3>::LinearMotionND(LinearMotionND<T, 3>&& motion)
+{
+ *this = std::move(motion);
+}
+
+template <typename T>
+LinearMotionND<T, 3>::LinearMotionND(const std::array<T, 3>& start, const std::array<T, 3>& end)
+{
+ m_motion[0] = LinearMotion<T>(start[0], end[0]);
+ m_motion[1] = LinearMotion<T>(start[1], end[1]);
+ m_motion[2] = LinearMotion<T>(start[2], end[2]);
+}
+
+template <typename T>
+LinearMotionND<T, 3>::LinearMotionND(const Vector3& start, const Vector3& end)
+{
+ m_motion[0] = LinearMotion<T>(start[0], end[0]);
+ m_motion[1] = LinearMotion<T>(start[1], end[1]);
+ m_motion[2] = LinearMotion<T>(start[2], end[2]);
+}
+
+template <typename T>
+LinearMotionND<T, 3>& LinearMotionND<T, 3>::operator=(const LinearMotionND<T, 3>& motion)
+{
+ m_motion[0] = motion.m_motion[0];
+ m_motion[1] = motion.m_motion[1];
+ m_motion[2] = motion.m_motion[2];
+ return *this;
+}
+
+template <typename T>
+LinearMotionND<T, 3>& LinearMotionND<T, 3>::operator=(LinearMotionND<T, 3>&& motion)
+{
+ m_motion[0] = std::move(motion.m_motion[0]);
+ m_motion[1] = std::move(motion.m_motion[1]);
+ m_motion[2] = std::move(motion.m_motion[2]);
+ return *this;
+}
+
+template <typename T>
+IntervalND<T, 3> LinearMotionND<T, 3>::toInterval() const
+{
+ std::array<Interval<T>, 3> intervals;
+ intervals[0] = m_motion[0].toInterval();
+ intervals[1] = m_motion[1].toInterval();
+ intervals[2] = m_motion[2].toInterval();
+ return IntervalND<T, 3>(intervals);
+}
+
+template <typename T>
+bool LinearMotionND<T, 3>::isApprox(const LinearMotionND<T, 3>& motion, const T& epsilon) const
+{
+ return (m_motion[0].isApprox(motion.m_motion[0], epsilon) &&
+ m_motion[1].isApprox(motion.m_motion[1], epsilon) &&
+ m_motion[2].isApprox(motion.m_motion[2], epsilon));
+}
+
+template <typename T>
+bool LinearMotionND<T, 3>::operator==(const LinearMotionND<T, 3>& motion) const
+{
+ return (m_motion[0] == motion.m_motion[0] &&
+ m_motion[1] == motion.m_motion[1] &&
+ m_motion[2] == motion.m_motion[2]);
+}
+
+template <typename T>
+bool LinearMotionND<T, 3>::operator!=(const LinearMotionND<T, 3>& motion) const
+{
+ return !(this->operator==(motion));
+}
+
+template <typename T>
+LinearMotionND<T, 3> LinearMotionND<T, 3>::operator+(const LinearMotionND<T, 3>& m) const
+{
+ LinearMotionND<T, 3> ret(*this);
+ ret += m;
+ return ret;
+}
+
+template <typename T>
+LinearMotionND<T, 3>& LinearMotionND<T, 3>::operator+=(const LinearMotionND<T, 3>& m)
+{
+ m_motion[0] += m.m_motion[0];
+ m_motion[1] += m.m_motion[1];
+ m_motion[2] += m.m_motion[2];
+ return *this;
+}
+
+template <typename T>
+LinearMotionND<T, 3> LinearMotionND<T, 3>::operator-(const LinearMotionND<T, 3>& m) const
+{
+ LinearMotionND<T, 3> ret(*this);
+ ret -= m;
+ return ret;
+}
+
+template <typename T>
+LinearMotionND<T, 3>& LinearMotionND<T, 3>::operator-=(const LinearMotionND<T, 3>& m)
+{
+ m_motion[0] -= m.m_motion[0];
+ m_motion[1] -= m.m_motion[1];
+ m_motion[2] -= m.m_motion[2];
+ return *this;
+}
+
+template <typename T>
+IntervalND<T, 3> LinearMotionND<T, 3>::operator*(const LinearMotionND<T, 3>& m) const
+{
+ return this->toInterval() * m.toInterval();
+}
+
+template <typename T>
+IntervalND<T, 3> LinearMotionND<T, 3>::operator/(const LinearMotionND<T, 3>& m) const
+{
+ return this->toInterval() / m.toInterval();
+}
+
+template <typename T>
+Interval<T> LinearMotionND<T, 3>::dotProduct(const LinearMotionND<T, 3>& motion, const Interval<T>& range) const
+{
+ return valuesOverInterval(analyticDotProduct(*this, motion), range);
+}
+
+template <typename T>
+IntervalND<T, 3> LinearMotionND<T, 3>::crossProduct(const LinearMotionND<T, 3>& motion,
+ const Interval<T>& range) const
+{
+ // toInterval().crossProduct(motion.toInterval())
+ // results in intervals that are too broad.
+ return IntervalND<T, 3>(valuesOverInterval(analyticCrossProductAxis<double, 0>(*this, motion), range),
+ valuesOverInterval(analyticCrossProductAxis<double, 1>(*this, motion), range),
+ valuesOverInterval(analyticCrossProductAxis<double, 2>(*this, motion), range));
+}
+
+template <typename T>
+Interval<T> LinearMotionND<T, 3>::magnitudeSquared(const Interval<T>& range) const
+{
+ return valuesOverInterval(analyticMagnitudeSquared(*this), range);
+}
+
+template <typename T>
+Interval<T> LinearMotionND<T, 3>::magnitude(const Interval<T>& range) const
+{
+ Interval<T> magnitudeSq = magnitudeSquared(range);
+ // Both minimum and maximum are guaranteed to be non-negative.
+ return Interval<T>(sqrt(magnitudeSq.getMin()), sqrt(magnitudeSq.getMax()));
+}
+
+template <typename T>
+const LinearMotion<T>& LinearMotionND<T, 3>::getAxis(int i) const
+{
+ SURGSIM_ASSERT((i >= 0) && (i < 3)) << "Asking for an axis greater than the dimensionality of the linear motion";
+ return m_motion[i];
+}
+
+template <typename T>
+void LinearMotionND<T, 3>::getStart(std::array<T, 3>* start) const
+{
+ (*start)[0] = m_motion[0].getStart();
+ (*start)[1] = m_motion[1].getStart();
+ (*start)[2] = m_motion[2].getStart();
+}
+
+template <typename T>
+void LinearMotionND<T, 3>::getEnd(std::array<T, 3>* end) const
+{
+ (*end)[0] = m_motion[0].getEnd();
+ (*end)[1] = m_motion[1].getEnd();
+ (*end)[2] = m_motion[2].getEnd();
+}
+
+template <typename T>
+typename LinearMotionND<T, 3>::Vector3 LinearMotionND<T, 3>::getStart() const
+{
+ return LinearMotionND<T, 3>::Vector3(m_motion[0].getStart(), m_motion[1].getStart(), m_motion[2].getStart());
+}
+
+template <typename T>
+typename LinearMotionND<T, 3>::Vector3 LinearMotionND<T, 3>::getEnd() const
+{
+ return Vector3(m_motion[0].getEnd(), m_motion[1].getEnd(), m_motion[2].getEnd());
+}
+
+template <typename T>
+typename LinearMotionND<T, 3>::Vector3 LinearMotionND<T, 3>::atTime(const T& t) const
+{
+ return Vector3(m_motion[0].atTime(t), m_motion[1].atTime(t), m_motion[2].atTime(t));
+}
+
+template <typename T>
+LinearMotionND<T, 3> LinearMotionND<T, 3>::firstHalf() const
+{
+ LinearMotionND<T, 3> ret;
+ ret.m_motion[0] = m_motion[0].firstHalf();
+ ret.m_motion[1] = m_motion[1].firstHalf();
+ ret.m_motion[2] = m_motion[2].firstHalf();
+ return ret;
+}
+
+template <typename T>
+LinearMotionND<T, 3> LinearMotionND<T, 3>::secondHalf() const
+{
+ LinearMotionND<T, 3> ret;
+ ret.m_motion[0] = m_motion[0].secondHalf();
+ ret.m_motion[1] = m_motion[1].secondHalf();
+ ret.m_motion[2] = m_motion[2].secondHalf();
+ return ret;
+}
+
+// Utility functions not part of any class
+
+// Interval functions
+template <typename T>
+std::ostream& operator<<(std::ostream& o, const LinearMotion<T>& motion)
+{
+ o << "(" << motion.getStart() << " -> " << motion.getEnd() << ")";
+ return o;
+}
+
+// Interval ND functions
+template <typename T, int N>
+std::ostream& operator<<(std::ostream& o, const LinearMotionND<T, N>& motion)
+{
+ o << "([" << motion.getAxis(0).getStart();
+ for (int i = 1; i < N; ++i)
+ {
+ o << "," << motion.getAxis(i).getStart();
+ }
+ o << "] -> [" << motion.getAxis(0).getEnd();
+ for (int i = 1; i < N; ++i)
+ {
+ o << "," << motion.getAxis(i).getEnd();
+ }
+ o << "])";
+ return o;
+}
+
+// Interval 3D functions
+template <typename T>
+Polynomial<T, 2> analyticDotProduct(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b)
+{
+ return a.getAxis(0).toPolynomial() * b.getAxis(0).toPolynomial() +
+ a.getAxis(1).toPolynomial() * b.getAxis(1).toPolynomial() +
+ a.getAxis(2).toPolynomial() * b.getAxis(2).toPolynomial();
+}
+
+template <typename T, int A>
+Polynomial<T, 2> analyticCrossProductAxis(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b)
+{
+ // The labels here are probably a bit confusing for anyone else, but at least this makes sense.
+ // For A == 0, the "Y" and "Z" mean what they say, and the output is the X component.
+ // For A == 1, they get rotated "down" by one (so Y -> Z, Z -> X), and the output is the Y component.
+ // For A == 2, they get rotated "down" by two (so Y -> X, Z -> Y), and the output is the Z component.
+ const LinearMotion<T>& aY = a.getAxis((A + 1) % 3);
+ const LinearMotion<T>& aZ = a.getAxis((A + 2) % 3);
+ const LinearMotion<T>& bY = b.getAxis((A + 1) % 3);
+ const LinearMotion<T>& bZ = b.getAxis((A + 2) % 3);
+ return aY.toPolynomial() * bZ.toPolynomial() - aZ.toPolynomial() * bY.toPolynomial();
+}
+
+template <typename T>
+Polynomial<T, 2> analyticCrossProductXAxis(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b)
+{
+ return analyticCrossProductAxis<double, 0>(a, b);
+}
+
+template <typename T>
+Polynomial<T, 2> analyticCrossProductYAxis(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b)
+{
+ return analyticCrossProductAxis<double, 1>(a, b);
+}
+
+template <typename T>
+Polynomial<T, 2> analyticCrossProductZAxis(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b)
+{
+ return analyticCrossProductAxis<double, 2>(a, b);
+}
+
+template <typename T>
+void analyticCrossProduct(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b,
+ Polynomial<T, 2>* resultXAxis, Polynomial<T, 2>* resultYAxis, Polynomial<T, 2>* resultZAxis)
+{
+ (*resultXAxis) = analyticCrossProductXAxis(a, b);
+ (*resultYAxis) = analyticCrossProductYAxis(a, b);
+ (*resultZAxis) = analyticCrossProductZAxis(a, b);
+}
+
+template <typename T>
+Polynomial <T, 3> analyticTripleProduct(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b,
+ const LinearMotionND<T, 3>& c)
+{
+ const Polynomial<T, 1> aX = a.getAxis(0).toPolynomial();
+ const Polynomial<T, 1> aY = a.getAxis(1).toPolynomial();
+ const Polynomial<T, 1> aZ = a.getAxis(2).toPolynomial();
+ const Polynomial<T, 1> bX = b.getAxis(0).toPolynomial();
+ const Polynomial<T, 1> bY = b.getAxis(1).toPolynomial();
+ const Polynomial<T, 1> bZ = b.getAxis(2).toPolynomial();
+ const Polynomial<T, 1> cX = c.getAxis(0).toPolynomial();
+ const Polynomial<T, 1> cY = c.getAxis(1).toPolynomial();
+ const Polynomial<T, 1> cZ = c.getAxis(2).toPolynomial();
+ return ((bY * cZ - bZ * cY) * aX + (bZ * cX - bX * cZ) * aY + (bX * cY - bY * cX) * aZ);
+}
+
+template <typename T>
+static Interval<T> tripleProduct(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b,
+ const LinearMotionND<T, 3>& c, const Interval<T>& range)
+{
+ return valuesOverInterval(analyticTripleProduct(a, b, c), range);
+}
+
+template <typename T>
+Polynomial<T, 2> analyticMagnitudeSquared(const LinearMotionND<T, 3>& motion)
+{
+ return analyticDotProduct(motion, motion);
+}
+
+}; // Math
+}; // SurgSim
+
+#endif // SURGSIM_MATH_LINEARMOTIONARITHMETIC_INL_H
diff --git a/SurgSim/Math/LinearMotionArithmetic.h b/SurgSim/Math/LinearMotionArithmetic.h
new file mode 100644
index 0000000..afaf03e
--- /dev/null
+++ b/SurgSim/Math/LinearMotionArithmetic.h
@@ -0,0 +1,499 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_LINEARMOTIONARITHMETIC_H
+#define SURGSIM_MATH_LINEARMOTIONARITHMETIC_H
+
+#include <array>
+#include <iostream>
+
+#include <Eigen/Core>
+
+#include "SurgSim/Math/IntervalArithmetic.h"
+#include "SurgSim/Math/PolynomialValues.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// LinearMotion is (intentionally) a lot like Interval, but it deals with linear motion where
+/// all operands start and end their motion simultaneously.
+///
+/// LinearMotion results in much tighter bounds compared to Interval, since Interval must consider
+/// *any* value of each operand, and LinearMotion only considers values that are synchronous with
+/// one another.
+///
+/// The bounds of a LinearMotion are a start value and an end value; there's no requirement that
+/// start <= end (or vice versa).
+///
+/// Many operations on LinearMotion arguments (*, /) return results that are not linear in time,
+/// so those operations will return Interval instead.
+///
+/// \tparam T underlying data type over which the linear motion is defined.
+///
+/// \sa Interval<T>, and IntervalND<T, N>
+template <typename T>
+class LinearMotion
+{
+public:
+ /// Constructor
+ LinearMotion();
+
+ /// Constructor
+ /// \param start initial value of the constructed linear motion
+ /// \param end final value of the constructed linear motion
+ /// \exception if max is less than min
+ LinearMotion(const T& start, const T& end);
+
+ /// Copy constructor
+ /// \param m Interval to be copied
+ LinearMotion(const LinearMotion<T>& m);
+
+ /// Move constructor
+ /// \param m LinearMotion to be copied
+ LinearMotion(LinearMotion<T>&& m);
+
+ /// Assignment operator
+ /// \param m Interval to be copied
+ LinearMotion<T>& operator=(const LinearMotion<T>& m);
+
+ /// Move assignment operator
+ /// \param m Interval to be moved
+ LinearMotion<T>& operator=(LinearMotion<T>&& m);
+
+ /// Convert from LinearMotion to an Interval
+ /// \return the conversion of the LinearMotion to an Interval
+ Interval<T> toInterval() const;
+
+ /// Returns a linear expression (degree-1 polynomial) whose value for t=0..1
+ /// progresses from `start' to `end'.
+ /// \return the conversion of the LinearMotion to a polynomial
+ Polynomial<T, 1> toPolynomial() const;
+
+ /// \return true if the linear motion crosses through 0.
+ bool containsZero() const;
+
+ /// \param i the linear motion to be tested
+ /// \param epsilon the nearness parameter
+ /// \return true if the current linear motion is within epsilon of the input linear motion
+ bool isApprox(const LinearMotion<T>& i, const T& epsilon) const;
+
+ /// \param m the linear motion to be tested
+ /// \return true if the current linear motion is identical to the input linear motion
+ bool operator==(const LinearMotion<T>& m) const;
+
+ /// \param m the linear motion to be tested
+ /// \return true if the current linear motion is not identical to the input linear motion
+ bool operator!=(const LinearMotion<T>& m) const;
+
+ /// @{
+ /// Standard arithmetic operators extended to linear motions
+ LinearMotion<T> operator+(const LinearMotion<T>& m) const;
+ LinearMotion<T>& operator+=(const LinearMotion<T>& m);
+ LinearMotion<T> operator-(const LinearMotion<T>& m) const;
+ LinearMotion<T>& operator-=(const LinearMotion<T>& m);
+ /// @}
+
+ /// Standard arithmetic operators extended to interval groups
+ /// \note Multiplication and division operators by their nature do not
+ /// preserve time ordering and so the return value is an IntervalND instead
+ /// of a LinearMotionND
+ Interval<T> operator*(const LinearMotion<T>& m) const;
+
+ /// Standard arithmetic operators extended to interval groups
+ /// \note Multiplication and division operators by their nature do not
+ /// preserve time ordering and so the return value is an IntervalND instead
+ /// of a LinearMotionND
+ /// \exception if any component of interval includes 0
+ Interval<T> operator/(const LinearMotion<T>& m) const;
+
+ /// \return the initial value of the linear motion
+ T getStart() const;
+
+ /// \return the end point of the linear motion
+ T getEnd() const;
+
+ /// \param t the parametric time at which to evaluate the linear motion
+ /// \return the value of the linear motion at parametric time t
+ T atTime(const T& t) const;
+
+ /// \return the linear motion from the initial time to the midpoint
+ LinearMotion<T> firstHalf() const;
+
+ /// \return the linear motion from the midpoint to the endpoint
+ LinearMotion<T> secondHalf() const;
+
+private:
+ /// The start point of the linear motion
+ T m_start;
+
+ /// The end point of the linear motion
+ T m_end;
+};
+
+/// LinearMotionND<T, N> defines the concept of a group of linear motions and provides
+/// operations on them including arithmetic operations, construction, and I/O.
+///
+/// \tparam T underlying data type over which the linear motion is defined.
+/// \tparam N Dimensionality of the interval
+///
+/// \sa LinearMotionND<T> and IntervalArthmetic<T, N>
+template <class T, int N>
+class LinearMotionND
+{
+ static_assert(N > 0, "LinearMotion must have dimensionality > 0.");
+
+public:
+ /// Constructor
+ LinearMotionND();
+
+ /// Constructor
+ /// \param x array of N motions to be copied into the group
+ explicit LinearMotionND(const std::array<LinearMotion<T>, N>& x);
+
+ /// Copy constructor
+ /// \param motion motion group to copied
+ LinearMotionND(const LinearMotionND<T, N>& motion);
+
+ /// Move constructor
+ /// \param motion motion to be copied
+ LinearMotionND(LinearMotionND<T, N>&& motion);
+
+ /// Constructor
+ /// \param a array of N values to be used as the respective starts for the linear motion entries.
+ /// \param b array of N values to be used as the respective ends for the linear motion entries.
+ LinearMotionND(const std::array<T, N>& a, const std::array<T, N>& b);
+
+ /// Assignment operator
+ /// \param motion Linear motion group to be copied
+ LinearMotionND<T, N>& operator=(const LinearMotionND<T, N>& motion);
+
+ /// Move assignment operator
+ /// \param motion Linear motion group to be moved
+ LinearMotionND<T, N>& operator=(LinearMotionND<T, N>&& motion);
+
+ /// Convert from LinearMotion to an Interval
+ /// \return the conversion of the ND LinearMotion to an ND Interval
+ IntervalND<T, N> toInterval() const;
+
+ /// \param motion the linear motion group to be tested
+ /// \param epsilon the nearness parameter
+ /// \return true if each linear motion in the input group is approximately equal to its correspondent
+ /// element in motion.
+ bool isApprox(const LinearMotionND<T, N>& motion, const T& epsilon) const;
+
+ /// \param motion the linear motion group to be tested
+ /// \return true if the current linear motion group is identical to the input group
+ bool operator==(const LinearMotionND<T, N>& motion) const;
+
+ /// \param motion the linear motion group to be tested
+ /// \return true if the current linear motion group is not identical to the input group
+ bool operator!=(const LinearMotionND<T, N>& motion) const;
+
+ /// @{
+ /// Standard arithmetic operators extended to interval groups
+ LinearMotionND<T, N> operator+(const LinearMotionND<T, N>& m) const;
+ LinearMotionND<T, N>& operator+=(const LinearMotionND<T, N>& m);
+ LinearMotionND<T, N> operator-(const LinearMotionND<T, N>& m) const;
+ LinearMotionND<T, N>& operator-=(const LinearMotionND<T, N>& m);
+ /// @}
+
+ /// Standard arithmetic operators extended to interval groups
+ /// \note Multiplication and division operators by their nature do not
+ /// preserve time ordering and so the return value is an IntervalND instead
+ /// of a LinearMotionND
+ IntervalND<T, N> operator*(const LinearMotionND<T, N>& m) const;
+
+ /// Standard arithmetic operators extended to interval groups
+ /// \note Multiplication and division operators by their nature do not
+ /// preserve time ordering and so the return value is an IntervalND instead
+ /// of a LinearMotionND
+ /// \exception if any component of interval includes 0
+ IntervalND<T, N> operator/(const LinearMotionND<T, N>& m) const;
+
+ /// \param motion the input linear motion group
+ /// \return the interval dot product of the current group and interval
+ Interval<T> dotProduct(const LinearMotionND<T, N>& motion) const;
+
+ /// \param i the selector for the linear motion to be returned
+ /// \return the ith interval in the current group
+ /// \exception thrown if the requested axis is < 0 or greater than N - 1
+ const LinearMotion<T>& getAxis(int i) const;
+
+ /// \param start [out] the starting points of the linear motion group as an N dimension array.
+ void getStart(std::array<T, N>* start) const;
+
+ /// \param end [out] the ending points of the linear motion group as an N dimension array.
+ void getEnd(std::array<T, N>* end) const;
+
+ /// \return the linear motion from the starting point to the midpoint
+ LinearMotionND<T, N> firstHalf() const;
+
+ /// \return the linear motion from the midpoint to the ending point
+ LinearMotionND<T, N> secondHalf() const;
+
+private:
+ /// The N dimensional group of linear motions
+ std::array<LinearMotion<T>, N> m_motion;
+};
+
+/// LinearMotionND<T, 3> specializes the LinearMotionND<T, N> class for 3 dimensions
+///
+/// \sa LinearMotion<T>, LinearMotionND<T, N> and IntervalArthmetic<T, 3>
+template <class T>
+class LinearMotionND<T, 3>
+{
+public:
+ /// Typedef for a vector 3 return
+ typedef Eigen::Matrix<T, 3, 1> Vector3;
+
+ /// Constructor
+ LinearMotionND();
+
+ /// Constructor
+ /// \param x array of 3 linear motions to be copied into the group
+ explicit LinearMotionND(const std::array<LinearMotion<T>, 3>& x);
+
+ /// Constructor
+ /// \param a first linear motion to be added to the 3 group
+ /// \param b second linear motion to be added to the 3 group
+ /// \param c third linear motion to be added to the 3 group
+ LinearMotionND(const LinearMotion<T>& a, const LinearMotion<T>& b, const LinearMotion<T>& c);
+
+ /// Copy constructor
+ /// \param motion linear motion 3 group to be copied
+ LinearMotionND(const LinearMotionND<T, 3>& motion);
+
+ /// Move constructor
+ /// \param motion Linear motion to be copied
+ LinearMotionND(LinearMotionND<T, 3>&& motion);
+
+ /// Constructor
+ /// \param start array of 3 values to be used as the respective starts for the linear motion entries.
+ /// \param end array of 3 values to be used as the respective ends for the linear motion entries.
+ LinearMotionND(const std::array<T, 3>& start, const std::array<T, 3>& end);
+
+ /// Constructor
+ /// \param start array of 3 values to be used as the respective starts for the linear motion entries.
+ /// \param end array of 3 values to be used as the respective ends for the linear motion entries.
+ LinearMotionND(const Vector3& start, const Vector3& end);
+
+ /// Assignment operator
+ /// \param motion Linear motion 3 group to be copied
+ LinearMotionND<T, 3>& operator=(const LinearMotionND<T, 3>& motion);
+
+ /// Move assignment operator
+ /// \param motion Linear motion 3 group to be moved
+ LinearMotionND<T, 3>& operator=(LinearMotionND<T, 3>&& motion);
+
+ /// Convert from LinearMotion to an Interval
+ /// \return the conversion of the 3D LinearMotion to a 3D Interval
+ IntervalND<T, 3> toInterval() const;
+
+ /// \param motion the motion group to be tested
+ /// \param epsilon the nearness parameter
+ /// \return true if each linear motion in the input group is approximately equal to its correspondent
+ /// element in motion.
+ bool isApprox(const LinearMotionND<T, 3>& motion, const T& epsilon) const;
+
+ /// \param motion the linear motion group to be tested
+ /// \return true if the current linear motion 3 group is identical to the input 3 group motion
+ bool operator==(const LinearMotionND<T, 3>& motion) const;
+
+ /// \param motion the linear motion group to be tested
+ /// \return true if the current linear motion 3 group is not identical to the input 3 group motion.
+ bool operator!=(const LinearMotionND<T, 3>& motion) const;
+
+ /// @{
+ /// Standard arithmetic operators extended to 3 interval groups
+ LinearMotionND<T, 3> operator+(const LinearMotionND<T, 3>& m) const;
+ LinearMotionND<T, 3>& operator+=(const LinearMotionND<T, 3>& m);
+ LinearMotionND<T, 3> operator-(const LinearMotionND<T, 3>& m) const;
+ LinearMotionND<T, 3>& operator-=(const LinearMotionND<T, 3>& m);
+ /// @}
+
+ /// Standard arithmetic operators extended to interval groups
+ /// \note Multiplication and division operators by their nature do not
+ /// preserve time ordering and so the return value is an IntervalND instead
+ /// of a LinearMotionND
+ IntervalND<T, 3> operator*(const LinearMotionND<T, 3>& m) const;
+
+ /// Standard arithmetic operators extended to interval groups
+ /// \note Multiplication and division operators by their nature do not
+ /// preserve time ordering and so the return value is an IntervalND instead
+ /// of a LinearMotionND
+ /// \exception if any component of interval includes 0
+ IntervalND<T, 3> operator/(const LinearMotionND<T, 3>& m) const;
+
+ /// \param motion the input linear motion 3 group
+ /// \param range the range over which the dot product is to be evaluated.
+ /// \return the interval dot product of the current 3 group and interval evaluated over the interval range.
+ Interval<T> dotProduct(const LinearMotionND<T, 3>& motion, const Interval<T>& range) const;
+
+ /// \param motion the input linear motion 3 group
+ /// \param range the range over which the cross product is to be evaluated.
+ /// \return the interval cross product of the current 3 group and interval evaluated over the interval range.
+ IntervalND<T, 3> crossProduct(const LinearMotionND<T, 3>& motion, const Interval<T>& range) const;
+
+ /// \return the square of the linear motion magnitude for the current 3 group
+ Interval<T> magnitudeSquared(const Interval<T>& range) const;
+
+ /// \return the linear motion magnitude for the current 3 group
+ Interval<T> magnitude(const Interval<T>& range) const;
+
+ /// \param i the selector for the linear motion to be returned
+ /// \return the ith linear motion in the current 3 group
+ /// \exception thrown if the requested axis is < 0 or greater than 2
+ const LinearMotion<T>& getAxis(int i) const;
+
+ /// \param start [out] the start of the linear motion 3D as a 3 value array
+ void getStart(std::array<T, 3>* start) const;
+
+ /// \param end [out] the end of the linear motion 3D as a 3 value array
+ void getEnd(std::array<T, 3>* end) const;
+
+ /// \return the start of the linear motion 3D as a 3 Vector
+ Vector3 getStart() const;
+
+ /// \return the end of the linear motion 3D as a 3 Vector
+ Vector3 getEnd() const;
+
+ /// \param t the parametric value at which to evaluate the linear motion
+ /// \return the value of the linear motion 3D at time t as a 3 Vector
+ Vector3 atTime(const T& t) const;
+
+ /// \return the linear motion 3D from the start to the midpoint
+ LinearMotionND<T, 3> firstHalf() const;
+
+ /// \return the linear motion 3D from the midpoint to the start
+ LinearMotionND<T, 3> secondHalf() const;
+
+private:
+ /// The 3 dimensional group of linear motions
+ std::array<LinearMotion<T>, 3> m_motion;
+};
+
+// Linear motion utilities
+
+/// Write a textual version of a linear motion to an output stream
+/// \tparam T underlying type of the linear motion
+/// \param o the ostream to be written to
+/// \param motion the motion to write
+/// \return the active ostream
+template <typename T>
+std::ostream& operator<<(std::ostream& o, const LinearMotion<T>& motion);
+
+// Linear motion ND utilities
+
+/// Write a textual version of a linear motion group to an output stream
+/// \tparam T underlying type of the linear motion
+/// \tparam N number of linear motions in the group
+/// \param o the ostream to be written to
+/// \param motion the motion group to write
+/// \return the active ostream
+template <typename T, int N>
+std::ostream& operator<<(std::ostream& o, const LinearMotionND<T, N>& motion);
+
+// Linear motion 3D utilities
+
+/// Calculate an analytic dot product as a Polynomial
+/// \tparam T underlying type of the linear motion
+/// \param a the first linear motion 3 group
+/// \param b the second linear motion 3 group
+/// \return the dot product in a polynomial representation
+template <class T>
+Polynomial<T, 2> analyticDotProduct(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b);
+
+/// Calculate a single axis of an analytic cross product as a Polynomial
+/// \tparam T underlying type of the linear motion
+/// \tparam A axis to generate: 0 = X, 1=Y, 2=Z
+/// \param a the first linear motion 3 group
+/// \param b the second linear motion 3 group
+/// \return the selected axis in a polynomial representation
+template <class T, int A>
+Polynomial<T, 2> analyticCrossProductAxis(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b);
+
+/// Calculate the X axis of an analytic cross product as a Polynomial
+/// \tparam T underlying type of the linear motion
+/// \param a the first linear motion 3 group
+/// \param b the second linear motion 3 group
+/// \return the X axis in a polynomial representation
+template <class T>
+Polynomial<T, 2> analyticCrossProductXAxis(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b);
+
+/// Calculate the Y axis of an analytic cross product as a Polynomial
+/// \tparam T underlying type of the linear motion
+/// \param a the first linear motion 3 group
+/// \param b the second linear motion 3 group
+/// \return the Y axis in a polynomial representation
+template <class T>
+Polynomial<T, 2> analyticCrossProductYAxis(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b);
+
+/// Calculate the Z axis of an analytic cross product as a Polynomial
+/// \tparam T underlying type of the linear motion
+/// \param a the first linear motion 3 group
+/// \param b the second linear motion 3 group
+/// \return the Z axis in a polynomial representation
+template <class T>
+Polynomial<T, 2> analyticCrossProductZAxis(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b);
+
+/// Calculate an analytic cross product as a Polynomial
+/// \tparam T underlying type of the linear motion
+/// \param a the first linear motion 3 group
+/// \param b the second linear motion 3 group
+/// \param [out] resultXAxis the X axis in a polynomial representation
+/// \param [out] resultYAxis the Y axis in a polynomial representation
+/// \param [out] resultZAxis the Z axis in a polynomial representation
+template <class T>
+void analyticCrossProduct(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b,
+ Polynomial<T, 2>* resultXAxis, Polynomial<T, 2>* resultYAxis, Polynomial<T, 2>* resultZAxis);
+
+/// Calculate an analytic cross product as a Polynomial, as a polynomial whose value for t=0..1 is
+/// the value of the triple product.
+/// \tparam T underlying type of the linear motion
+/// \param a the first linear motion 3 group
+/// \param b the second linear motion 3 group
+/// \param c the third linear motion 3 group
+/// \return a 3rd order polynomial representation of the triple product
+template <class T>
+Polynomial<T, 3> analyticTripleProduct(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b,
+ const LinearMotionND<T, 3>& c);
+
+/// Calculate the triple product, as an interval.
+/// \tparam T underlying type of the linear motion
+/// \param a the first linear motion 3 group
+/// \param b the second linear motion 3 group
+/// \param c the third linear motion 3 group
+/// \param range the range over which the triple product is to be evaluated
+/// \return an interval representation of the triple product
+template <class T>
+Interval<T> tripleProduct(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b,
+ const LinearMotionND<T, 3>& c, const Interval<T>& range);
+
+/// Calculate the magnitude squared of a linear motion 3 group as a polynomial
+/// \tparam T underlying type of the linear motion
+/// \param motion the linear motion 3 group
+/// \return the magnitude squared of the linear motion as a polynomial
+template <class T>
+Polynomial<T, 2> analyticMagnitudeSquared(const LinearMotionND<T, 3>& motion);
+
+}; // Math
+}; // SurgSim
+
+#include "SurgSim/Math/LinearMotionArithmetic-inl.h"
+
+#endif // SURGSIM_MATH_LINEARMOTIONARITHMETIC_H
diff --git a/SurgSim/Math/LinearSolveAndInverse-inl.h b/SurgSim/Math/LinearSolveAndInverse-inl.h
index 786721a..36099a7 100644
--- a/SurgSim/Math/LinearSolveAndInverse-inl.h
+++ b/SurgSim/Math/LinearSolveAndInverse-inl.h
@@ -24,28 +24,28 @@ namespace SurgSim
namespace Math
{
-template <int BlockSize>
+template <size_t 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>
+template <size_t 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>
+template <size_t 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>
+template <size_t BlockSize>
void LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::inverseTriDiagonalBlock(const SurgSim::Math::Matrix& A,
SurgSim::Math::Matrix* inverse,
bool isSymmetric)
@@ -151,47 +151,28 @@ void LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::inverseTriDiagonalB
}
}
-template <int BlockSize>
-void LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::operator ()(const Matrix& A, const Vector& b, Vector* x,
- Matrix* Ainv)
+template <size_t BlockSize>
+void LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::setMatrix(const Matrix& matrix)
{
- SURGSIM_ASSERT(A.cols() == A.rows()) << "Cannot inverse a non square matrix";
+ inverseTriDiagonalBlock(matrix, &m_inverse);
+}
- 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 <size_t BlockSize>
+Vector LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::solve(const Vector& b)
+{
+ return m_inverse * b;
}
-template <int BlockSize>
-void LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<BlockSize>::operator ()(const Matrix& A, const Vector& b,
- Vector* x,
- Matrix* Ainv)
+template <size_t BlockSize>
+Matrix LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::getInverse()
{
- SURGSIM_ASSERT(A.cols() == A.rows()) << "Cannot inverse a non square matrix";
+ return m_inverse;
+}
- 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;
- }
+template <size_t BlockSize>
+void LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<BlockSize>::setMatrix(const Matrix& matrix)
+{
+ inverseTriDiagonalBlock(matrix, &m_inverse, true);
}
}; // namespace Math
diff --git a/SurgSim/Math/LinearSolveAndInverse.cpp b/SurgSim/Math/LinearSolveAndInverse.cpp
index f5dc13a..ccb81ce 100644
--- a/SurgSim/Math/LinearSolveAndInverse.cpp
+++ b/SurgSim/Math/LinearSolveAndInverse.cpp
@@ -21,46 +21,38 @@ namespace SurgSim
namespace Math
{
-void LinearSolveAndInverseDiagonalMatrix::operator ()(const Matrix& A, const Vector& b, Vector* x, Matrix* Ainv)
+void LinearSolveAndInverseDiagonalMatrix::setMatrix(const Matrix& matrix)
{
- SURGSIM_ASSERT(A.cols() == A.rows()) << "Cannot inverse a non square matrix";
+ SURGSIM_ASSERT(matrix.cols() == matrix.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();
+ m_inverseDiagonal = matrix.diagonal().cwiseInverse();
+}
+
+Vector LinearSolveAndInverseDiagonalMatrix::solve(const Vector& b)
+{
+ return m_inverseDiagonal.cwiseProduct(b);
+}
- if (x != nullptr)
- {
- (*x) = Ainv->diagonal().cwiseProduct(b);
- }
- }
- else if (x != nullptr)
- {
- (*x) = A.diagonal().cwiseInverse().cwiseProduct(b);
- }
+Matrix LinearSolveAndInverseDiagonalMatrix::getInverse()
+{
+ return m_inverseDiagonal.asDiagonal();
}
-void LinearSolveAndInverseDenseMatrix::operator ()(const Matrix& A, const Vector& b, Vector* x, Matrix* Ainv)
+void LinearSolveAndInverseDenseMatrix::setMatrix(const Matrix& matrix)
{
- SURGSIM_ASSERT(A.cols() == A.rows()) << "Cannot inverse a non square matrix";
+ SURGSIM_ASSERT(matrix.cols() == matrix.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();
- }
- }
+ m_luDecomposition = matrix.partialPivLu();
+}
+
+Vector LinearSolveAndInverseDenseMatrix::solve(const Vector& b)
+{
+ return m_luDecomposition.solve(b);
+}
+
+Matrix LinearSolveAndInverseDenseMatrix::getInverse()
+{
+ return m_luDecomposition.inverse();
}
}; // namespace Math
diff --git a/SurgSim/Math/LinearSolveAndInverse.h b/SurgSim/Math/LinearSolveAndInverse.h
index 1bf9030..451b3fc 100644
--- a/SurgSim/Math/LinearSolveAndInverse.h
+++ b/SurgSim/Math/LinearSolveAndInverse.h
@@ -16,13 +16,12 @@
#ifndef SURGSIM_MATH_LINEARSOLVEANDINVERSE_H
#define SURGSIM_MATH_LINEARSOLVEANDINVERSE_H
-#include "SurgSim/Framework/Assert.h"
+#include <Eigen/Core>
+#include "SurgSim/Framework/Assert.h"
#include "SurgSim/Math/Vector.h"
#include "SurgSim/Math/Matrix.h"
-#include <Eigen/Core>
-
namespace SurgSim
{
@@ -38,33 +37,59 @@ 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;
+ /// Set the linear solver matrix
+ /// \param matrix the new matrix to solve/inverse for
+ virtual void setMatrix(const Matrix& matrix) = 0;
+
+ /// Solve the linear system (matrix.x=b) using the matrix provided by the latest setMatrix call
+ /// \param b The rhs vector
+ /// \return The solution vector
+ virtual Vector solve(const Vector& b) = 0;
+
+ /// \return The linear system's inverse matrix, i.e. the inverse of the matrix provided on the last setMatrix call
+ virtual Matrix getInverse() = 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;
+ void setMatrix(const Matrix& matrix) override;
+
+ Vector solve(const Vector& b) override;
+
+ Matrix getInverse() override;
+
+private:
+ Eigen::PartialPivLU<typename Eigen::MatrixBase<Matrix>::PlainObject> m_luDecomposition;
};
/// Derivation for diagonal matrix type
class LinearSolveAndInverseDiagonalMatrix : public LinearSolveAndInverse
{
+private:
+ Vector m_inverseDiagonal;
+
public:
- virtual void operator ()(const Matrix& A, const Vector& b, Vector* x = nullptr, Matrix* Ainv = nullptr) override;
+ void setMatrix(const Matrix& matrix) override;
+
+ Vector solve(const Vector& b) override;
+
+ Matrix getInverse() override;
};
/// Derivation for tri-diagonal block matrix type
/// \tparam BlockSize Define the block size of the tri-diagonal block matrix
-template <int BlockSize>
+template <size_t BlockSize>
class LinearSolveAndInverseTriDiagonalBlockMatrix : public LinearSolveAndInverse
{
+public:
+ void setMatrix(const Matrix& matrix) override;
+
+ Vector solve(const Vector& b) override;
+
+ Matrix getInverse() override;
+
protected:
/// Computes the inverse matrix
/// \param A The matrix to inverse
@@ -80,9 +105,6 @@ protected:
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)
@@ -107,19 +129,16 @@ private:
/// 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>
+template <size_t BlockSize>
class LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix :
public LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>
{
public:
- virtual void operator ()(const Matrix& A, const Vector& b, Vector* x = nullptr, Matrix* Ainv = nullptr) override;
+ void setMatrix(const Matrix& matrix) override;
using LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::inverseTriDiagonalBlock;
using LinearSolveAndInverseTriDiagonalBlockMatrix<BlockSize>::m_inverse;
diff --git a/SurgSim/Math/LinearSparseSolveAndInverse.cpp b/SurgSim/Math/LinearSparseSolveAndInverse.cpp
new file mode 100644
index 0000000..8aafe58
--- /dev/null
+++ b/SurgSim/Math/LinearSparseSolveAndInverse.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/Framework/Assert.h"
+#include "SurgSim/Math/LinearSparseSolveAndInverse.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+
+Matrix LinearSparseSolveAndInverse::getInverse() const
+{
+ return m_matrix.toDense().inverse();
+}
+
+void LinearSparseSolveAndInverseLU::setMatrix(const SparseMatrix& matrix)
+{
+ SURGSIM_ASSERT(matrix.cols() == matrix.rows()) << "Cannot inverse a non square matrix";
+ m_solver.compute(matrix);
+ SURGSIM_ASSERT(m_solver.info() == Eigen::Success) << m_solver.lastErrorMessage();
+ m_matrix = matrix;
+}
+
+Matrix LinearSparseSolveAndInverseLU::solve(const Matrix& b) const
+{
+ return m_solver.solve(b);
+}
+
+void LinearSparseSolveAndInverseCG::setTolerance(double tolerance)
+{
+ m_solver.setTolerance(tolerance);
+}
+
+double LinearSparseSolveAndInverseCG::getTolerance()
+{
+ return m_solver.tolerance();
+}
+
+void LinearSparseSolveAndInverseCG::setMaxIterations(SparseMatrix::Index iterations)
+{
+ m_solver.setMaxIterations(iterations);
+}
+
+SparseMatrix::Index LinearSparseSolveAndInverseCG::getMaxIterations()
+{
+ return m_solver.maxIterations();
+}
+
+void LinearSparseSolveAndInverseCG::setMatrix(const SparseMatrix& matrix)
+{
+ SURGSIM_ASSERT(matrix.cols() == matrix.rows()) << "Cannot inverse a non square matrix";
+ m_solver.compute(matrix);
+ m_matrix = matrix;
+}
+
+Matrix LinearSparseSolveAndInverseCG::solve(const Matrix& b) const
+{
+ return m_solver.solve(b);
+}
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/LinearSparseSolveAndInverse.h b/SurgSim/Math/LinearSparseSolveAndInverse.h
new file mode 100644
index 0000000..1b27405
--- /dev/null
+++ b/SurgSim/Math/LinearSparseSolveAndInverse.h
@@ -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.
+
+#ifndef SURGSIM_MATH_LINEARSPARSESOLVEANDINVERSE_H
+#define SURGSIM_MATH_LINEARSPARSESOLVEANDINVERSE_H
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4244)
+#endif
+
+#include <Eigen/SparseCore>
+#include <unordered_map>
+
+#include <boost/assign/list_of.hpp> // for 'map_list_of()'
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// The linear numerical integration scheme supported
+/// Each Linear Solver should have its own entry in this enum
+enum LinearSolver
+{
+ LINEARSOLVER_LU = 0,
+ LINEARSOLVER_CONJUGATEGRADIENT,
+ MAX_LINEARSOLVER
+};
+
+const std::unordered_map<LinearSolver, std::string, std::hash<int>> LinearSolverNames =
+ boost::assign::map_list_of
+ (LINEARSOLVER_LU, "LINEARSOLVER_LU")
+ (LINEARSOLVER_CONJUGATEGRADIENT, "LINEARSOLVER_CONJUGATEGRADIENT");
+
+/// LinearSparseSolveAndInverse 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 LinearSparseSolveAndInverse
+{
+public:
+ virtual ~LinearSparseSolveAndInverse() {}
+
+
+ /// Set the linear solver matrix
+ /// \param matrix the new matrix to solve/inverse for
+ virtual void setMatrix(const SparseMatrix& matrix) = 0;
+
+ /// Solve the linear system (matrix.x=b) using the matrix provided by the latest setMatrix call
+ /// for all columns of the rhs matrix b.
+ /// \param b The rhs matrix
+ /// \return The solution matrix
+ virtual Matrix solve(const Matrix& b) const = 0;
+
+ /// \return The linear system's inverse matrix, i.e. the inverse of the matrix provided on the last setMatrix call
+ virtual Matrix getInverse() const;
+
+protected:
+ /// A copy of the system matrix for use when an inverse is necessary.
+ SparseMatrix m_matrix;
+};
+
+/// Derivation for sparse LU solver
+class LinearSparseSolveAndInverseLU : public LinearSparseSolveAndInverse
+{
+public:
+ void setMatrix(const SparseMatrix& matrix) override;
+
+ Matrix solve(const Matrix& b) const override;
+
+private:
+ Eigen::SparseLU<SparseMatrix> m_solver;
+};
+
+/// Derivation for sparse CG solver
+class LinearSparseSolveAndInverseCG : public LinearSparseSolveAndInverse
+{
+public:
+ /// Set the conjugate gradient convergence tolerance
+ /// \param tolerance the new convergence tolerance
+ void setTolerance(double tolerance);
+
+ /// Get the conjugate gradient convergence tolerance
+ /// \return the convergence tolerance
+ double getTolerance();
+
+ /// Set the maximum number of iterations for conjugate gradient
+ /// \param iterations the new maximum number of iterations
+ void setMaxIterations(SparseMatrix::Index iterations);
+
+ /// Get the conjugate gradient maximum iterations
+ /// \return the maximum number of iterations allowed
+ SparseMatrix::Index getMaxIterations();
+
+ void setMatrix(const SparseMatrix& matrix) override;
+
+ Matrix solve(const Matrix& b) const override;
+
+private:
+ Eigen::ConjugateGradient<SparseMatrix> m_solver;
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif
+
+#endif // SURGSIM_MATH_LINEARSPARSESOLVEANDINVERSE_H
diff --git a/SurgSim/Math/MathConvert-inl.h b/SurgSim/Math/MathConvert-inl.h
index 9135067..b708f8e 100644
--- a/SurgSim/Math/MathConvert-inl.h
+++ b/SurgSim/Math/MathConvert-inl.h
@@ -28,95 +28,85 @@ 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)
+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)
{
- if (! node.IsSequence() || node.size() != Rows)
- {
- return false;
- }
-
- for (unsigned i = 0; i < node.size(); ++i)
+ typedef typename Eigen::Matrix<Type, Rows, Cols, MOpt>::Index Index;
+ YAML::Node node;
+ node.SetStyle(YAML::EmitterStyle::Flow);
+ if (Cols == 1)
{
- try
+ for (Index i = 0; i < rhs.size(); ++i)
{
- 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";
+ node.push_back(rhs(i, 0));
}
}
- 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)
+ else
{
- YAML::Node rowNode;
- for (int col = 0; col < Cols; ++col)
+ for (size_t row = 0; row < Rows; ++row)
{
- rowNode.push_back(rhs.row(row)[col]);
+ YAML::Node rowNode;
+ for (size_t col = 0; col < Cols; ++col)
+ {
+ rowNode.push_back(rhs.row(row)[col]);
+ }
+ node.push_back(rowNode);
}
- 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)
+ typename Eigen::Matrix<Type, Rows, Cols, MOpt>& rhs) //NOLINT
{
if (! node.IsSequence() || node.size() != Rows)
{
return false;
}
- for (size_t row = 0; row < node.size(); ++row)
+ if (Cols == 1)
{
- YAML::Node rowNode = node[row];
- if (!rowNode.IsSequence() || node.size() != Cols)
- {
- return false;
- }
- for (size_t col = 0; col < rowNode.size(); ++col)
+ for (size_t i = 0; i < node.size(); ++i)
{
try
{
- rhs.row(row)[col] = rowNode[col].as<Type>();
+ rhs(i, 0) = node[i].as<Type>();
}
catch (YAML::RepresentationException)
{
- rhs.row(row)[col] = std::numeric_limits<Type>::quiet_NaN();
+ rhs(i, 0) = std::numeric_limits<Type>::quiet_NaN();
+
auto logger = SurgSim::Framework::Logger::getLogger(serializeLogger);
- SURGSIM_LOG(logger, WARNING) << "Bad conversion: #NaN value";
+ SURGSIM_LOG(logger, WARNING) << "Bad conversion, using #NaN value. For node: " << node;
+ }
+ }
+ }
+ else
+ {
+ 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, using #NaN value. For node: " << node;
+ }
}
}
}
@@ -126,7 +116,7 @@ bool YAML::convert<typename Eigen::Matrix<Type, Rows, Cols, MOpt>>::decode(
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)
+ const typename Eigen::Quaternion<Type, QOpt>& rhs)
{
return Node(convert<typename Eigen::Matrix<Type, 4, 1, QOpt>>::encode(rhs.coeffs()));
}
@@ -134,21 +124,30 @@ YAML::Node YAML::convert<typename Eigen::Quaternion<Type, QOpt>>::encode(
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)
+ const Node& node,
+ typename Eigen::Quaternion<Type, QOpt>& rhs) //NOLINT
{
bool result = false;
if (node.IsSequence() && node.size() == 4)
{
result = convert<typename Eigen::Matrix<Type, 4, 1, QOpt>>::decode(node, rhs.coeffs());
}
+ else
+ {
+ Eigen::AngleAxis<Type> angleAxis;
+ result = convert<typename Eigen::AngleAxis<Type>>::decode(node, angleAxis);
+ if (result)
+ {
+ rhs = angleAxis;
+ }
+ }
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)
+ const typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs)
{
typedef typename Eigen::Transform<Type, Dim, TMode, TOptions>::LinearMatrixType LinearMatrixType;
LinearMatrixType linear(rhs.linear());
@@ -165,7 +164,7 @@ 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)
+ typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs) //NOLINT
{
bool result = false;
@@ -191,4 +190,40 @@ bool YAML::convert<typename Eigen::Transform<Type, Dim, TMode, TOptions>>::decod
return result;
}
+SURGSIM_DOUBLE_SPECIALIZATION
+template <class Type>
+YAML::Node YAML::convert<typename Eigen::AngleAxis<Type>>::encode(
+ const typename Eigen::AngleAxis<Type>& rhs)
+{
+ Node node;
+ node.SetStyle(EmitterStyle::Flow);
+ node["Angle"] = rhs.angle();
+ node["Axis"] = convert<typename Eigen::Matrix<Type, 3, 1>>::encode(rhs.axis());
+ return node;
+}
+
+SURGSIM_DOUBLE_SPECIALIZATION
+template <class Type>
+bool YAML::convert<typename Eigen::AngleAxis<Type>>::decode(
+ const Node& node,
+ typename Eigen::AngleAxis<Type>& rhs) //NOLINT
+{
+ bool result = false;
+ if (node.IsMap() && node["Angle"].IsDefined() && node["Axis"].IsDefined())
+ {
+ try
+ {
+ rhs.angle() = node["Angle"].as<Type>();
+ }
+ catch (RepresentationException)
+ {
+ rhs.angle() = std::numeric_limits<Type>::quiet_NaN();
+ auto logger = SurgSim::Framework::Logger::getLogger(serializeLogger);
+ SURGSIM_LOG(logger, WARNING) << "Bad conversion, using #NaN value. For node: " << node;
+ }
+ result = convert<typename Eigen::Matrix<Type, 3, 1>>::decode(node["Axis"], rhs.axis());
+ }
+ return result;
+}
+
#endif // SURGSIM_MATH_MATHCONVERT_INL_H
diff --git a/SurgSim/Math/MathConvert.cpp b/SurgSim/Math/MathConvert.cpp
index 0a514fe..62259a4 100644
--- a/SurgSim/Math/MathConvert.cpp
+++ b/SurgSim/Math/MathConvert.cpp
@@ -23,7 +23,7 @@ namespace YAML
{
Node convert<std::shared_ptr<SurgSim::Math::Shape>>::encode(
- const std::shared_ptr<SurgSim::Math::Shape>& rhs)
+ const std::shared_ptr<SurgSim::Math::Shape>& rhs)
{
SURGSIM_ASSERT(nullptr != rhs) << "Trying to encode nullptr SurgSim::Math::Shape";
Node result;
@@ -33,8 +33,8 @@ Node convert<std::shared_ptr<SurgSim::Math::Shape>>::encode(
}
bool convert<std::shared_ptr<SurgSim::Math::Shape>>::decode(
- const Node& node,
- std::shared_ptr<SurgSim::Math::Shape>& rhs)
+ const Node& node,
+ std::shared_ptr<SurgSim::Math::Shape>& rhs) //NOLINT
{
bool result = false;
@@ -67,7 +67,6 @@ bool convert<std::shared_ptr<SurgSim::Math::Shape>>::decode(
return result;
}
-
Node convert<SurgSim::Math::IntegrationScheme>::encode(
const SurgSim::Math::IntegrationScheme& rhs)
{
@@ -75,7 +74,7 @@ Node convert<SurgSim::Math::IntegrationScheme>::encode(
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?";
+ "SurgSim::Math::IntegrationSchemeNames. Is the enum value registered?";
result["SurgSim::Math::IntegrationScheme"] = it->second;
@@ -84,7 +83,7 @@ Node convert<SurgSim::Math::IntegrationScheme>::encode(
bool convert<SurgSim::Math::IntegrationScheme>::decode(
const Node& node,
- SurgSim::Math::IntegrationScheme& rhs)
+ SurgSim::Math::IntegrationScheme& rhs) //NOLINT
{
bool result = false;
@@ -99,13 +98,59 @@ bool convert<SurgSim::Math::IntegrationScheme>::decode(
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;
- }
+ {
+ return pair.second == schemeName;
+ }
+ );
+
+ SURGSIM_ASSERT(it != std::end(SurgSim::Math::IntegrationSchemeNames)) <<
+ "Unknown IntegrationScheme " << schemeName;
+
+ rhs = it->first;
+ result = true;
+ }
+
+ return result;
+}
+
+Node convert<SurgSim::Math::LinearSolver>::encode(
+ const SurgSim::Math::LinearSolver& rhs)
+{
+ Node result;
+
+ auto it = SurgSim::Math::LinearSolverNames.find(rhs);
+ SURGSIM_ASSERT(it != std::end(SurgSim::Math::LinearSolverNames)) << "Can not find the enum value in " <<
+ "SurgSim::Math::LinearSolverNames. Is the enum value registered?";
+
+ result["SurgSim::Math::LinearSolver"] = it->second;
+
+ return result;
+}
+
+bool convert<SurgSim::Math::LinearSolver>::decode(
+ const Node& node,
+ SurgSim::Math::LinearSolver& rhs) //NOLINT
+{
+ bool result = false;
+
+ if (node.IsMap())
+ {
+ std::string className = node.begin()->first.as<std::string>();
+ SURGSIM_ASSERT("SurgSim::Math::LinearSolver" == 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::LinearSolverNames),
+ std::end(SurgSim::Math::LinearSolverNames),
+ [&schemeName](const std::pair<SurgSim::Math::LinearSolver, std::string>& pair)
+ {
+ return pair.second == schemeName;
+ }
);
- SURGSIM_ASSERT (it != std::end(SurgSim::Math::IntegrationSchemeNames)) <<
- "Unknown IntegrationScheme " << schemeName;
+ SURGSIM_ASSERT(it != std::end(SurgSim::Math::LinearSolverNames)) <<
+ "Unknown LinearSolver " << schemeName;
rhs = it->first;
result = true;
@@ -114,4 +159,4 @@ bool convert<SurgSim::Math::IntegrationScheme>::decode(
return result;
}
-}; // namespace YAML
\ No newline at end of file
+}; // namespace YAML
diff --git a/SurgSim/Math/MathConvert.h b/SurgSim/Math/MathConvert.h
index 618802b..6d1c3c3 100644
--- a/SurgSim/Math/MathConvert.h
+++ b/SurgSim/Math/MathConvert.h
@@ -22,6 +22,7 @@
#include <yaml-cpp/yaml.h>
#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Math/LinearSparseSolveAndInverse.h"
#include "SurgSim/Math/OdeSolver.h"
namespace SurgSim
@@ -42,57 +43,66 @@ class Shape;
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);
- };
+
+/// 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); //NOLINT
+};
+
+/// 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); //NOLINT
+};
+
+/// 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); //NOLINT
+};
+
+/// Specialization of convert for Eigen::AngleAxis
+SURGSIM_DOUBLE_SPECIALIZATION
+template <typename Type>
+struct convert<typename Eigen::AngleAxis<Type>>
+{
+ static Node encode(const typename Eigen::AngleAxis<Type>& rhs);
+ static bool decode(const Node& node, typename Eigen::AngleAxis<Type>& rhs); //NOLINT
+};
+
+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); //NOLINT
+};
+
+template <>
+struct convert<SurgSim::Math::IntegrationScheme>
+{
+ static Node encode(const SurgSim::Math::IntegrationScheme& rhs);
+ static bool decode(const Node& node, SurgSim::Math::IntegrationScheme& rhs); //NOLINT
+};
+
+template <>
+struct convert<SurgSim::Math::LinearSolver>
+{
+ static Node encode(const SurgSim::Math::LinearSolver& rhs);
+ static bool decode(const Node& node, SurgSim::Math::LinearSolver& rhs); //NOLINT
+};
+
};
#include "SurgSim/Math/MathConvert-inl.h"
-#endif // SURGSIM_MATH_MATHCONVERT_H
\ No newline at end of file
+#endif // SURGSIM_MATH_MATHCONVERT_H
diff --git a/SurgSim/Math/Matrix.h b/SurgSim/Math/Matrix.h
index 99f0a3f..a230a36 100644
--- a/SurgSim/Math/Matrix.h
+++ b/SurgSim/Math/Matrix.h
@@ -155,7 +155,7 @@ inline T computeAngle(const Eigen::Matrix<T, 3, 3, MOpt>& 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)
+ size_t blockSizeRow, size_t blockSizeCol, Matrix* matrix)
{
matrix->block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol, blockSizeRow, blockSizeCol) += subMatrix;
}
@@ -180,8 +180,8 @@ void addSubMatrix(const SubMatrix& subMatrix, const std::vector<size_t> blockIds
{
size_t blockId1 = blockIds[block1];
- matrix->block(blockSize * blockId0, blockSize * blockId1, blockSize, blockSize)
- += subMatrix.block(blockSize * block0, blockSize * block1, blockSize, blockSize);
+ matrix->block(blockSize * blockId0, blockSize * blockId1, blockSize, blockSize) +=
+ subMatrix.block(blockSize * block0, blockSize * block1, blockSize, blockSize);
}
}
}
@@ -195,10 +195,10 @@ void addSubMatrix(const SubMatrix& subMatrix, const std::vector<size_t> blockIds
/// \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)
+ size_t blockSizeRow, size_t blockSizeCol, Matrix* matrix)
{
matrix->block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol,
- blockSizeRow, blockSizeCol) = subMatrix;
+ blockSizeRow, blockSizeCol) = subMatrix;
}
/// Helper method to access a sub-matrix from a matrix, for the sake of clarity
@@ -212,11 +212,31 @@ void setSubMatrix(const SubMatrix& subMatrix, size_t blockIdRow, size_t blockIdC
/// \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)
+ size_t blockSizeRow, size_t blockSizeCol)
{
return matrix.block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol, blockSizeRow, blockSizeCol);
}
+/// Helper method to zero a row of a matrix.
+/// \tparam Matrix The matrix type
+/// \param row The row to set to zero
+/// \param[in,out] matrix The matrix to set the zero row on.
+template <class Derived>
+void zeroRow(size_t row, Eigen::DenseBase<Derived>* matrix)
+{
+ matrix->middleRows(row, 1).setZero();
+}
+
+/// Helper method to zero a column of a matrix.
+/// \tparam Matrix The matrix type
+/// \param column The column to set to zero
+/// \param[in,out] matrix The matrix to set the zero column on.
+template <class Derived>
+void zeroColumn(size_t column, Eigen::DenseBase<Derived>* matrix)
+{
+ (*matrix).middleCols(column, 1).setZero();
+}
+
}; // namespace Math
}; // namespace SurgSim
diff --git a/SurgSim/Math/MeshShape-inl.h b/SurgSim/Math/MeshShape-inl.h
index 9e6d769..6448aa0 100644
--- a/SurgSim/Math/MeshShape-inl.h
+++ b/SurgSim/Math/MeshShape-inl.h
@@ -22,21 +22,19 @@ 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)
+template <class V, class E, class T>
+MeshShape::MeshShape(const SurgSim::DataStructures::TriangleMesh<V, E, T>& other) :
+ SurgSim::DataStructures::TriangleMesh<SurgSim::DataStructures::EmptyData, SurgSim::DataStructures::EmptyData,
+ SurgSim::DataStructures::NormalData>::TriangleMesh(other)
{
- SURGSIM_ASSERT(mesh.isValid()) << "Invalid mesh";
+ SURGSIM_ASSERT(other.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();
+ calculateNormals();
}
}; // namespace Math
}; // namespace SurgSim
-#endif
\ No newline at end of file
+#endif
diff --git a/SurgSim/Math/MeshShape.cpp b/SurgSim/Math/MeshShape.cpp
index 4b7a48c..9144075 100644
--- a/SurgSim/Math/MeshShape.cpp
+++ b/SurgSim/Math/MeshShape.cpp
@@ -12,84 +12,114 @@
// WITHOUT WARRANTIES 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"
+
+#include "SurgSim/DataStructures/AabbTree.h"
+#include "SurgSim/DataStructures/AabbTreeData.h"
+#include "SurgSim/Framework/Assert.h"
+
+using SurgSim::DataStructures::EmptyData;
+using SurgSim::DataStructures::NormalData;
+
+
+template<>
+std::string SurgSim::DataStructures::TriangleMesh<EmptyData, EmptyData, NormalData>
+::m_className = "SurgSim::Math::MeshShape";
namespace SurgSim
{
namespace Math
{
+
SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::MeshShape, MeshShape);
-MeshShape::MeshShape() : m_volume(0.0)
+MeshShape::MeshShape() :
+ m_center(Vector3d::Constant(std::numeric_limits<double>::quiet_NaN())),
+ m_volume(std::numeric_limits<double>::quiet_NaN()),
+ m_secondMomentOfVolume(Matrix33d::Constant(std::numeric_limits<double>::quiet_NaN()))
{
- serializeFileName(this);
}
-int MeshShape::getType()
+MeshShape::MeshShape(const MeshShape& other) :
+ DataStructures::TriangleMesh<DataStructures::EmptyData, DataStructures::EmptyData, DataStructures::NormalData>
+ ::TriangleMesh(other),
+ m_center(other.getCenter()),
+ m_volume(other.getVolume()),
+ m_secondMomentOfVolume(other.getSecondMomentOfVolume())
{
- return SHAPE_TYPE_MESH;
+ updateAabbTree();
}
-bool MeshShape::isValid() const
+const SurgSim::Math::Vector3d& MeshShape::getNormal(size_t triangleId) const
{
- return (nullptr != m_mesh) && (m_mesh->isValid());
+ return getTriangle(triangleId).data.normal;
}
-bool MeshShape::doLoad(const std::string& filePath)
+bool MeshShape::calculateNormals()
{
- 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.";
+ bool result = true;
+ 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]);
- m_mesh = std::make_shared<SurgSim::DataStructures::TriangleMesh>(*m_initialMesh);
+ // Calculate normal vector
+ SurgSim::Math::Vector3d normal = (vertex1 - vertex0).cross(vertex2 - vertex0);
+ if (normal.isZero())
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getLogger("Math/MeshShape")) <<
+ "MeshShape::calculateNormals unable to calculate normals. For example, for triangle #" << i <<
+ " with vertices:" << std::endl << "1: " << vertex0.transpose() << std::endl <<
+ "2: " << vertex1.transpose() << std::endl << "3: " << vertex2.transpose();
+ result = false;
+ break;
+ }
+ normal.normalize();
+ getTriangle(i).data.normal = normal;
+ }
+ return result;
+}
+bool MeshShape::doUpdate()
+{
updateAabbTree();
computeVolumeIntegrals();
-
- return true;
+ return calculateNormals();
}
-std::shared_ptr<SurgSim::DataStructures::TriangleMesh> MeshShape::getInitialMesh()
+bool MeshShape::doLoad(const std::string& fileName)
{
- return m_initialMesh;
+ if (!SurgSim::DataStructures::TriangleMesh<EmptyData, EmptyData, NormalData>::doLoad(fileName))
+ {
+ return false;
+ }
+ return update();
}
-std::shared_ptr<SurgSim::DataStructures::TriangleMesh> MeshShape::getMesh()
+int MeshShape::getType() const
{
- return m_mesh;
+ return SHAPE_TYPE_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;
}
+bool MeshShape::isValid() const
+{
+ return SurgSim::DataStructures::TriangleMesh<EmptyData, EmptyData, NormalData>::isValid();
+}
+
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;
}
@@ -122,16 +152,16 @@ void MeshShape::computeVolumeIntegrals()
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())
+ for (auto const& triangle : 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]);
+ const Vector3d& v0 = getVertexPosition(triangle.verticesId[0]);
+ const Vector3d& v1 = getVertexPosition(triangle.verticesId[1]);
+ const Vector3d& v2 = getVertexPosition(triangle.verticesId[2]);
// get edges and cross product of edges
Vector3d normal = (v1 - v0).cross(v2 - v0);
@@ -183,13 +213,15 @@ void MeshShape::computeVolumeIntegrals()
m_secondMomentOfVolume(2, 0) = m_secondMomentOfVolume(0, 2);
}
-void MeshShape::setPose(const SurgSim::Math::RigidTransform3d& pose)
+std::shared_ptr<Shape> MeshShape::getTransformed(const RigidTransform3d& pose) const
{
- m_mesh->copyWithTransform(pose, *m_initialMesh);
- updateAabbTree();
+ auto transformed = std::make_shared<MeshShape>(*this);
+ transformed->transform(pose);
+ transformed->update();
+ return transformed;
}
-std::shared_ptr<SurgSim::DataStructures::AabbTree> MeshShape::getAabbTree()
+const std::shared_ptr<const SurgSim::DataStructures::AabbTree> MeshShape::getAabbTree() const
{
return m_aabbTree;
}
@@ -198,15 +230,28 @@ 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)
+ std::list<DataStructures::AabbTreeData::Item> items;
+
+ auto const& triangles = getTriangles();
+
+ for (size_t id = 0, count = triangles.size(); id < count; ++id)
{
if (triangles[id].isValid)
{
- auto vertices = m_mesh->getTrianglePositions(id);
- m_aabbTree->add(SurgSim::Math::makeAabb(vertices[0], vertices[1], vertices[2]), id);
+ auto vertices = getTrianglePositions(id);
+ items.emplace_back(SurgSim::Math::makeAabb(vertices[0], vertices[1], vertices[2]), id);
}
}
+ m_aabbTree->set(std::move(items));
+
+ m_aabb = m_aabbTree->getAabb();
+}
+
+bool MeshShape::isTransformable() const
+{
+ return true;
}
+
+
}; // namespace Math
}; // namespace SurgSim
diff --git a/SurgSim/Math/MeshShape.h b/SurgSim/Math/MeshShape.h
index 70b7159..616637b 100644
--- a/SurgSim/Math/MeshShape.h
+++ b/SurgSim/Math/MeshShape.h
@@ -13,36 +13,37 @@
// 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 <memory>
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/NormalData.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 DataStructures
+{
+class AabbTree;
+}
namespace Math
{
-SURGSIM_STATIC_REGISTRATION(MeshShape);
+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
+/// David Eberly's work (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.
///
/// \note The internal mesh should not be modified, otherwise the geometric properties will be invalid.
/// \note Practical use cases:
@@ -50,70 +51,66 @@ SURGSIM_STATIC_REGISTRATION(MeshShape);
/// \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
+class MeshShape : public Shape, public SurgSim::DataStructures::TriangleMesh<SurgSim::DataStructures::EmptyData,
+ SurgSim::DataStructures::EmptyData, SurgSim::DataStructures::NormalData>
{
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);
+ /// Copy constructor
+ /// \param other The MeshShape to be copied from
+ explicit MeshShape(const MeshShape& other);
- SURGSIM_CLASSNAME(SurgSim::Math::MeshShape);
-
- /// \return the type of the shape
- virtual int getType() override;
+ /// Copy constructor when the template data is a different type
+ /// \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
+ /// \param other The mesh to be copied from. Vertex, edge and triangle data will not be copied
+ template <class V, class E, class T>
+ explicit MeshShape(const SurgSim::DataStructures::TriangleMesh<V, E, T>& other);
- /// Gets the initial mesh
- /// \return The collision mesh associated to this MeshShape
- std::shared_ptr<SurgSim::DataStructures::TriangleMesh> getInitialMesh();
+ SURGSIM_CLASSNAME(SurgSim::Math::MeshShape);
- /// Get mesh
- /// \return The collision mesh associated to this MeshShape
- std::shared_ptr<SurgSim::DataStructures::TriangleMesh> getMesh();
+ int getType() const override;
- /// 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 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) const;
- /// 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;
+ double getVolume() 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;
+ Vector3d getCenter() const override;
- /// Set the object's global pose
- /// \param pose the rigid transform to apply
- void setPose(const SurgSim::Math::RigidTransform3d &pose);
+ Matrix33d getSecondMomentOfVolume() const override;
- /// Update the AabbTree, which is an axis-aligned bounding box r-tree used to accelerate spatial searches
- void updateAabbTree();
+ std::shared_ptr<Shape> getTransformed(const RigidTransform3d& pose) const override;
/// Get the AabbTree
/// \return The object's associated AabbTree
- std::shared_ptr<SurgSim::DataStructures::AabbTree> getAabbTree();
+ const std::shared_ptr<const SurgSim::DataStructures::AabbTree> getAabbTree() const;
- /// 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;
+ bool isValid() const override;
+
+ bool isTransformable() const override;
protected:
- virtual bool doLoad(const std::string& filePath) override;
+ bool doUpdate() override;
+
+ bool doLoad(const std::string& fileName) override;
+
+ /// Calculate normals for all triangles.
+ /// \note Normals will be normalized.
+ /// \return true on success, or false if any triangle has an indeterminate normal.
+ bool calculateNormals();
+
+ /// Update the AabbTree, which is an axis-aligned bounding box r-tree used to accelerate spatial searches
+ void updateAabbTree();
-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();
+ virtual void computeVolumeIntegrals();
/// Center (considering a uniform distribution in the mesh volume)
SurgSim::Math::Vector3d m_center;
@@ -124,12 +121,7 @@ private:
/// 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;
-
+private:
/// The aabb tree used to accelerate collision detection against the mesh
std::shared_ptr<SurgSim::DataStructures::AabbTree> m_aabbTree;
};
diff --git a/SurgSim/Math/MinMax-inl.h b/SurgSim/Math/MinMax-inl.h
new file mode 100644
index 0000000..5f901d5
--- /dev/null
+++ b/SurgSim/Math/MinMax-inl.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_MATH_MINMAX_INL_H
+#define SURGSIM_MATH_MINMAX_INL_H
+
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+template <class T>
+void minMax(const T& a1, const T& a2, T* minVal, T* maxVal)
+{
+ if (a1 < a2)
+ {
+ *minVal = a1;
+ *maxVal = a2;
+ }
+ else
+ {
+ *minVal = a2;
+ *maxVal = a1;
+ }
+}
+
+template <class T>
+void minMax(const T& a1, const T& a2, const T& a3, T* minVal, T* maxVal)
+{
+ T min = a1;
+ T max = a1;
+ if (a2 < min)
+ {
+ min = a2;
+ }
+ else if (a2 > max)
+ {
+ max = a2;
+ }
+ if (a3 < min)
+ {
+ min = a3;
+ }
+ else if (a3 > max)
+ {
+ max = a3;
+ }
+
+ *minVal = min;
+ *maxVal = max;
+}
+
+template <class T>
+void minMax(const T& a1, const T& a2, const T& a3, const T& a4, T* minVal, T* maxVal)
+{
+ T min = a1;
+ T max = a1;
+ if (a2 < min)
+ {
+ min = a2;
+ }
+ else if (a2 > max)
+ {
+ max = a2;
+ }
+ if (a3 < min)
+ {
+ min = a3;
+ }
+ else if (a3 > max)
+ {
+ max = a3;
+ }
+ if (a4 < min)
+ {
+ min = a4;
+ }
+ else if (a4 > max)
+ {
+ max = a4;
+ }
+
+ *minVal = min;
+ *maxVal = max;
+}
+
+template <class T>
+void minMax(const T& a1, const T& a2, const T& a3, const T& a4, const T& a5, T* minVal, T* maxVal)
+{
+ T min = a1;
+ T max = a1;
+ if (a2 < min)
+ {
+ min = a2;
+ }
+ else if (a2 > max)
+ {
+ max = a2;
+ }
+ if (a3 < min)
+ {
+ min = a3;
+ }
+ else if (a3 > max)
+ {
+ max = a3;
+ }
+ if (a4 < min)
+ {
+ min = a4;
+ }
+ else if (a4 > max)
+ {
+ max = a4;
+ }
+ if (a5 < min)
+ {
+ min = a5;
+ }
+ else if (a5 > max)
+ {
+ max = a5;
+ }
+
+ *minVal = min;
+ *maxVal = max;
+}
+
+template <class T>
+void minMax(const T* values, int numValues, T* minVal, T* maxVal)
+{
+ SURGSIM_ASSERT(numValues > 0) << "MinMax was called with a negative or null" <<
+ " number of values; the result is indeterminate.";
+ T min = values[0];
+ T max = values[0];
+ for (int i = 1; i < numValues; ++i)
+ {
+ if (values[i] < min)
+ {
+ min = values[i];
+ }
+ else if (values[i] > max)
+ {
+ max = values[i];
+ }
+ }
+ *minVal = min;
+ *maxVal = max;
+}
+
+};
+};
+
+#endif // SURGSIM_MATH_MINMAX_INL_H
+
diff --git a/SurgSim/Math/MinMax.h b/SurgSim/Math/MinMax.h
new file mode 100644
index 0000000..ac9ca30
--- /dev/null
+++ b/SurgSim/Math/MinMax.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_MATH_MINMAX_H
+#define SURGSIM_MATH_MINMAX_H
+
+namespace SurgSim
+{
+namespace Math
+{
+/// Calculate the minimum and maximum of two values
+/// \tparam T underlying type
+/// \param a1 the first value
+/// \param a2 the second value
+/// \param minVal [out] the minimum of a1 and a2
+/// \param maxVal [out] the maximum of a1 and a2
+template <class T>
+void minMax(const T& a1, const T& a2, T* minVal, T* maxVal);
+
+/// Calculate the minimum and maximum of three values
+/// \tparam T underlying type
+/// \param a1 the first value
+/// \param a2 the second value
+/// \param a3 the third value
+/// \param minVal [out] the minimum of a1, a2 and a3
+/// \param maxVal [out] the maximum of a1, a2 and a3
+template <class T>
+void minMax(const T& a1, const T& a2, const T& a3, T* minVal, T* maxVal);
+
+/// Calculate the minimum and maximum of four values
+/// \tparam T underlying type
+/// \param a1 the first value
+/// \param a2 the second value
+/// \param a3 the third value
+/// \param a4 the fourth value
+/// \param minVal [out] the minimum of a1, a2, a3 and a4
+/// \param maxVal [out] the maximum of a1, a2, a3 and a4
+template <class T>
+void minMax(const T& a1, const T& a2, const T& a3, const T& a4, T* minVal, T* maxVal);
+
+/// Calculate the minimum and maximum of five values
+/// \tparam T underlying type
+/// \param a1 the first value
+/// \param a2 the second value
+/// \param a3 the third value
+/// \param a4 the fourth value
+/// \param a5 the fifth value
+/// \param minVal [out] the minimum of a1, a2, a3, a4 and a5
+/// \param maxVal [out] the maximum of a1, a2, a3, a4 and a5
+template <class T>
+void minMax(const T& a1, const T& a2, const T& a3, const T& a4, const T& a5, T* minVal, T* maxVal);
+
+/// Calculate the minimum and maximum of numValues values
+/// \tparam T underlying type
+/// \param values a series of numValues values
+/// \param numValues the number of values in the series
+/// \param minVal [out] the minimum value in values
+/// \param maxVal [out] the maximum value in values
+template <class T>
+void minMax(const T* values, int numValues, T* minVal, T* maxVal);
+};
+};
+
+#include "SurgSim/Math/MinMax-inl.h"
+
+#endif // SURGSIM_MATH_MINMAX_H
diff --git a/SurgSim/Math/MlcpGaussSeidelSolver.cpp b/SurgSim/Math/MlcpGaussSeidelSolver.cpp
index efaec33..6164993 100644
--- a/SurgSim/Math/MlcpGaussSeidelSolver.cpp
+++ b/SurgSim/Math/MlcpGaussSeidelSolver.cpp
@@ -13,17 +13,13 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "SurgSim/Math/MlcpConstraintTypeName.h"
#include "SurgSim/Math/MlcpGaussSeidelSolver.h"
#include <math.h>
-#include <stdio.h>
-
-#include <Eigen/Core>
-#include <Eigen/LU>
-#include <Eigen/Dense>
+#include "SurgSim/Framework/Log.h"
#include "SurgSim/Math/Valid.h"
-#include "SurgSim/Framework/Assert.h"
namespace SurgSim
@@ -31,432 +27,307 @@ 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
-*/
+MlcpGaussSeidelSolver::MlcpGaussSeidelSolver() :
+ m_epsilonConvergence(1e-4),
+ m_contactTolerance(2e-5),
+ m_maxIterations(30),
+ m_numEnforcedAtomicConstraints(0),
+ m_logger(SurgSim::Framework::Logger::getLogger("Math/MlcpGaussSeidelSolver"))
+{
+}
+
+MlcpGaussSeidelSolver::MlcpGaussSeidelSolver(double epsilonConvergence, double contactTolerance,
+ size_t maxIterations) :
+ m_epsilonConvergence(epsilonConvergence),
+ m_contactTolerance(contactTolerance),
+ m_maxIterations(maxIterations),
+ m_numEnforcedAtomicConstraints(0),
+ m_logger(SurgSim::Framework::Logger::getLogger("Math/MlcpGaussSeidelSolver"))
+{
+}
+
+MlcpGaussSeidelSolver::~MlcpGaussSeidelSolver()
+{
+}
+
+double MlcpGaussSeidelSolver::getEpsilonConvergence() const
+{
+ return m_epsilonConvergence;
+}
+
+void MlcpGaussSeidelSolver::setEpsilonConvergence(double precision)
+{
+ m_epsilonConvergence = precision;
+}
+
+double MlcpGaussSeidelSolver::getContactTolerance() const
+{
+ return m_contactTolerance;
+}
+
+void MlcpGaussSeidelSolver::setContactTolerance(double tolerance)
+{
+ m_contactTolerance = tolerance;
+}
+
+size_t MlcpGaussSeidelSolver::getMaxIterations() const
+{
+ return m_maxIterations;
+}
+
+void MlcpGaussSeidelSolver::setMaxIterations(size_t maxIterations)
+{
+ m_maxIterations = maxIterations;
+}
+
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 size_t problemSize = 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;
+ MlcpSolution::Vector& initialGuessAndSolution = solution->x;
const std::vector<MlcpConstraintType>& constraintsType = problem.constraintTypes;
- double subStep = 1.0;//XXX
- int* MLCP_nbIterations = &solution->numIterations;
+ size_t* iteration = &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;
+ *iteration = 0;
+ *validSignorini = true;
- 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);
+ calculateConvergenceCriteria(problemSize, A, b, initialGuessAndSolution,
+ constraintsType, initialConstraintConvergenceCriteria, initialConvergenceCriteria,
+ validSignorini);
// If it is already converged, fill the output and return true.
- if (initial_convergence_criteria <= m_epsilonConvergence && initialSignoriniVerified)
+ if (*initialConvergenceCriteria <= m_epsilonConvergence && *validSignorini)
{
- 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];
- }
-
+ *validConvergence = true;
+ *convergenceCriteria = *initialConvergenceCriteria;
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;
+ doOneIteration(problemSize, A, b, &initialGuessAndSolution, problem.mu, constraintsType,
+ constraintConvergenceCriteria, convergenceCriteria, validSignorini);
+
+ calculateConvergenceCriteria(problemSize, A, b, initialGuessAndSolution,
+ constraintsType, constraintConvergenceCriteria, convergenceCriteria,
+ validSignorini);
+ ++(*iteration);
+
+ // 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(*convergenceCriteria) || *convergenceCriteria > 1.0)
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Convergence (" << *convergenceCriteria <<
+ ") is NaN, infinite, or greater than 1.0! MLCP is exploding after " <<
+ *iteration << " Gauss Seidel iterations!!";
+ break;
}
}
+ while ((!(*validSignorini) || (*convergenceCriteria > m_epsilonConvergence)) && *iteration < m_maxIterations);
- 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
+ *validConvergence = SurgSim::Math::isValid(*convergenceCriteria) && *convergenceCriteria <= 1.0;
- return false;
- }
-#ifdef PRINTOUT_TEST_APP
- else
- {
- std::cout << "LCP_3DContactFriction_GaussSeidel_Christian::solve converged after " <<
- nbLoop << " iterations" << std::endl;
- }
-#endif // PRINTOUT_TEST_APP
+ SURGSIM_LOG_IF(*convergenceCriteria >= sqrt(m_epsilonConvergence), m_logger, WARNING) <<
+ "Convergence criteria (" << *convergenceCriteria << ") is greater than " << sqrt(m_epsilonConvergence) <<
+ " at end of " << *iteration << " Gauss Seidel iterations.";
+ SURGSIM_LOG_IF(*convergenceCriteria > *initialConvergenceCriteria, m_logger, WARNING) <<
+ "Convergence criteria (" << *convergenceCriteria << ") is greater than before " << *iteration <<
+ " Gauss Seidel iterations (" << *initialConvergenceCriteria << ").";
+ SURGSIM_LOG_IF(!(*validSignorini), m_logger, WARNING) <<
+ "Signorini not verified after " << *iteration << " Gauss Seidel iterations.";
- return true;
+ return (SurgSim::Math::isValid(*convergenceCriteria) && *convergenceCriteria <= m_epsilonConvergence);
}
-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)
+void MlcpGaussSeidelSolver::calculateConvergenceCriteria(size_t problemSize, const MlcpProblem::Matrix& A,
+ const MlcpProblem::Vector& b,
+ const MlcpSolution::Vector& initialGuessAndSolution,
+ const std::vector<MlcpConstraintType>& constraintsType,
+ double constraintConvergenceCriteria[MLCP_NUM_CONSTRAINT_TYPES],
+ double* convergenceCriteria,
+ bool* validSignorini)
{
// Calculate initial convergence criteria.
-
- for (int i = 0; i < MLCP_NUM_CONSTRAINT_TYPES; i++)
+ for (size_t constraint = 0; constraint < MLCP_NUM_CONSTRAINT_TYPES; ++constraint)
{
- constraint_convergence_criteria[i] = 0.0;
+ constraintConvergenceCriteria[constraint] = 0.0;
}
- convergence_criteria = 0.0;
- signoriniVerified = true;
- signoriniValid = true;
-
- int currentAtomicIndex=0;
- int nbConstraints = static_cast<int>(constraintsType.size());
+ *convergenceCriteria = 0.0;
+ *validSignorini = true;
- int nbNonContactConstraints = 0;
+ size_t currentAtomicIndex = 0;
+ const size_t nbConstraints = constraintsType.size();
+ size_t nbNonContactConstraints = 0;
- for (int i=0 ; i<nbConstraints ; i++)
+ for (size_t constraint = 0; constraint < nbConstraints; ++constraint)
{
- switch (constraintsType[i])
+ switch (constraintsType[constraint])
{
- case MLCP_BILATERAL_1D_CONSTRAINT:
- {
- double violation = b[currentAtomicIndex]*subStep;
- //XXX REWRITE
- for (int j=0 ; j<n ; j++)
+ case MLCP_BILATERAL_1D_CONSTRAINT:
{
- violation += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
- }
- double criteria = sqrt(violation * violation);
- convergence_criteria += criteria;
- constraint_convergence_criteria[constraintsType[i]] += criteria;
+ const double criteria =
+ (b.segment<1>(currentAtomicIndex) + A.row(currentAtomicIndex) * initialGuessAndSolution).norm();
+ *convergenceCriteria += criteria;
+ constraintConvergenceCriteria[constraintsType[constraint]] += 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];
+ ++nbNonContactConstraints;
+ currentAtomicIndex += 1;
+ break;
}
- 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++)
+ case MLCP_BILATERAL_2D_CONSTRAINT:
{
- 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;
+ const double criteria = (b.segment<2>(currentAtomicIndex) +
+ A.block(currentAtomicIndex, 0, 2, problemSize) *
+ initialGuessAndSolution).norm();
+ *convergenceCriteria += criteria;
+ constraintConvergenceCriteria[constraintsType[constraint]] += 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];
+ ++nbNonContactConstraints;
+ currentAtomicIndex += 2;
+ break;
}
- // Enforce orthogonality condition
- if (! SurgSim::Math::isValid(violation) || violation < -m_contactTolerance ||
- (initialGuess_and_solution[currentAtomicIndex] > m_epsilonConvergence &&
- violation > m_contactTolerance))
+
+ case MLCP_BILATERAL_3D_CONSTRAINT:
{
- signoriniVerified=false;
+ const double criteria = (b.segment<3>(currentAtomicIndex) +
+ A.block(currentAtomicIndex, 0, 3, problemSize) *
+ initialGuessAndSolution).norm();
+ *convergenceCriteria += criteria;
+ constraintConvergenceCriteria[constraintsType[constraint]] += criteria;
+
+ ++nbNonContactConstraints;
+ currentAtomicIndex += 3;
+ break;
}
- }
- currentAtomicIndex++;
- break;
- case MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT:
- {
- double violation = b[currentAtomicIndex]*subStep;
- for (int j=0 ; j<n ; j++)
+ case MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT:
{
- violation += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ const double violation = b[currentAtomicIndex] +
+ A.row(currentAtomicIndex) * initialGuessAndSolution;
+ // Enforce orthogonality condition
+ if (!SurgSim::Math::isValid(violation) || violation < -m_contactTolerance ||
+ (initialGuessAndSolution[currentAtomicIndex] > m_epsilonConvergence &&
+ violation > m_contactTolerance))
+ {
+ *validSignorini = false;
+ }
+ currentAtomicIndex += 1;
+ break;
}
- // Enforce orthogonality condition
- if (! SurgSim::Math::isValid(violation) || violation < -m_contactTolerance ||
- (initialGuess_and_solution[currentAtomicIndex] > m_epsilonConvergence &&
- violation > m_contactTolerance))
+
+ case MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT:
{
- signoriniVerified=false;
+ const double violation = b[currentAtomicIndex] + A.row(currentAtomicIndex) * initialGuessAndSolution;
+ // Enforce orthogonality condition
+ if (!SurgSim::Math::isValid(violation) || violation < -m_contactTolerance ||
+ (initialGuessAndSolution[currentAtomicIndex] > m_epsilonConvergence &&
+ violation > m_contactTolerance))
+ {
+ *validSignorini = false;
+ }
+ currentAtomicIndex += 3;
+ break;
}
- }
- currentAtomicIndex+=3;
- break;
- case MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT:
- {
- double violation[2] = { b[currentAtomicIndex] , b[currentAtomicIndex+1] };
- for (int j=0 ; j<n ; j++)
+ case MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT:
{
- 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;
+ const double criteria = (b.segment<2>(currentAtomicIndex) +
+ A.block(currentAtomicIndex, 0, 2, problemSize) *
+ initialGuessAndSolution).norm();
+ *convergenceCriteria += criteria;
+ constraintConvergenceCriteria[constraintsType[constraint]] += criteria;
- nbNonContactConstraints++;
- }
- currentAtomicIndex+=2;
- break;
+ ++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++)
+ case MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT:
{
- 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;
+ // We verify that the sliding point is on the line...no matter what the friction violation is
+ // (3rd component)
+ const double criteria = (b.segment<2>(currentAtomicIndex) +
+ A.block(currentAtomicIndex, 0, 2, problemSize) *
+ initialGuessAndSolution).norm();
+ *convergenceCriteria += criteria;
+ constraintConvergenceCriteria[constraintsType[constraint]] += criteria;
- nbNonContactConstraints++;
- }
- currentAtomicIndex+=3;
- break;
+ ++nbNonContactConstraints;
+ currentAtomicIndex += 3;
+ break;
+ }
- default:
- //XXX
- SURGSIM_FAILURE() << "unknown constraint type [" << constraintsType[i] << "]";
- break;
+ default:
+ SURGSIM_FAILURE() << "unknown constraint type [" << constraintsType[constraint] << "]";
+ break;
}
}
- if (nbNonContactConstraints)
+ if (nbNonContactConstraints > 0)
{
- convergence_criteria /= nbNonContactConstraints; // normalize if necessary
+ *convergenceCriteria /= 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)
+ size_t problemSize, const MlcpProblem::Matrix& A, const MlcpProblem::Vector& b,
+ const MlcpSolution::Vector& initialGuessAndSolution,
+ const std::vector<MlcpConstraintType>& constraintsType,
+ size_t constraintID, size_t 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
+ const size_t nbConstraints = constraintsType.size();
+ // Total size of the system (number of line and column in the final matrix)
+ size_t systemSize = 0;
+ // Total size of the system counting only the {1D,2D,3D} bilateral constraints
+ size_t systemSizeWithoutConstraintID = 0;
// 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++)
+ bool done = false;
+ for (size_t 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;
+ 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;
@@ -467,30 +338,34 @@ void MlcpGaussSeidelSolver::computeEnforcementSystem(
// 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;
+ case MLCP_BILATERAL_1D_CONSTRAINT:
+ case MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT:
+ // The system will solve the normal contact, not the frictional parts !
+ case MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT:
+ {
+ systemSize += 1;
+ break;
+ }
+
+ case MLCP_BILATERAL_2D_CONSTRAINT:
+ case MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT:
+ // The system will solve the sliding case, not the frictional part !
+ case MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT:
+ {
+ systemSize += 2;
+ break;
+ }
+
+ case MLCP_BILATERAL_3D_CONSTRAINT:
+ {
+ systemSize += 3;
+ break;
+ }
+
+ default:
+ SURGSIM_LOG_SEVERE(m_logger) <<
+ "MlcpGaussSeidelSolver::computeEnforcementSystem Unknown constraint !?";
+ break;
}
}
m_numEnforcedAtomicConstraints = systemSize;
@@ -501,225 +376,81 @@ void MlcpGaussSeidelSolver::computeEnforcementSystem(
// 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];
- }
- }
+ m_rhsEnforcedLocalSystem.head(systemSizeWithoutConstraintID) = b.head(systemSizeWithoutConstraintID) +
+ A.block(0, 0, systemSizeWithoutConstraintID, problemSize) * initialGuessAndSolution;
+ m_lhsEnforcedLocalSystem.block(0, 0, systemSizeWithoutConstraintID, systemSizeWithoutConstraintID) =
+ A.block(0, 0, systemSizeWithoutConstraintID, systemSizeWithoutConstraintID);
// 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];
+ case MLCP_BILATERAL_1D_CONSTRAINT:
+ // 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] +
+ A.row(matrixEntryForConstraintID) * initialGuessAndSolution;
+ m_lhsEnforcedLocalSystem.block(0, systemSizeWithoutConstraintID, systemSizeWithoutConstraintID, 1) =
+ A.block(0, matrixEntryForConstraintID, systemSizeWithoutConstraintID, 1);
+ m_lhsEnforcedLocalSystem.block(systemSizeWithoutConstraintID, 0, 1, systemSizeWithoutConstraintID) =
+ A.block(matrixEntryForConstraintID, 0, 1, systemSizeWithoutConstraintID);
+ // Compliance part for the {contact|sliding}
+ m_lhsEnforcedLocalSystem(systemSizeWithoutConstraintID, systemSizeWithoutConstraintID) =
+ A(matrixEntryForConstraintID, matrixEntryForConstraintID);
+ break;
}
- }
- 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];
+ case MLCP_BILATERAL_2D_CONSTRAINT:
+ // 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.segment<2>(systemSizeWithoutConstraintID) =
+ b.segment<2>(matrixEntryForConstraintID) +
+ A.block(matrixEntryForConstraintID, 0, 2, problemSize) * initialGuessAndSolution;
+ m_lhsEnforcedLocalSystem.block(0, systemSizeWithoutConstraintID, systemSizeWithoutConstraintID, 2) =
+ A.block(0, matrixEntryForConstraintID, systemSizeWithoutConstraintID, 2);
+ m_lhsEnforcedLocalSystem.block(systemSizeWithoutConstraintID, 0, 2, systemSizeWithoutConstraintID) =
+ A.block(matrixEntryForConstraintID, 0, 2, systemSizeWithoutConstraintID);
+ // Compliance part for the {contact|sliding}
+ m_lhsEnforcedLocalSystem.block(systemSizeWithoutConstraintID, systemSizeWithoutConstraintID, 2, 2) =
+ A.block(matrixEntryForConstraintID, matrixEntryForConstraintID, 2, 2);
+ break;
}
- // 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++)
+ case MLCP_BILATERAL_3D_CONSTRAINT:
{
- m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID ] +=
- A(matrixEntryForConstraintID, column)*initialGuess_and_solution[column];
- m_rhsEnforcedLocalSystem[systemSizeWithoutConstraintID+1] +=
- A(matrixEntryForConstraintID+1, column)*initialGuess_and_solution[column];
+ // Coupling part (fill up LHS and RHS)
+ m_rhsEnforcedLocalSystem.segment<3>(systemSizeWithoutConstraintID) =
+ b.segment<3>(matrixEntryForConstraintID) +
+ A.block(matrixEntryForConstraintID, 0, 3, problemSize) * initialGuessAndSolution;
+ m_lhsEnforcedLocalSystem.block(0, systemSizeWithoutConstraintID, systemSizeWithoutConstraintID, 3) =
+ A.block(0, matrixEntryForConstraintID, systemSizeWithoutConstraintID, 3);
+ m_lhsEnforcedLocalSystem.block(systemSizeWithoutConstraintID, 0, 3, systemSizeWithoutConstraintID) =
+ A.block(matrixEntryForConstraintID, 0, 3, systemSizeWithoutConstraintID);
+ // Compliance part for the {contact|sliding}
+ m_lhsEnforcedLocalSystem.block(systemSizeWithoutConstraintID, systemSizeWithoutConstraintID, 3, 3) =
+ A.block(matrixEntryForConstraintID, matrixEntryForConstraintID, 3, 3);
+ break;
}
- }
- break;
-
- default:
- printf("MlcpGaussSeidelSolver::computeEnforcementSystem Unkown constraint !?\n");
- break;
+ default:
+ SURGSIM_LOG_SEVERE(m_logger) <<
+ "MlcpGaussSeidelSolver::computeEnforcementSystem Unknown constraint !?";
+ 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,
+static inline void solveSystem(const MlcpProblem::Matrix& A, const MlcpProblem::Vector& b, size_t size,
MlcpSolution::Vector* x)
{
MlcpProblem::Matrix AA = A.block(0, 0, size, size);
@@ -729,1036 +460,332 @@ static inline bool solveSystem(const MlcpProblem::Matrix& A, const MlcpProblem::
//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)
+void MlcpGaussSeidelSolver::doOneIteration(size_t problemSize, const MlcpProblem::Matrix& A,
+ const MlcpProblem::Vector& b,
+ MlcpSolution::Vector* initialGuessAndSolution,
+ const MlcpProblem::Vector& frictionCoefs,
+ const std::vector<MlcpConstraintType>& constraintsType,
+ double constraintConvergenceCriteria[MLCP_NUM_CONSTRAINT_TYPES],
+ double* convergenceCriteria, bool* validSignorini)
{
- for (int i = 0; i < MLCP_NUM_CONSTRAINT_TYPES; i++)
+ for (size_t constraint = 0; constraint < MLCP_NUM_CONSTRAINT_TYPES; ++constraint)
{
- constraint_convergence_criteria[i] = 0.0;
+ constraintConvergenceCriteria[constraint] = 0.0;
}
- convergence_criteria = 0.0;
- signoriniVerified=true;
+ *convergenceCriteria = 0.0;
+ *validSignorini = true;
- int currentAtomicIndex=0;
- int nbConstraints = static_cast<int>(constraintsType.size());
+ size_t currentAtomicIndex = 0;
+ size_t nbConstraints = constraintsType.size();
// For each constraint, we look if the constraint is violated or not !
- for (int i=0 ; i<nbConstraints ; i++)
+ for (size_t constraint = 0; constraint < nbConstraints; ++constraint)
{
- switch (constraintsType[i])
+ switch (constraintsType[constraint])
{
-
- //####################################
- //####################################
- case MLCP_BILATERAL_1D_CONSTRAINT:
- {
- double& F = (*initialGuess_and_solution)[currentAtomicIndex];
- double violation = b[currentAtomicIndex]*subStep;
- for (int j=0 ; j<n ; j++)
+ case MLCP_BILATERAL_1D_CONSTRAINT:
{
- violation += A(currentAtomicIndex, j) * (*initialGuess_and_solution)[j];
+ (*initialGuessAndSolution)[currentAtomicIndex] -=
+ (b[currentAtomicIndex] + A.row(currentAtomicIndex) * (*initialGuessAndSolution)) /
+ A(currentAtomicIndex, currentAtomicIndex);
+ ++currentAtomicIndex;
+ break;
}
- 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++)
+ case MLCP_BILATERAL_2D_CONSTRAINT:
{
- violation[0] += A(currentAtomicIndex, j) * (*initialGuess_and_solution)[j];
- violation[1] += A(currentAtomicIndex+1, j) * (*initialGuess_and_solution)[j];
+ (*initialGuessAndSolution).segment<2>(currentAtomicIndex) -=
+ A.block<2, 2>(currentAtomicIndex, currentAtomicIndex).inverse() *
+ (b.segment<2>(currentAtomicIndex) +
+ A.block(currentAtomicIndex, 0, 2, problemSize) * (*initialGuessAndSolution));
+ currentAtomicIndex += 2;
+ break;
}
- // 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++)
+ case MLCP_BILATERAL_3D_CONSTRAINT:
{
- 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];
+ (*initialGuessAndSolution).segment<3>(currentAtomicIndex) -=
+ A.block<3, 3>(currentAtomicIndex, currentAtomicIndex).inverse() *
+ (b.segment<3>(currentAtomicIndex) +
+ A.block(currentAtomicIndex, 0, 3, problemSize) * (*initialGuessAndSolution));
+ currentAtomicIndex += 3;
+ break;
}
- 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:
+ {
+ // Form the local system
+ computeEnforcementSystem(problemSize, A, b, *initialGuessAndSolution, constraintsType,
+ constraint, currentAtomicIndex);
- //####################################
- //####################################
- case MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT:
- {
- double& Fn = (*initialGuess_and_solution)[currentAtomicIndex];
+ // Solve A.f = violation
+ solveSystem(m_lhsEnforcedLocalSystem, m_rhsEnforcedLocalSystem, m_numEnforcedAtomicConstraints,
+ &m_rhsEnforcedLocalSystem);
- // Form the local system
- computeEnforcementSystem(n,A,nbColumnInA,b,(*initialGuess_and_solution),frictionCoefs,constraintsType,
- subStep, i, currentAtomicIndex);
+ // Correct the forces accordingly
+ (*initialGuessAndSolution).head(m_numEnforcedAtomicConstraints - 1) -=
+ m_rhsEnforcedLocalSystem.head(m_numEnforcedAtomicConstraints - 1);
- // Solve A.f = violation
- if (! solveSystem(m_lhsEnforcedLocalSystem, m_rhsEnforcedLocalSystem, m_numEnforcedAtomicConstraints,
- &m_rhsEnforcedLocalSystem))
- {
- return;
- }
+ double& Fn = (*initialGuessAndSolution)[currentAtomicIndex];
+ Fn -= m_rhsEnforcedLocalSystem[m_numEnforcedAtomicConstraints - 1];
- // Correct the forces accordingly
- for (int i=0 ; i<m_numEnforcedAtomicConstraints-1 ; i++)
- {
- (*initialGuess_and_solution)[i] -= m_rhsEnforcedLocalSystem[i];
+ if (Fn < 0.0)
+ {
+ Fn = 0; // inactive contact on normal
+ }
+ ++currentAtomicIndex;
+ break;
}
- Fn -= m_rhsEnforcedLocalSystem[m_numEnforcedAtomicConstraints-1];
- if (Fn<0.0)
+ case MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT:
{
- Fn = 0; // inactive contact on normal
- }
- }
- currentAtomicIndex++;
- break;
+ // Form the local system
+ computeEnforcementSystem(problemSize, A, b, *initialGuessAndSolution, constraintsType,
+ constraint, currentAtomicIndex);
+ // Solve A.f = violation
+ solveSystem(m_lhsEnforcedLocalSystem, m_rhsEnforcedLocalSystem, m_numEnforcedAtomicConstraints,
+ &m_rhsEnforcedLocalSystem);
- //####################################
- //####################################
- 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
+ (*initialGuessAndSolution).head(m_numEnforcedAtomicConstraints - 1) -=
+ m_rhsEnforcedLocalSystem.head(m_numEnforcedAtomicConstraints - 1);
- // 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];
+ double& Fn = (*initialGuessAndSolution)[currentAtomicIndex];
+ Eigen::VectorBlock<MlcpSolution::Vector, 2> Ft =
+ (*initialGuessAndSolution).segment<2>(currentAtomicIndex + 1);
+ 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++)
+ if (Fn > 0.0)
{
- violation[0] += A(currentAtomicIndex+1, i)*(*initialGuess_and_solution)[i];
- violation[1] += A(currentAtomicIndex+2, i)*(*initialGuess_and_solution)[i];
+ // Compute the frictions violation
+ Ft -= 2.0 * (b.segment<2>(currentAtomicIndex + 1) +
+ A.block(currentAtomicIndex + 1, 0, 2, problemSize) * (*initialGuessAndSolution)) /
+ (A(currentAtomicIndex + 1, currentAtomicIndex + 1) +
+ A(currentAtomicIndex + 2, currentAtomicIndex + 2));
+
+ const double maxFriction = frictionCoefs[currentAtomicIndex] * Fn;
+ if (Ft.norm() > maxFriction)
+ {
+ // Here, the Friction is too strong, we keep the direction, but modulate its length
+ // to verify the Coulomb's law: |Ft| = mu |Fn|
+ Ft = Ft.normalized() * maxFriction;
+ }
}
-
- 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)
+ else
{
- // 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;
+ Fn = 0; // inactive contact on normal
+ // inactive contact on tangent
+ Ft.setZero();
}
+ currentAtomicIndex += 3;
+ break;
}
- 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))
+ case MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT:
{
- 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;
+ // Form the local system
+ computeEnforcementSystem(problemSize, A, b, *initialGuessAndSolution, constraintsType,
+ constraint, currentAtomicIndex);
+ // Solve A.f = violation
+ solveSystem(m_lhsEnforcedLocalSystem, m_rhsEnforcedLocalSystem, m_numEnforcedAtomicConstraints,
+ &m_rhsEnforcedLocalSystem);
- //####################################
- //####################################
- 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
+ (*initialGuessAndSolution).head(m_numEnforcedAtomicConstraints - 2) -=
+ m_rhsEnforcedLocalSystem.head(m_numEnforcedAtomicConstraints - 2);
+ (*initialGuessAndSolution).segment<2>(currentAtomicIndex) -=
+ m_rhsEnforcedLocalSystem.segment<2>(m_numEnforcedAtomicConstraints - 2);
+ currentAtomicIndex += 2;
+ break;
}
- // Correct the forces accordingly
- for (int i=0 ; i<m_numEnforcedAtomicConstraints-2 ; i++)
+ case MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT:
{
- (*initialGuess_and_solution)[i] -= m_rhsEnforcedLocalSystem[i];
- }
- Fn1 -= m_rhsEnforcedLocalSystem[m_numEnforcedAtomicConstraints-2];
- Fn2 -= m_rhsEnforcedLocalSystem[m_numEnforcedAtomicConstraints-1];
+ // Form the local system
+ computeEnforcementSystem(problemSize, A, b, *initialGuessAndSolution, constraintsType,
+ constraint, currentAtomicIndex);
- // 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];
- }
+ // Solve A.f = violation
+ solveSystem(m_lhsEnforcedLocalSystem, m_rhsEnforcedLocalSystem, m_numEnforcedAtomicConstraints,
+ &m_rhsEnforcedLocalSystem);
- Ft -= violation/A(currentAtomicIndex+2, currentAtomicIndex+2);
+ // Correct the forces accordingly
+ (*initialGuessAndSolution).head(m_numEnforcedAtomicConstraints - 2) -=
+ m_rhsEnforcedLocalSystem.head(m_numEnforcedAtomicConstraints - 2);
+ Eigen::VectorBlock<MlcpSolution::Vector, 2> Fn =
+ (*initialGuessAndSolution).segment<2>(currentAtomicIndex);
+ Fn -= m_rhsEnforcedLocalSystem.segment<2>(m_numEnforcedAtomicConstraints - 2);
- double normFn = sqrt(Fn1*Fn1 + Fn2*Fn2);
- double normFt = fabs(Ft);
- if (normFt>local_mu*normFn)
+ // No Signorini to verify here, it is NOT a unilateral constraint, but bilateral
{
- // 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;
+ // Complete the violation of the friction along t, with the missing terms...
+ double& Ft = (*initialGuessAndSolution)[currentAtomicIndex + 2];
+ Ft -= (b[currentAtomicIndex + 2] + A.row(currentAtomicIndex + 2) * (*initialGuessAndSolution)) /
+ A(currentAtomicIndex + 2, currentAtomicIndex + 2);
+
+ const double maxFriction = frictionCoefs[currentAtomicIndex] * Fn.norm();
+ const double ftNorm = fabs(Ft);
+ if (ftNorm > maxFriction)
+ {
+ // Here, the Friction is too strong, we keep the direction, but modulate its length
+ // to verify the Coulomb's law: |Ft| = mu |Fn|
+ Ft *= maxFriction / ftNorm;
+ }
}
+
+ currentAtomicIndex += 3;
+ break;
}
-// // 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;
+ default:
+ SURGSIM_FAILURE() << "unknown constraint type [" << constraintsType[constraint] << "]";
+ 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)
+void MlcpGaussSeidelSolver::printViolationsAndConvergence(size_t problemSize,
+ const MlcpProblem::Matrix& A,
+ const MlcpProblem::Vector& b,
+ const MlcpSolution::Vector& initialGuessAndSolution,
+ const std::vector<MlcpConstraintType>& constraintsType,
+ double convergenceCriteria, bool validSignorini,
+ size_t iterations)
{
- printf("MLCP at iteration %d =\n",nbLoop);
+ SURGSIM_LOG_INFO(m_logger) << "MLCP at iteration " << iterations << " =";
- int currentAtomicIndex=0;
- int nbConstraints = static_cast<int>(constraintsType.size());
+ size_t currentAtomicIndex = 0;
+ size_t nbConstraints = constraintsType.size();
- for (int i=0 ; i<nbConstraints ; i++)
+ for (size_t constraint = 0; constraint < nbConstraints; ++constraint)
{
- 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:
+ switch (constraintsType[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++)
+ case MLCP_BILATERAL_1D_CONSTRAINT:
{
- 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 violation = b[currentAtomicIndex] + A.row(currentAtomicIndex) * initialGuessAndSolution;
+ SURGSIM_LOG_INFO(m_logger) << "Constraint [" << constraint << "] of type " <<
+ getMlcpConstraintTypeName(constraintsType[constraint]) <<
+ std::endl << "\t with initial violation b=(" << b[currentAtomicIndex] <<
+ ")" << std::endl <<
+ "\t with final violation b-Ax=(" << violation << ")" << std::endl <<
+ "\t force=(" << initialGuessAndSolution[currentAtomicIndex] << ")";
+ currentAtomicIndex += 1;
+ break;
}
- 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++)
+
+ case MLCP_BILATERAL_2D_CONSTRAINT:
{
- violation += A(currentAtomicIndex, j) * initialGuess_and_solution[j];
+ Vector2d violation = b.segment<2>(currentAtomicIndex) +
+ A.block(currentAtomicIndex, 0, 2, problemSize) * initialGuessAndSolution;
+ SURGSIM_LOG_INFO(m_logger) << "Constraint [" << constraint << "] of type " <<
+ getMlcpConstraintTypeName(constraintsType[constraint]) <<
+ std::endl << "\t with initial violation b=(" <<
+ b.segment<2>(currentAtomicIndex).transpose() << ") " << std::endl <<
+ "\t with final violation b-Ax=(" << violation.transpose() << ")" <<
+ std::endl << "\t force=(" <<
+ initialGuessAndSolution.segment<2>(currentAtomicIndex).transpose() << ")";
+ currentAtomicIndex += 2;
+ break;
}
- printf("\n\t with final violation b-Ax=(%g) ",violation);
- if (violation < -m_contactTolerance)
+
+ case MLCP_BILATERAL_3D_CONSTRAINT:
{
- printf("\n\t => normal violation = %g < -m_contactTolerance => Signorini not verified yet !",
- violation);
+ Vector3d violation = b.segment<3>(currentAtomicIndex) +
+ A.block(currentAtomicIndex, 0, 3, problemSize) * initialGuessAndSolution;
+ SURGSIM_LOG_INFO(m_logger) << "Constraint [" << constraint << "] of type " <<
+ getMlcpConstraintTypeName(constraintsType[constraint]) <<
+ std::endl << "\t with initial violation b=(" <<
+ b.segment<3>(currentAtomicIndex).transpose() << ")" << std::endl <<
+ "\t with final violation b-Ax=(" << violation.transpose() << ") " <<
+ std::endl << "\t force=(" <<
+ initialGuessAndSolution.segment<3>(currentAtomicIndex).transpose() << ")";
+ currentAtomicIndex += 3;
+ break;
}
- 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++)
+
+ case MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT:
{
- 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 violation = b[currentAtomicIndex] + A.row(currentAtomicIndex) * initialGuessAndSolution;
+ SURGSIM_LOG_INFO(m_logger) << "Constraint [" << constraint << "] of type " <<
+ getMlcpConstraintTypeName(constraintsType[constraint]) <<
+ std::endl << "\t with initial violation b=(" << b[currentAtomicIndex] <<
+ ") " << std::endl <<
+ "\t with final violation b-Ax=(" << violation << ") ";
+ if (violation < -m_contactTolerance)
+ {
+ SURGSIM_LOG_INFO(m_logger) << "\t => normal violation = " << violation <<
+ " < -m_contactTolerance => Signorini not verified yet !";
+ }
+ SURGSIM_LOG_INFO(m_logger) << "\t force=(" << initialGuessAndSolution[currentAtomicIndex] << ")";
+ currentAtomicIndex += 1;
+ break;
}
- printf("\n\t with final violation b-Ax=(%g %g %g) ",violation[0],violation[1],violation[2]);
- if (violation[0] < -m_contactTolerance)
+
+ case MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT:
{
- printf("\n\t => normal violation = %g < -contactTolerance => Signorini not verified yet !",
- violation[0]);
+ Vector3d violation = b.segment<3>(currentAtomicIndex) +
+ A.block(currentAtomicIndex, 0, 3, problemSize) * initialGuessAndSolution;
+ SURGSIM_LOG_INFO(m_logger) << "Constraint [" << constraint << "] of type " <<
+ getMlcpConstraintTypeName(constraintsType[constraint]) <<
+ std::endl << "\t with initial violation b=(" <<
+ b.segment<3>(currentAtomicIndex).transpose() << ")" <<
+ std::endl << "\t with final violation b-Ax=(" <<
+ violation.transpose() << ")";
+ if (violation[0] < -m_contactTolerance)
+ {
+ SURGSIM_LOG_INFO(m_logger) << "\t => normal violation = " << violation[0] <<
+ " < -contactTolerance => Signorini not verified yet !";
+ }
+ SURGSIM_LOG_INFO(m_logger) << "\t force=(" <<
+ initialGuessAndSolution.segment<3>(currentAtomicIndex).transpose() << ")";
+ currentAtomicIndex += 3;
+ break;
}
- 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];
+
+ case MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT:
+ {
+ Vector2d violation = b.segment<2>(currentAtomicIndex) +
+ A.block(currentAtomicIndex, 0, 2, problemSize) * initialGuessAndSolution;
+ getMlcpConstraintTypeName(constraintsType[constraint]);
+ SURGSIM_LOG_INFO(m_logger) << "Constraint [" << constraint << "] of type " <<
+ getMlcpConstraintTypeName(constraintsType[constraint]) <<
+ std::endl << "\t with initial violation b=(" <<
+ b.segment<2>(currentAtomicIndex).transpose() << ") " <<
+ std::endl << "\t with final violation b-Ax=(" <<
+ violation.transpose() << ") " << std::endl <<
+ "\t force=(" <<
+ initialGuessAndSolution.segment<2>(currentAtomicIndex).transpose() << ")";
+ currentAtomicIndex += 2;
+ break;
}
- 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++)
+
+ case MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT:
{
- 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];
+ Vector3d violation = b.segment<3>(currentAtomicIndex) +
+ A.block(currentAtomicIndex, 0, 3, problemSize) * initialGuessAndSolution;
+ SURGSIM_LOG_INFO(m_logger) << "Constraint [" << constraint << "] of type " <<
+ getMlcpConstraintTypeName(constraintsType[constraint]) <<
+ std::endl << "\t with initial violation b=(" <<
+ b.segment<3>(currentAtomicIndex).transpose() << ") " <<
+ std::endl << "\t with final violation b-Ax=(" <<
+ violation.transpose() << ")" << std::endl <<
+ "\t force=(" <<
+ initialGuessAndSolution.segment<3>(currentAtomicIndex).transpose() << ")";
+ currentAtomicIndex += 3;
+ break;
}
- 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;
+
+ default:
+ break;
}
- printf("\n");
}
- printf("convergence_criteria=%g Signorini verified=%s\n",
- convergence_criteria, (signorini_verified ? "yes" : "NO"));
+ SURGSIM_LOG_INFO(m_logger) << "convergence_criteria=" << convergenceCriteria << " Signorini verified=" <<
+ (validSignorini ? "yes" : "NO");
}
}; // namespace Math
diff --git a/SurgSim/Math/MlcpGaussSeidelSolver.h b/SurgSim/Math/MlcpGaussSeidelSolver.h
index b1b12a9..8444e98 100644
--- a/SurgSim/Math/MlcpGaussSeidelSolver.h
+++ b/SurgSim/Math/MlcpGaussSeidelSolver.h
@@ -16,20 +16,26 @@
#ifndef SURGSIM_MATH_MLCPGAUSSSEIDELSOLVER_H
#define SURGSIM_MATH_MLCPGAUSSSEIDELSOLVER_H
-#include "SurgSim/Math/MlcpSolver.h"
-#include <Eigen/Core>
+#include <memory.h>
+
+#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Math/MlcpProblem.h"
+#include "SurgSim/Math/MlcpSolver.h"
#include "SurgSim/Math/MlcpSolution.h"
+#include "SurgSim/Math/Vector.h"
namespace SurgSim
{
+namespace Framework
+{
+class Logger;
+}
+
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
@@ -47,145 +53,95 @@ namespace Math
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()
- {
- }
+ /// Constructor.
+ MlcpGaussSeidelSolver();
+
+ /// Constructor.
+ /// \param epsilonConvergence The precision.
+ /// \param contactTolerance The contact tolerance.
+ /// \param maxIterations The max iterations.
+ MlcpGaussSeidelSolver(double epsilonConvergence, double contactTolerance, size_t maxIterations);
+
+ /// Destructor.
+ virtual ~MlcpGaussSeidelSolver();
+
+ /// Resolution of a given MLCP (Gauss Seidel iterative solver)
+ /// \param problem The mlcp problem
+ /// \param [out] solution The mlcp solution
+ /// \return true if successfully converged.
+ bool solve(const MlcpProblem& problem, MlcpSolution* solution);
+ /// \return The precision.
+ double getEpsilonConvergence() const;
- bool solve(const MlcpProblem& problem, MlcpSolution* solution);
+ /// Set the precision.
+ /// \param precision The precision.
+ void setEpsilonConvergence(double precision);
+
+ /// \return The contact tolerance.
+ double getContactTolerance() const;
+ /// Set the contact tolerance.
+ /// \param tolerance The contact tolerance.
+ void setContactTolerance(double tolerance);
- 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;
- }
+ /// \return The max number of iterations.
+ size_t getMaxIterations() const;
+
+ /// Set the max number of iterations.
+ /// \param maxIterations The max number of iterations.
+ void setMaxIterations(size_t maxIterations);
private:
- void computeEnforcementSystem(int n, const MlcpProblem::Matrix& A, int nbColumnInA,
+ void computeEnforcementSystem(size_t problemSize, const MlcpProblem::Matrix& A,
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);
+ const MlcpSolution::Vector& initialGuessAndSolution,
+ const std::vector<MlcpConstraintType>& constraintsType,
+ size_t constraintID, size_t matrixEntryForConstraintID);
- void calculateConvergenceCriteria(int n, const MlcpProblem::Matrix& A, int nbColumnInA,
+ void calculateConvergenceCriteria(size_t problemSize, const MlcpProblem::Matrix& A,
const MlcpProblem::Vector& b,
- const MlcpSolution::Vector& initialGuess_and_solution,
+ const MlcpSolution::Vector& initialGuessAndSolution,
const std::vector<MlcpConstraintType>& constraintsType,
- double subStep,
- double constraint_convergence_criteria[MLCP_NUM_CONSTRAINT_TYPES],
- double& convergence_criteria,
- bool& signoriniVerified, bool& signoriniValid);
+ double constraintConvergenceCriteria[MLCP_NUM_CONSTRAINT_TYPES],
+ double* convergenceCriteria,
+ bool* validSignorini);
- void doOneIteration(int n, const MlcpProblem::Matrix& A, int nbColumnInA, const MlcpProblem::Vector& b,
- MlcpSolution::Vector* initialGuess_and_solution,
+ void doOneIteration(size_t problemSize, const MlcpProblem::Matrix& A,
+ const MlcpProblem::Vector& b,
+ MlcpSolution::Vector* initialGuessAndSolution,
const MlcpProblem::Vector& frictionCoefs,
- const std::vector<MlcpConstraintType>& constraintsType, double subStep,
- double constraint_convergence_criteria[MLCP_NUM_CONSTRAINT_TYPES], double& convergence_criteria,
- bool& signoriniVerified);
+ const std::vector<MlcpConstraintType>& constraintsType,
+ double constraintConvergenceCriteria[MLCP_NUM_CONSTRAINT_TYPES], double* convergenceCriteria,
+ bool* validSignorini);
- void printViolationsAndConvergence(int n, const MlcpProblem::Matrix& A, int nbColumnInA,
+ void printViolationsAndConvergence(size_t problemSize, const MlcpProblem::Matrix& A,
const MlcpProblem::Vector& b,
- const MlcpSolution::Vector& initialGuess_and_solution,
+ const MlcpSolution::Vector& initialGuessAndSolution,
const std::vector<MlcpConstraintType>& constraintsType,
- double subStep, double convergence_criteria,
- bool signorini_verified, int nbLoop);
+ double convergenceCriteria,
+ bool validSignorini, size_t iterations);
+ /// The precision.
+ double m_epsilonConvergence;
- typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
- typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
+ /// The contact tolerance.
+ double m_contactTolerance;
+ /// The maximum number of iterations
+ size_t m_maxIterations;
- double m_epsilonConvergence;
- double m_contactTolerance;
- double m_substepRatio;
- unsigned int m_maxIterations;
- bool m_catchExplodingConvergenceCriteria;
- bool m_verbose;
+ /// The number of atomic constraints, aka the system size.
+ size_t m_numEnforcedAtomicConstraints;
- int m_numEnforcedAtomicConstraints;
+ /// The left-hand side matrix.
Matrix m_lhsEnforcedLocalSystem;
+
+ /// The right-hand side vector.
Vector m_rhsEnforcedLocalSystem;
+
+ /// The logger.
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
};
}; // namespace Math
diff --git a/SurgSim/Math/MlcpProblem.cpp b/SurgSim/Math/MlcpProblem.cpp
index 5549169..4ae1fb0 100644
--- a/SurgSim/Math/MlcpProblem.cpp
+++ b/SurgSim/Math/MlcpProblem.cpp
@@ -26,12 +26,9 @@ 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();
+ A.setZero(numConstraintDof, numConstraintDof);
+ b.setZero(numConstraintDof);
+ mu.setZero(numConstraintDof);
constraintTypes.clear();
}
diff --git a/SurgSim/Math/MlcpProblem.h b/SurgSim/Math/MlcpProblem.h
index 29d9ad0..87045d3 100644
--- a/SurgSim/Math/MlcpProblem.h
+++ b/SurgSim/Math/MlcpProblem.h
@@ -95,8 +95,7 @@ struct MlcpProblem
{
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));
+ && (numConstraintTypes <= static_cast<size_t>(b.rows())) && (mu.size() >= 0));
}
/// Resize an MlcpProblem and set to zero.
diff --git a/SurgSim/Math/MlcpSolution.h b/SurgSim/Math/MlcpSolution.h
index a037325..e10d15a 100644
--- a/SurgSim/Math/MlcpSolution.h
+++ b/SurgSim/Math/MlcpSolution.h
@@ -38,7 +38,7 @@ struct MlcpSolution
Vector x;
/// The number of iterations performed.
- int numIterations;
+ size_t numIterations;
/// True if the final value of the convergence criteria is valid.
bool validConvergence;
/// True if the final solution satisfies the Signorini conditions.
diff --git a/SurgSim/Math/OctreeShape.cpp b/SurgSim/Math/OctreeShape.cpp
index 80c1e2e..cbd33fc 100644
--- a/SurgSim/Math/OctreeShape.cpp
+++ b/SurgSim/Math/OctreeShape.cpp
@@ -16,6 +16,8 @@
#include "SurgSim/Math/OctreeShape.h"
#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Framework/Asset.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
namespace SurgSim
{
@@ -24,26 +26,37 @@ namespace Math
{
SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::OctreeShape, OctreeShape);
-OctreeShape::OctreeShape()
+OctreeShape::OctreeShape() :
+ m_rootNode(std::make_shared<NodeType>())
{
- serializeFileName(this);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(
+ SurgSim::Math::OctreeShape,
+ std::shared_ptr<SurgSim::Framework::Asset>,
+ Octree,
+ getOctree,
+ setOctree);
+
+ SURGSIM_ADD_SETTER(OctreeShape, std::string, OctreeFileName, loadOctree);
}
OctreeShape::~OctreeShape()
{
}
-int OctreeShape::getType()
+int OctreeShape::getType() const
{
return SHAPE_TYPE_OCTREE;
}
-bool OctreeShape::doLoad(const std::string& filePath)
+void OctreeShape::loadOctree(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;
+ auto rootNode = std::make_shared<NodeType>();
+ rootNode->load(filePath);
+
+ SURGSIM_ASSERT(isValid(rootNode))
+ << "Loading failed " << filePath << " contains an invalid octree.";
+
+ setOctree(rootNode);
}
double OctreeShape::getVolume() const
@@ -63,25 +76,43 @@ Matrix33d OctreeShape::getSecondMomentOfVolume() const
return Matrix33d::Zero();
}
-std::shared_ptr<OctreeShape::NodeType> OctreeShape::getRootNode()
+std::shared_ptr<OctreeShape::NodeType> OctreeShape::getOctree()
{
return m_rootNode;
}
-const std::shared_ptr<const OctreeShape::NodeType> OctreeShape::getRootNode() const
+void OctreeShape::setOctree(std::shared_ptr<SurgSim::Framework::Asset> node)
{
- return m_rootNode;
+ SURGSIM_ASSERT(node != nullptr) << "Tried to set the shape with a nullptr";
+ auto octreeNode = std::dynamic_pointer_cast<NodeType>(node);
+ SURGSIM_ASSERT(octreeNode != nullptr)
+ << "OctreeShape needs OctreeNode<SurgSim::DataStructures::EmptyData> but received " << node->getClassName();
+ SURGSIM_ASSERT(isValid(octreeNode))
+ << "OctreeShape was passed an invalid Octree.";
+ m_rootNode = octreeNode;
}
-void OctreeShape::setRootNode(std::shared_ptr<OctreeShape::NodeType> node)
+bool OctreeShape::isValid(std::shared_ptr<NodeType> node) const
{
- m_rootNode = node;
+ return (nullptr != node) && (node->getBoundingBox().sizes().minCoeff() >= 0);
}
bool OctreeShape::isValid() const
{
- return (nullptr != m_rootNode) && (m_rootNode->getBoundingBox().sizes().minCoeff() >= 0);
+ return isValid(m_rootNode);
+}
+
+const Math::Aabbd& OctreeShape::getBoundingBox() const
+{
+ if (m_rootNode != nullptr)
+ {
+ return m_rootNode->getBoundingBox();
+ }
+ else
+ {
+ return m_aabb;
+ }
}
}; // namespace Math
-}; // namespace SurgSim
\ No newline at end of file
+}; // namespace SurgSim
diff --git a/SurgSim/Math/OctreeShape.h b/SurgSim/Math/OctreeShape.h
index c41163b..29c7918 100644
--- a/SurgSim/Math/OctreeShape.h
+++ b/SurgSim/Math/OctreeShape.h
@@ -31,7 +31,7 @@ SURGSIM_STATIC_REGISTRATION(OctreeShape);
/// Octree Shape
/// A defined by an octree data structure
-class OctreeShape : public Shape, public SurgSim::Framework::Asset
+class OctreeShape : public Shape
{
public:
typedef SurgSim::DataStructures::OctreeNode<SurgSim::DataStructures::EmptyData> NodeType;
@@ -53,39 +53,42 @@ public:
virtual ~OctreeShape();
/// \return the type of shape
- virtual int getType() override;
+ int getType() const override;
/// Get the volume of the shape
/// \return The volume of the shape (in m-3)
- virtual double getVolume() const override;
+ double getVolume() const override;
/// Get the volumetric center of the shape
/// \return The center of the shape
- virtual Vector3d getCenter() const override;
+ 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;
+ 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;
+ std::shared_ptr<NodeType> getOctree();
/// Set the root node
/// \param node the octree root node of this shape
- void setRootNode(std::shared_ptr<NodeType> node);
+ void setOctree(std::shared_ptr<SurgSim::Framework::Asset> node);
/// \return True if the bounding box is bigger than or equal to 0; Otherwise, false.
- virtual bool isValid() const override;
+ bool isValid() const override;
+
+ void loadOctree(const std::string& filePath);
- virtual bool doLoad(const std::string& filePath) override;
+ const Math::Aabbd& getBoundingBox() const override;
private:
+
+ /// \param node the OctreeNode to check
+ /// \return True if the bounding box is bigger than or equal to 0; Otherwise, false.
+ bool isValid(std::shared_ptr<NodeType> node) const;
+
/// Root node of the octree datastructure
std::shared_ptr<NodeType> m_rootNode;
};
@@ -95,4 +98,4 @@ private:
#include "SurgSim/Math/OctreeShape-inl.h"
-#endif // SURGSIM_MATH_OCTREESHAPE_H
\ No newline at end of file
+#endif // SURGSIM_MATH_OCTREESHAPE_H
diff --git a/SurgSim/Math/OdeEquation.cpp b/SurgSim/Math/OdeEquation.cpp
index 8dd8a8e..140de33 100644
--- a/SurgSim/Math/OdeEquation.cpp
+++ b/SurgSim/Math/OdeEquation.cpp
@@ -15,6 +15,7 @@
#include "SurgSim/Math/OdeEquation.h"
#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Framework/Log.h"
namespace SurgSim
{
@@ -27,6 +28,56 @@ const std::shared_ptr<OdeState> OdeEquation::getInitialState() const
return m_initialState;
}
+const Vector& OdeEquation::getF() const
+{
+ return m_f;
+}
+
+const SparseMatrix& OdeEquation::getM() const
+{
+ return m_M;
+}
+
+const SparseMatrix& OdeEquation::getD() const
+{
+ return m_D;
+}
+
+const SparseMatrix& OdeEquation::getK() const
+{
+ return m_K;
+}
+
+void OdeEquation::updateFMDK(const OdeState& state, int options)
+{
+ if (options == ODEEQUATIONUPDATE_FMDK)
+ {
+ computeFMDK(state);
+ }
+ else
+ {
+ if (options & ODEEQUATIONUPDATE_F)
+ {
+ computeF(state);
+ }
+
+ if (options & ODEEQUATIONUPDATE_M)
+ {
+ computeM(state);
+ }
+
+ if (options & ODEEQUATIONUPDATE_D)
+ {
+ computeD(state);
+ }
+
+ if (options & ODEEQUATIONUPDATE_K)
+ {
+ computeK(state);
+ }
+ }
+}
+
}; // namespace Math
}; // namespace SurgSim
diff --git a/SurgSim/Math/OdeEquation.h b/SurgSim/Math/OdeEquation.h
index 9dab154..78a1fff 100644
--- a/SurgSim/Math/OdeEquation.h
+++ b/SurgSim/Math/OdeEquation.h
@@ -18,6 +18,7 @@
#include <memory>
+#include "SurgSim/Math/SparseMatrix.h"
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Math/Vector.h"
@@ -29,63 +30,101 @@ 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
+/// Enum to identify which of the data need to be updated by the OdeEquation::update()
+enum OdeEquationUpdate
+{
+ ODEEQUATIONUPDATE_F = 1<<0,
+ ODEEQUATIONUPDATE_M = 1<<1,
+ ODEEQUATIONUPDATE_D = 1<<2,
+ ODEEQUATIONUPDATE_K = 1<<3,
+ ODEEQUATIONUPDATE_FMDK = ODEEQUATIONUPDATE_F | ODEEQUATIONUPDATE_M | ODEEQUATIONUPDATE_D | ODEEQUATIONUPDATE_K
+};
+
+/// Ode equation of 2nd order of the form \f$M(x,v).a = F(x, v)\f$ with \f$(x0, v0)\f$ 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) )
+/// This ode equation is solved as an ode of order 1 by defining the state vector
+/// \f$y = \left(\begin{array}{c}x\\v\end{array}\right)\f$:
+/// \f[
+/// y' = \left(\begin{array}{c} x' \\ v' \end{array}\right) =
+/// \left(\begin{array}{c} v \\ M(x, v)^{-1}.f(t, x, v) \end{array}\right)
+/// \f]
/// \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 \f$M(x, v)\f$, \f$f(t, x, v)\f$ but also \f$K = -dF/dx(x, v)\f$ and \f$D = -dF/dv(x, v)\f$
/// \note Models wanting the use of implicit solvers will need to compute these Jacobian matrices.
class OdeEquation
{
public:
/// Virtual destructor
- virtual ~OdeEquation(){}
+ virtual ~OdeEquation() {}
- /// Retrieves the ode initial conditions (x0, v0) (i.e. the initial state)
+ /// Retrieves the ode initial conditions \f$(x0, v0)\f$ (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)
+ /// Calculate the product C.b where C is the compliance matrix with boundary conditions
+ /// applied. Note that this can be rewritten as (Bt)(M^-1)(B.b) = (Bt)((M^-1)(B.b)) = x,
+ /// where (M^-1)(B.b) = y is simply the solution to M.y = B.b and Bt.y = x.
+ /// \param state \f$(x, v)\f$ the current position and velocity to evaluate the various terms with
+ /// \param b The input matrix
+ /// \return The matrix \f$C.b\f$
+ virtual Matrix applyCompliance(const OdeState& state, const Matrix& b) = 0;
+
+ /// Update the OdeEquation (and support data) based on the given state.
+ /// \param state \f$(x, v)\f$ the current position and velocity to evaluate the various terms with
+ /// \param options Flag to specify which of F, M, D, K needs to be updated.
+ virtual void updateFMDK(const OdeState& state, int options);
+
+ /// \return The vector containing \f$f(x, v)\f$
+ const Vector& getF() const;
+
+ /// \return The matrix \f$M(x,v)\f$
+ const SparseMatrix& getM() const;
+
+ /// \return The matrix \f$D = -\frac{\partial f}{\partial v}(x,v)\f$
+ const SparseMatrix& getD() const;
+
+ /// \return The matrix \f$K = -\frac{\partial f}{\partial x}(x,v)\f$
+ const SparseMatrix& getK() const;
+
+protected:
+ /// Evaluation of the RHS function \f$f(x, v)\f$ for a given state
+ /// \param state \f$(x, v)\f$ the current position and velocity to evaluate the function \f$f(x,v)\f$ with
+ virtual void computeF(const OdeState& state) = 0;
+
+ /// Evaluation of the LHS matrix \f$M(x,v)\f$ for a given state
+ /// \param state \f$(x, v)\f$ the current position and velocity to evaluate the matrix \f$M(x,v)\f$ with
+ virtual void computeM(const OdeState& state) = 0;
+
+ /// Evaluation of \f$D = -\frac{\partial f}{\partial v}(x,v)\f$ for a given state
+ /// \param state \f$(x, v)\f$ the current position and velocity to evaluate the Jacobian matrix with
+ virtual void computeD(const OdeState& state) = 0;
+
+ /// Evaluation of \f$K = -\frac{\partial f}{\partial x}(x,v)\f$ for a given state
+ /// \param state \f$(x, v)\f$ the current position and velocity to evaluate the Jacobian matrix with
+ virtual void computeK(const OdeState& state) = 0;
+
+ /// Evaluation of \f$f(x,v)\f$, \f$M(x,v)\f$, \f$D = -\frac{\partial f}{\partial v}(x,v)\f$ and
+ /// \f$K = -\frac{\partial f}{\partial x}(x,v)\f$.
/// 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
+ /// \param state \f$(x, v)\f$ the current position and velocity to evaluate the various terms with
/// \note computeF(), computeM(), computeD(), computeK()
- virtual void computeFMDK(const OdeState& state, Vector** f, Matrix** M, Matrix** D, Matrix** K) = 0;
+ virtual void computeFMDK(const OdeState& state) = 0;
-protected:
- /// The initial state (which defines the ODE initial conditions (x0, v0))
+ /// The initial state (which defines the ODE initial conditions \f$(x0, v0)\f$)
/// \note MUST be set by the derived classes
std::shared_ptr<OdeState> m_initialState;
+
+ /// The vector containing \f$f(x, v)\f$
+ Vector m_f;
+
+ /// The matrix \f$M(x,v)\f$
+ SparseMatrix m_M;
+
+ /// The The matrix \f$D = -\frac{\partial f}{\partial v}(x,v)\f$
+ SparseMatrix m_D;
+
+ /// The The matrix \f$K = -\frac{\partial f}{\partial x}(x,v)\f$
+ SparseMatrix m_K;
};
}; // namespace Math
diff --git a/SurgSim/Math/OdeSolver.cpp b/SurgSim/Math/OdeSolver.cpp
index e9c7343..4d467c7 100644
--- a/SurgSim/Math/OdeSolver.cpp
+++ b/SurgSim/Math/OdeSolver.cpp
@@ -24,10 +24,8 @@ namespace Math
OdeSolver::OdeSolver(OdeEquation* equation) : m_equation(*equation)
{
- allocate(m_equation.getInitialState()->getPositions().size());
-
// Default linear solver
- setLinearSolver(std::make_shared<LinearSolveAndInverseDenseMatrix>());
+ setLinearSolver(std::make_shared<LinearSparseSolveAndInverseLU>());
}
const std::string OdeSolver::getName() const
@@ -35,30 +33,46 @@ const std::string OdeSolver::getName() const
return m_name;
}
-void OdeSolver::setLinearSolver(std::shared_ptr<LinearSolveAndInverse> linearSolver)
+void OdeSolver::setLinearSolver(std::shared_ptr<LinearSparseSolveAndInverse> linearSolver)
{
m_linearSolver = linearSolver;
}
-std::shared_ptr<LinearSolveAndInverse> OdeSolver::getLinearSolver() const
+std::shared_ptr<LinearSparseSolveAndInverse> OdeSolver::getLinearSolver() const
{
return m_linearSolver;
}
-const Matrix& OdeSolver::getSystemMatrix() const
+const SparseMatrix& OdeSolver::getSystemMatrix() const
{
return m_systemMatrix;
}
-const Matrix& OdeSolver::getCompliance() const
+const Matrix& OdeSolver::getComplianceMatrix() const
+{
+ return m_complianceMatrix;
+}
+
+
+void OdeSolver::computeMatrices(double dt, const OdeState& state, bool computeCompliance)
{
- return m_compliance;
+ /// Compute the system matrix (and discard the RHS calculation)
+ assembleLinearSystem(dt, state, state, false);
+
+ /// Compute the compliance matrix
+ if (computeCompliance)
+ {
+ computeComplianceMatrixFromSystemMatrix(state);
+ }
}
-void OdeSolver::allocate(size_t size)
+void OdeSolver::computeComplianceMatrixFromSystemMatrix(const OdeState& state)
{
- m_systemMatrix.resize(size, size);
- m_compliance.resize(size, size);
+ // The compliance matrix is the inverse of the system matrix
+ m_complianceMatrix = m_linearSolver->getInverse();
+ // The boundary conditions needs to be set on the compliance matrix and no compliance should be used for the nodes
+ // Which means that the compliance matrix has entire rows and columns of zeros for the boundary conditions.
+ state.applyBoundaryConditionsToMatrix(&m_complianceMatrix, false);
}
}; // namespace Math
diff --git a/SurgSim/Math/OdeSolver.h b/SurgSim/Math/OdeSolver.h
index 448481f..59f1a0f 100644
--- a/SurgSim/Math/OdeSolver.h
+++ b/SurgSim/Math/OdeSolver.h
@@ -21,7 +21,7 @@
#include <boost/assign/list_of.hpp> // for 'map_list_of()'
-#include "SurgSim/Math/LinearSolveAndInverse.h"
+#include "SurgSim/Math/LinearSparseSolveAndInverse.h"
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Math/OdeEquation.h"
@@ -33,41 +33,46 @@ namespace Math
/// The diverse numerical integration scheme supported
/// Each Ode Solver should have its own entry in this enum
-enum IntegrationScheme {
+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_EULER_EXPLICIT,
+ INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT,
+ INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED,
+ INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT_MODIFIED,
+ INTEGRATIONSCHEME_EULER_IMPLICIT,
+ INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT,
INTEGRATIONSCHEME_RUNGE_KUTTA_4,
- INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4
+ INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4,
+ MAX_INTEGRATIONSCHEMES
};
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))
+ boost::assign::map_list_of
+ (INTEGRATIONSCHEME_STATIC, "INTEGRATIONSCHEME_STATIC")
+ (INTEGRATIONSCHEME_LINEAR_STATIC, "INTEGRATIONSCHEME_LINEAR_STATIC")
+ (INTEGRATIONSCHEME_EULER_EXPLICIT, "INTEGRATIONSCHEME_EULER_EXPLICIT")
+ (INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT, "INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT")
+ (INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED, "INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED")
+ (INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT_MODIFIED, "INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT_MODIFIED")
+ (INTEGRATIONSCHEME_EULER_IMPLICIT, "INTEGRATIONSCHEME_EULER_IMPLICIT")
+ (INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT, "INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT")
+ (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 \f$M(x(t), v(t)).a(t) = f(t, x(t), v(t))\f$. <br>
+/// This ode equation is solved as an ode of order 1 by defining the state vector
+/// \f$y = \left(\begin{array}{c}x\\v\end{array}\right)\f$:
+/// \f[
+/// y' = \left(\begin{array}{c} x' \\ v' \end{array}\right) =
+/// \left(\begin{array}{c} v \\ M(x, v)^{-1}.f(t, x, v) \end{array}\right)
+/// \f]
+/// \note To allow the use of explicit and implicit solver, we need to be able to evaluate:
+/// \note \f$M(x(t), v(t))\f$
+/// \note \f$f(t, x(t), v(t))\f$ but also
+/// \note \f$K = -\frac{\partial f}{\partial x}(x(t), v(t))\f$
+/// \note \f$D = -\frac{\partial f}{\partial v}(x(t), v(t))\f$
/// \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
@@ -87,30 +92,48 @@ public:
/// 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);
+ void setLinearSolver(std::shared_ptr<LinearSparseSolveAndInverse> 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;
+ std::shared_ptr<LinearSparseSolveAndInverse> 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;
+ /// \param computeCompliance True to explicitly compute the compliance matrix, False otherwise
+ virtual void solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance = true) = 0;
+
+ /// Computes the system and compliance matrices for a given state
+ /// \param dt The time step
+ /// \param state The state to compute the system and compliance matrices for
+ /// \param computeCompliance True to explicitly compute the compliance matrix, False otherwise
+ void computeMatrices(double dt, const OdeState& state, bool computeCompliance = true);
/// Queries the current system matrix
/// \return The latest system matrix calculated
- const Matrix& getSystemMatrix() const;
+ const SparseMatrix& getSystemMatrix() const;
- /// Queries the current compliance matrix
- /// \return The latest compliance matrix calculated
- const Matrix& getCompliance() const;
+ /// \return The latest compliance matrix computed (either by calling solve or computeMatrices)
+ const Matrix& getComplianceMatrix() const;
protected:
- /// Allocates the system and compliance matrices
- /// \param size The size to account for in the data structure
- void allocate(size_t size);
+ /// Assemble the linear system (A.x=b) to be solved for the state and new states (useful for certain ode solver).
+ /// \param dt The time step used in the system
+ /// \param state, newState The state and newState to be used to evaluate the system
+ /// \param computeRHS True to compute the RHS vector, False otherwise
+ /// \note The method should fill up the LHS matrix in m_systemMatrix and the RHS vector in m_b (if requested)
+ /// \note The method should take care of the boundary conditions properly on both the matrix and the vector.
+ /// \note The method should prepare the linear solver m_linearSolver to be used with the m_systemMatrix
+ virtual void assembleLinearSystem(double dt, const OdeState& state, const OdeState& newState,
+ bool computeRHS = true) = 0;
+
+ /// Helper method computing the compliance matrix from the system matrix and setting the boundary conditions
+ /// \param state The state describing the boundary conditions
+ /// \note The full system is not re-evaluated from the state, the current m_systemMatrix is directly used.
+ /// \note This method supposes that the linear solver has been updated with the current m_systemMatrix.
+ void computeComplianceMatrixFromSystemMatrix(const OdeState& state);
/// Name for this solver
/// \note MUST be set by the derived classes
@@ -120,16 +143,19 @@ protected:
OdeEquation& m_equation;
/// The specialized linear solver to use when solving the ode equation
- std::shared_ptr<LinearSolveAndInverse> m_linearSolver;
+ std::shared_ptr<LinearSparseSolveAndInverse> m_linearSolver;
+
+ /// Linear system matrix (can be M, K, combination of MDK depending on the solver), including boundary conditions
+ /// \note A static solver will have K for system matrix
+ /// \note A dynamic explicit solver will have M for system matrix
+ /// \note A dynamic implicit solver will have a combination of M, D and K for system matrix
+ SparseMatrix m_systemMatrix;
- /// 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;
+ /// Linear system solution and rhs vectors (including boundary conditions)
+ Vector m_solution, m_rhs;
- /// Compliance matrix which is the inverse of the system matrix
- Matrix m_compliance;
+ /// Compliance matrix which is the inverse of the system matrix, including boundary conditions
+ Matrix m_complianceMatrix;
};
}; // namespace Math
diff --git a/SurgSim/Math/OdeSolverEulerExplicit.cpp b/SurgSim/Math/OdeSolverEulerExplicit.cpp
index 9d1dd59..a63e003 100644
--- a/SurgSim/Math/OdeSolverEulerExplicit.cpp
+++ b/SurgSim/Math/OdeSolverEulerExplicit.cpp
@@ -28,35 +28,61 @@ OdeSolverEulerExplicit::OdeSolverEulerExplicit(OdeEquation* equation)
m_name = "Ode Solver Euler Explicit";
}
-void OdeSolverEulerExplicit::solve(double dt, const OdeState& currentState, OdeState* newState)
+void OdeSolverEulerExplicit::solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance)
{
// 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))
+ // Using Euler explicit { v(t+dt) = v(t) + dt.a(t)
+ // { x(t+dt) = x(t) + dt.v(t)
+ // The resulting linear system on the velocity level is:
+ // (M/dt) . deltaV = f(t, x(t), v(t))
+ // systemMatrix . solution = rhs
+ // Therefore, systemMatrix = M/dt, solution = deltaV and rhs = f
- // Computes f(t, x(t), v(t)) and M
- Vector& f = m_equation.computeF(currentState);
- const Matrix& M = m_equation.computeM(currentState);
+ // Assemble the linear system systemMatrix.solution = rhs
+ assembleLinearSystem(dt, currentState, *newState);
- // Computes the system matrix (left-hand-side matrix)
- m_systemMatrix = M / dt;
+ // Solve the linear system to find solution = deltaV
+ m_solution = m_linearSolver->solve(m_rhs);
- // Apply boundary conditions to the linear system
- currentState.applyBoundaryConditionsToVector(&f);
- currentState.applyBoundaryConditionsToMatrix(&m_systemMatrix);
+ // Compute the new state using the Euler Explicit scheme:
+ newState->getPositions() = currentState.getPositions() + dt * currentState.getVelocities();
+ newState->getVelocities() = currentState.getVelocities() + m_solution;
- // Computes deltaV (stored in the velocities) and m_compliance = 1/m_systemMatrix
- Vector& deltaV = newState->getVelocities();
- (*m_linearSolver)(m_systemMatrix, f, &deltaV, &m_compliance);
+ if (computeCompliance)
+ {
+ computeComplianceMatrixFromSystemMatrix(currentState);
+ }
+}
- // 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);
+void OdeSolverEulerExplicit::assembleLinearSystem(double dt, const OdeState& state, const OdeState& newState,
+ bool computeRHS)
+{
+ // General equation to solve:
+ // M.a(t) = f(t, x(t), v(t))
+ // Using Euler explicit { v(t+dt) = v(t) + dt.a(t)
+ // { x(t+dt) = x(t) + dt.v(t)
+ // The resulting linear system on the velocity level is:
+ // (M/dt) . deltaV = f(t, x(t), v(t))
+ // systemMatrix . solution = rhs
+ // Therefore, systemMatrix = M/dt, solution = deltaV and rhs = f
- // Compute the new state using the Euler Explicit scheme:
- newState->getPositions() = currentState.getPositions() + dt * currentState.getVelocities();
- newState->getVelocities() = currentState.getVelocities() + deltaV;
+ // Update the stiffness matrix
+ m_equation.updateFMDK(state, ODEEQUATIONUPDATE_F | ODEEQUATIONUPDATE_M);
+
+ // Computes the LHS systemMatrix
+ m_systemMatrix = m_equation.getM() / dt;
+ state.applyBoundaryConditionsToMatrix(&m_systemMatrix);
+
+ // Feed the systemMatrix to the linear solver, so it can be used after this call to solve or inverse the matrix
+ m_linearSolver->setMatrix(m_systemMatrix);
+
+ // Computes the RHS vector
+ if (computeRHS)
+ {
+ m_rhs = m_equation.getF();
+ state.applyBoundaryConditionsToVector(&m_rhs);
+ }
}
}; // namespace Math
diff --git a/SurgSim/Math/OdeSolverEulerExplicit.h b/SurgSim/Math/OdeSolverEulerExplicit.h
index a61614f..28ed0c2 100644
--- a/SurgSim/Math/OdeSolverEulerExplicit.h
+++ b/SurgSim/Math/OdeSolverEulerExplicit.h
@@ -24,17 +24,35 @@ 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)
+/// Euler Explicit ode solver solves the following \f$2^{nd}\f$ order ode
+/// \f$M(x(t), v(t)).a(t) = f(t, x(t), v(t))\f$.
+/// This ode is solved as an ode of order 1 by defining the state vector
+/// \f$y = \left(\begin{array}{c}x\\v\end{array}\right)\f$:
+/// \f[
+/// y' = \left(\begin{array}{c} x' \\ v' \end{array}\right) =
+/// \left(\begin{array}{c} v \\ M(x, v)^{-1}.f(t,x, v) \end{array}\right) =
+/// f(t, y)
+/// \f]
+/// After integrating this equation, we get:
+/// \f[ y(t+dt) - y(t) = \int_t^{t+dt} f(t,y) dt \f]
+/// \note Euler explicit uses a rectangular numerical integration on the left to evaluate this integral, leading to
+/// \f$ \int_t^{t+dt} f(t,y) dt \simeq dt.f(t, y(t))\f$, therefore:
+/// \f[
+/// \begin{array}{ccc}
+/// y(t+dt) - y(t) = dt.f(t, y(t))
+/// &
+/// \Leftrightarrow
+/// &
+/// \left\{
+/// \begin{array}{ccccl}
+/// x(t+dt) &=& x(t) &+& dt.v(t)
+/// \\ v(t+dt) &=& v(t) &+& dt.a(t)
+/// \end{array}
+/// \right.
+/// \end{array}
+/// \f]
+/// \note Euler Explicit is also known as forward Euler as it uses a forward evaluation of the derivative
+/// \f$y' = (y(t+dt) - y(t)) / dt\f$ which leads to the same result.
class OdeSolverEulerExplicit : public OdeSolver
{
public:
@@ -42,7 +60,11 @@ public:
/// \param equation The ode equation to be solved
explicit OdeSolverEulerExplicit(OdeEquation* equation);
- virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+ void solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance = true) override;
+
+protected:
+ void assembleLinearSystem(double dt, const OdeState& state, const OdeState& newState,
+ bool computeRHS = true) override;
};
}; // namespace Math
diff --git a/SurgSim/Math/OdeSolverEulerExplicitModified.cpp b/SurgSim/Math/OdeSolverEulerExplicitModified.cpp
index 473a7fe..03e03ac 100644
--- a/SurgSim/Math/OdeSolverEulerExplicitModified.cpp
+++ b/SurgSim/Math/OdeSolverEulerExplicitModified.cpp
@@ -28,35 +28,61 @@ OdeSolverEulerExplicitModified::OdeSolverEulerExplicitModified(OdeEquation* equa
m_name = "Ode Solver Euler Explicit Modified";
}
-void OdeSolverEulerExplicitModified::solve(double dt, const OdeState& currentState, OdeState* newState)
+void OdeSolverEulerExplicitModified::solve(double dt, const OdeState& currentState, OdeState* newState,
+ bool computeCompliance)
{
// 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))
+ // Using Euler explicit { v(t+dt) = v(t) + dt.a(t)
+ // { x(t+dt) = x(t) + dt.v(t+dt)
+ // The resulting linear system on the velocity level is:
+ // (M/dt) . deltaV = f(t, x(t), v(t))
+ // systemMatrix . solution = rhs
+ // Therefore, systemMatrix = M/dt, solution = deltaV and rhs = f
- // Computes f(t, x(t), v(t)) and M
- Vector& f = m_equation.computeF(currentState);
- const Matrix& M = m_equation.computeM(currentState);
+ // Assemble the linear system systemMatrix.solution = rhs
+ assembleLinearSystem(dt, currentState, *newState);
- // Computes the system matrix (left-hand-side matrix)
- m_systemMatrix = M / dt;
+ // Solve the linear system to find solution = deltaV
+ m_solution = m_linearSolver->solve(m_rhs);
- // Apply boundary conditions to the linear system
- currentState.applyBoundaryConditionsToVector(&f);
- currentState.applyBoundaryConditionsToMatrix(&m_systemMatrix);
+ // Compute the new state using the Modified Euler Explicit scheme:
+ newState->getVelocities() = currentState.getVelocities() + m_solution;
+ newState->getPositions() = currentState.getPositions() + dt * newState->getVelocities();
- // Computes deltaV (stored in the velocities) and m_compliance = 1/m_systemMatrix
- Vector& deltaV = newState->getVelocities();
- (*m_linearSolver)(m_systemMatrix, f, &deltaV, &m_compliance);
+ if (computeCompliance)
+ {
+ computeComplianceMatrixFromSystemMatrix(currentState);
+ }
+}
- // 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);
+void OdeSolverEulerExplicitModified::assembleLinearSystem(double dt, const OdeState& state, const OdeState& newState,
+ bool computeRHS)
+{
+ // General equation to solve:
+ // M.a(t) = f(t, x(t), v(t))
+ // Using Euler explicit { v(t+dt) = v(t) + dt.a(t)
+ // { x(t+dt) = x(t) + dt.v(t)
+ // The resulting linear system on the velocity level is:
+ // (M/dt) . deltaV = f(t, x(t), v(t))
+ // systemMatrix . x = b
+ // Therefore, systemMatrix = M/dt, x = deltaV and b = f
- // Compute the new state using the Modified Euler Explicit scheme:
- newState->getVelocities() = currentState.getVelocities() + deltaV;
- newState->getPositions() = currentState.getPositions() + dt * newState->getVelocities();
+ m_equation.updateFMDK(state, ODEEQUATIONUPDATE_F | ODEEQUATIONUPDATE_M);
+
+ // Computes the LHS systemMatrix
+ m_systemMatrix = m_equation.getM() / dt;
+ state.applyBoundaryConditionsToMatrix(&m_systemMatrix);
+
+ // Feed the systemMatrix to the linear solver, so it can be used after this call to solve or inverse the matrix
+ m_linearSolver->setMatrix(m_systemMatrix);
+
+ // Computes the RHS vector
+ if (computeRHS)
+ {
+ m_rhs = m_equation.getF();
+ state.applyBoundaryConditionsToVector(&m_rhs);
+ }
}
}; // namespace Math
diff --git a/SurgSim/Math/OdeSolverEulerExplicitModified.h b/SurgSim/Math/OdeSolverEulerExplicitModified.h
index 348d494..5ecdfb1 100644
--- a/SurgSim/Math/OdeSolverEulerExplicitModified.h
+++ b/SurgSim/Math/OdeSolverEulerExplicitModified.h
@@ -24,14 +24,31 @@ 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)
+/// Modified Euler Explicit ode solver solves the following \f$2^{nd}\f$ order ode
+/// \f$M(x(t), v(t)).a(t) = f(t, x(t), v(t))\f$.
+/// This ode is solved as an ode of order 1 by defining the state vector
+/// \f$y = \left(\begin{array}{c}x\\v\end{array}\right)\f$:
+/// \f[
+/// y' = \left(\begin{array}{c} x' \\ v' \end{array}\right) =
+/// \left(\begin{array}{c} v \\ M(x, v)^{-1}.f(t,x, v) \end{array}\right) =
+/// f(t, y)
+/// \f]
+/// After integrating this equation, we get:
+/// \f[ y(t+dt) - y(t) = \int_t^{t+dt} f(t,y) dt \f]
+/// \note The modified Euler explicit is the same as Euler explicit, but simply using the newly calculated velocity
+/// to update the position with instead of using the velocity from the previous time-step.
+/// \note This makes this solver explicit on the velocity and implicit on the position, it improves the stability
+/// of the method compare to the Euler explicit but is still an explicit method.
+/// \note The numerical integration scheme becomes:
+/// \f[
+/// \left\{
+/// \begin{array}{ccccl}
+/// x(t+dt) &=& x(t) &+& dt.v(t+dt)
+/// \\ v(t+dt) &=& v(t) &+& dt.a(t)
+/// \end{array}
+/// \right.
+/// \f]
+/// \sa OdeSolverEulerExplicit
class OdeSolverEulerExplicitModified : public OdeSolver
{
public:
@@ -39,7 +56,11 @@ public:
/// \param equation The ode equation to be solved
explicit OdeSolverEulerExplicitModified(OdeEquation* equation);
- virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+ void solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance = true) override;
+
+protected:
+ void assembleLinearSystem(double dt, const OdeState& state, const OdeState& newState,
+ bool computeRHS = true) override;
};
}; // namespace Math
diff --git a/SurgSim/Math/OdeSolverEulerImplicit.cpp b/SurgSim/Math/OdeSolverEulerImplicit.cpp
index 0e0bc40..6c6935e 100644
--- a/SurgSim/Math/OdeSolverEulerImplicit.cpp
+++ b/SurgSim/Math/OdeSolverEulerImplicit.cpp
@@ -23,51 +23,143 @@ namespace Math
{
OdeSolverEulerImplicit::OdeSolverEulerImplicit(OdeEquation* equation)
- : OdeSolver(equation)
+ : OdeSolver(equation), m_maximumIteration(1), m_epsilonConvergence(1e-5)
{
m_name = "Ode Solver Euler Implicit";
}
-void OdeSolverEulerImplicit::solve(double dt, const OdeState& currentState, OdeState* newState)
+void OdeSolverEulerImplicit::setNewtonRaphsonMaximumIteration(size_t maximumIteration)
+{
+ SURGSIM_ASSERT(maximumIteration >= 1) << "The maximum iteration needs to be at least 1";
+
+ m_maximumIteration = maximumIteration;
+}
+
+size_t OdeSolverEulerImplicit::getNewtonRaphsonMaximumIteration() const
+{
+ return m_maximumIteration;
+}
+
+void OdeSolverEulerImplicit::setNewtonRaphsonEpsilonConvergence(double epsilonConvergence)
+{
+ SURGSIM_ASSERT(epsilonConvergence >= 0.0) << "The epsilon convergence cannot be negative";
+
+ m_epsilonConvergence = epsilonConvergence;
+}
+
+double OdeSolverEulerImplicit::getNewtonRaphsonEpsilonConvergence() const
+{
+ return m_epsilonConvergence;
+}
+
+void OdeSolverEulerImplicit::solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance)
{
// 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();
+ // Let's note K = -df/dx and D = -df/dv.
+ // Using Euler implicit { v(t+dt) = v(t) + dt.a(t+dt)
+ // { x(t+dt) = x(t) + dt.v(t+dt)
+ // For a single linearization, we get the following linear system:
+ // M/dt.deltaV = f(t, x(t), v(t)) - K.deltaX - D.deltaV
+ // (M/dt + D + dt.K) . deltaV = f(t, x(t), v(t)) - dt.K.v(t)
+ // systemMatrix . solution = rhs
+ // Therefore systemMatrix = (M/dt + D + dt.K), solution = deltaV, rhs = f - dt.K.v(t)
+ // More terms are coming from the Newton-Raphson iterations (see class OdeSolverEulerImplicit doxygen doc for
+ // more details)
+
+ // Note that the resulting system is non-linear as K and D are non-linear in absence of information on the nature
+ // of the model. We use a Newton-Raphson algorithm (http://en.wikipedia.org/wiki/Newton%27s_method) to solve
+ // this non-linear problem. Note that each iteration will re-evaluate the complete system (forces and matrices).
+ // Also note that this method converges quadratically around the root. In our case, the solution is deltaV, which
+ // should be close to 0. This makes our problem well suited for this method.
+
+ if (m_maximumIteration > 1)
+ {
+ m_previousSolution = Vector::Zero(currentState.getNumDof());
+ }
+
+ // Prepare the newState to be used in the loop, it starts as the current state.
+ *newState = currentState;
+
+ // See the class doxygen documentation (.dox) for explanation of the algorithm.
+ // * currentState is y(t) = (x(t), v(t)).
+ // * newState is the current estimate, y_n = (x_n, v_n) (with y_0 = y(t))
+ // * Each iteration search for the next estimate y_{n+1} = (x_{n+1}, v_{n+1}).
+ size_t numIteration = 0;
+ while (numIteration < m_maximumIteration)
+ {
+ // Assemble the linear system systemMatrix*solution = rhs
+ assembleLinearSystem(dt, currentState, *newState);
+
+ // Solve the linear system to find solution = deltaV
+ m_solution = m_linearSolver->solve(m_rhs);
+
+ // Compute the new state using the Euler Implicit scheme:
+ newState->getVelocities() += m_solution;
+ newState->getPositions() = currentState.getPositions() + dt * newState->getVelocities();
+
+ if (m_maximumIteration > 1)
+ {
+ // Use the infinity norm, to treat models with small or large number of dof the same way.
+ double solutionVariation = (m_solution - m_previousSolution).lpNorm<Eigen::Infinity>();
+
+ if (solutionVariation < m_epsilonConvergence)
+ {
+ break;
+ }
+
+ m_previousSolution = m_solution;
+ }
+
+ numIteration++;
+ }
+
+ // The compliance matrix (if requested) is computed w.r.t. the latest state.
+ if (computeCompliance)
+ {
+ computeComplianceMatrixFromSystemMatrix(currentState);
+ }
+}
+
+void OdeSolverEulerImplicit::assembleLinearSystem(double dt, const OdeState& state, const OdeState& newState,
+ bool computeRHS)
+{
+ // General equation to solve:
+ // M.a(t+dt) = f(t+dt, x(t+dt), v(t+dt))
+ // Let's note K = -df/dx and D = -df/dv.
+ // Using Euler implicit { v(t+dt) = v(t) + dt.a(t+dt)
+ // { x(t+dt) = x(t) + dt.v(t+dt)
+ // For a single linearization, we get the following linear system:
+ // M/dt.deltaV = f(t, x(t), v(t)) - K.deltaX - D.deltaV
+ // (M/dt + D + dt.K) . deltaV = f(t, x(t), v(t)) - dt.K.v(t)
+ // systemMatrix . solution = rhs
+ // Therefore systemMatrix = (M/dt + D + dt.K), solution = deltaV, rhs = f - dt.K.v(t)
+ // More terms are coming from the Newton-Raphson iterations (see class OdeSolverEulerImplicit doxygen doc for
+ // more details)
+
+ m_equation.updateFMDK(newState, ODEEQUATIONUPDATE_FMDK);
+
+ const SparseMatrix& M = m_equation.getM();
+ const SparseMatrix& D = m_equation.getD();
+ const SparseMatrix& K = m_equation.getK();
+ const Vector& f = m_equation.getF();
+
+ // Computes the LHS systemMatrix
+ m_systemMatrix = M * (1.0 / dt);
+ m_systemMatrix += D;
+ m_systemMatrix += K * dt;
+ state.applyBoundaryConditionsToMatrix(&m_systemMatrix);
+
+ // Feed the systemMatrix to the linear solver, so it can be used after this call to solve or inverse the matrix
+ m_linearSolver->setMatrix(m_systemMatrix);
+
+ // Computes the RHS vector by adding the Euler Implicit/Newton-Raphson terms
+ if (computeRHS)
+ {
+ m_rhs = f + K * (newState.getPositions() - state.getPositions() - newState.getVelocities() * dt);
+ m_rhs -= (M * (newState.getVelocities() - state.getVelocities())) / dt;
+ state.applyBoundaryConditionsToVector(&m_rhs);
+ }
}
}; // namespace Math
diff --git a/SurgSim/Math/OdeSolverEulerImplicit.dox b/SurgSim/Math/OdeSolverEulerImplicit.dox
new file mode 100644
index 0000000..8f25507
--- /dev/null
+++ b/SurgSim/Math/OdeSolverEulerImplicit.dox
@@ -0,0 +1,109 @@
+/*!
+ \class SurgSim::Math::OdeSolverEulerImplicit
+
+ The ODE to be solved is of the form:
+ \f[ \displaystyle M.a(t) = f(t, x(t), v(t)) \f]
+ \note \f$f\f$ is supposed to be continuous with partial derivatives
+ \f$\displaystyle\frac{\partial f}{\partial x} = -K\f$ and
+ \f$\displaystyle\frac{\partial f}{\partial v} = -D\f$.<br>
+ \note Also, we define the system matrix \f$\displaystyle S = \frac{M}{dt} + D + dt.K\f$.
+
+ <b>ODE of order 1</b><br>
+ This ODE is solved as an ODE of order 1 by defining the state vector
+ \f$y = \left(\begin{array}{c} x \\ v \end{array}\right)\f$:
+ \f[ \displaystyle
+ y' =
+ \left(\begin{array}{c} x' \\ v' \end{array}\right) =
+ \left(\begin{array}{c} v \\ M^{-1}.f(t, x, v) \end{array}\right) = f(t, y)
+ \f]
+
+ Integrating this equation gives:
+ \f[ \displaystyle y(t+dt) - y(t) = \int_{t}^{t+dt} f(t, y) dt \f]
+
+ <b>Backward Euler to numerically integrate the ODE</b><br>
+ Euler Implicit uses a rectangular numerical integration evaluated on the right:
+ \f$\int_{t}^{t+dt} f(t, y) dt \simeq dt. f(t+dt, y(t+dt))\f$.
+ Unlike Explicit Euler (a.k.a. forward Euler) which uses a rectangular numerical integration evaluated on the
+ left: \f$\int_{t}^{t+dt} f(t, y) dt \simeq dt. f(t, y(t))\f$.<br>
+ Euler implicit is also called backward Euler because the tangent evaluation is numerically evaluated using a
+ backward evaluation: \f$y'(t) = (y(t) - y(t-dt)) / dt\f$ which leads to the well known integration scheme
+ (written using the state vector \f$y\f$ or the variables \f$x\f$ and \f$v\f$:
+ \f[
+ \begin{array}{ccc}
+ y(t+dt) = y(t) + dt. f(t+dt, y(t+dt)) & or &
+ \left\{
+ \begin{array}{ccccl}
+ x(t+dt) &=& x(t) &+& dt.v(t+dt) \\
+ v(t+dt) &=& v(t) &+& dt.M^{-1}.f(t+dt, x(t+dt), v(t+dt))
+ \end{array}
+ \right.
+ \end{array}
+ \f]
+ This system of equations is non-linear w.r.t. to the unknown \f$y(t+dt)\f$.
+
+ <b>Newton-Raphson to solve the non-linear resulting equations</b><br>
+ We use Newton-Raphson to solve this non-liner problem http://en.wikipedia.org/wiki/Newton%27s_method.
+ Let's pose the non-linear problem to solve as:
+ \f[
+ F(y) = y - y(t) - dt.f(t+dt, y) = 0
+ \f]
+ The solution to this non-linear equation is \f$y(t+dt)\f$.
+
+ The Newton-Raphson algorithm iteratively calculates a series of state \f$y_n\f$ converging to the solution
+ \f$y(t+dt)\f$ (if the problem has a solution and the initial solution is close enough).<br>
+ Each iteration computes:
+ \f[
+ y_{n+1} = y_{n} - \frac{\partial F}{\partial y}(y_{n})^{-1} F(y_{n})
+ \f]
+ where \f$y_n = \left(\begin{array}{c}x_n\\v_n\end{array}\right)\f$ and
+ \f$y_{n+1} = \left(\begin{array}{c}x_{n+1}\\v_{n+1}\end{array}\right)\f$ are 2 successive approximations of the
+ solution. They are neither \f$y(t) = \left(\begin{array}{c}x(t)\\v(t)\end{array}\right)\f$ nor the solution
+ \f$y(t+dt) = \left(\begin{array}{c}x(t+dt)\\v(t+dt)\end{array}\right)\f$. Except on the very first iteration where
+ \f$y_0 = y(t)\f$.
+
+ \note The initial solution \f$y_0\f$ is set to be \f$y(t)\f$ as it is the latest solution and should by nature be
+ close to the solution \f$y(t+dt)\f$.
+ \note Therefore, on the first iteration, \f$F(y_0)\f$ will evaluate to exactly \f$-dt.f(t+dt, y(t))\f$ which is the
+ force evaluation from the previous time step.
+
+ \f[
+ \frac{\partial F}{\partial y} = \left( I - dt.\frac{\partial f}{\partial y}\right) =
+ \left(
+ \begin{array}{cc}
+ I & -dt.I \\
+ dt.M^{-1}K & I + dt.M^{-1}D
+ \end{array}
+ \right)
+ \f]
+
+ It can be easily shown using a Gauss pivot technique that the inverse is:
+ \f[
+ \frac{\partial F}{\partial y}^{-1} =
+ \left(
+ \begin{array}{cc}
+ I - dt.S^{-1}K & S^{-1}M \\
+ -S^{-1}K & \frac{S^{-1}M}{dt}
+ \end{array}
+ \right)
+ \f]
+
+ Therefore, we can formally develop the solution:
+ \f[
+ y_{n+1} =
+ \left\{
+ \begin{array}{ccccl}
+ x_{n+1} &=& x_n &-& \left[\left(I - dt.S^{-1}K\right).\left(x_n - x(t) - dt.v_n\right) +
+ S^{-1}M.\left(v_n - v(t) - dt.M^{-1}.f(x_n, v_n)\right)\right] \\
+ &=& x_n &-& x_n + x(t) + dt.v_n + dt.S^{-1}K\left(x_n - x(t) - dt.v_n\right) -
+ S^{-1}M.\left(v_n - v(t) - dt.M^{-1}.f(x_n, v_n)\right) \\
+ &=& x(t) &+& dt.\left[v_n + S^{-1}\left(f(x_n, v_n) + K(x_n - x(t) - dt.v_n) -
+ \frac{M}{dt}(v_n - v(t))\right)\right] \\
+ &=& x(t) &+& dt.v_{n+1} \\
+ v_{n+1} &=& v_n &-& \left[-S^{-1}K.\left(x_n - x(t) - dt.v_n\right) +
+ \frac{S^{-1}M}{dt}.\left(v_n - v(t) - dt.M^{-1}.f(x_n, v_n)\right)\right] \\
+ &=& v_n &+& S^{-1}\left(f(x_n, v_n) + K(x_n - x(t) - dt.v_n) - \frac{M}{dt}(v_n - v(t)) \right)
+ \end{array}
+ \right.
+ \f]
+ We simply need to solve the system to find the velocity variation and we can deduct the new position from there.
+*/
diff --git a/SurgSim/Math/OdeSolverEulerImplicit.h b/SurgSim/Math/OdeSolverEulerImplicit.h
index 372ac90..c58a8f8 100644
--- a/SurgSim/Math/OdeSolverEulerImplicit.h
+++ b/SurgSim/Math/OdeSolverEulerImplicit.h
@@ -24,17 +24,7 @@ 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)
+/// Euler implicit (a.k.a backward Euler) ode solver (see %OdeSolverEulerImplicit.dox for more details).
class OdeSolverEulerImplicit : public OdeSolver
{
public:
@@ -42,7 +32,32 @@ public:
/// \param equation The ode equation to be solved
explicit OdeSolverEulerImplicit(OdeEquation* equation);
- virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+ /// \param maximumIteration The Newton-Raphson algorithm maximum number of iterations
+ virtual void setNewtonRaphsonMaximumIteration(size_t maximumIteration);
+
+ /// \return The Newton-Raphson algorithm maximum number of iterations
+ size_t getNewtonRaphsonMaximumIteration() const;
+
+ /// \param epsilonConvergence The Newton-Raphson algorithm epsilon convergence
+ void setNewtonRaphsonEpsilonConvergence(double epsilonConvergence);
+
+ /// \return The Newton-Raphson algorithm epsilon convergence
+ double getNewtonRaphsonEpsilonConvergence() const;
+
+ void solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance = true) override;
+
+protected:
+ void assembleLinearSystem(double dt, const OdeState& state, const OdeState& newState,
+ bool computeRHS = true) override;
+
+ /// Newton-Raphson maximum number of iteration (1 => linearization)
+ size_t m_maximumIteration;
+
+ /// Newton-Raphson convergence criteria (variation of the solution over time)
+ double m_epsilonConvergence;
+
+ /// Newton-Raphson previous solution (we solve a problem to find deltaV, the variation in velocity)
+ Vector m_previousSolution;
};
}; // namespace Math
diff --git a/SurgSim/Math/OdeSolverLinearEulerExplicit.cpp b/SurgSim/Math/OdeSolverLinearEulerExplicit.cpp
index 5879330..638d2ab 100644
--- a/SurgSim/Math/OdeSolverLinearEulerExplicit.cpp
+++ b/SurgSim/Math/OdeSolverLinearEulerExplicit.cpp
@@ -28,18 +28,21 @@ OdeSolverLinearEulerExplicit::OdeSolverLinearEulerExplicit(OdeEquation* equation
m_name = "Ode Solver Linear Euler Explicit";
}
-void OdeSolverLinearEulerExplicit::solve(double dt, const OdeState& currentState, OdeState* newState)
+void OdeSolverLinearEulerExplicit::solve(double dt, const OdeState& currentState, OdeState* newState,
+ bool computeCompliance)
{
if (!m_initialized)
{
- OdeSolverEulerExplicit::solve(dt, currentState, newState);
+ // The compliance matrix is constant and used in all following calls, so we force its calculation on 1st pass.
+ OdeSolverEulerExplicit::solve(dt, currentState, newState, true);
m_initialized = true;
}
else
{
- Vector& f = m_equation.computeF(currentState);
- currentState.applyBoundaryConditionsToVector(&f);
- Vector deltaV = m_compliance * f;
+ m_equation.updateFMDK(currentState, ODEEQUATIONUPDATE_F);
+
+ const Vector& f = m_equation.getF();
+ Vector deltaV = m_equation.applyCompliance(currentState, f);
newState->getPositions() = currentState.getPositions() + dt * currentState.getVelocities();
newState->getVelocities() = currentState.getVelocities() + deltaV;
diff --git a/SurgSim/Math/OdeSolverLinearEulerExplicit.h b/SurgSim/Math/OdeSolverLinearEulerExplicit.h
index 1a9bf15..70ebaa8 100644
--- a/SurgSim/Math/OdeSolverLinearEulerExplicit.h
+++ b/SurgSim/Math/OdeSolverLinearEulerExplicit.h
@@ -34,7 +34,9 @@ public:
/// \param equation The ode equation to be solved
explicit OdeSolverLinearEulerExplicit(OdeEquation* equation);
- virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+ /// The parameter computeCompliance is irrelevant for any Linear solver as it is a constant matrix.
+ /// It will be precomputed on the first call and use in the following calls, no matter what the parameter value is.
+ void solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance = true) override;
private:
/// Has the solver been initialized
diff --git a/SurgSim/Math/OdeSolverLinearEulerExplicitModified.cpp b/SurgSim/Math/OdeSolverLinearEulerExplicitModified.cpp
index bb54494..037413e 100644
--- a/SurgSim/Math/OdeSolverLinearEulerExplicitModified.cpp
+++ b/SurgSim/Math/OdeSolverLinearEulerExplicitModified.cpp
@@ -28,18 +28,21 @@ OdeSolverLinearEulerExplicitModified::OdeSolverLinearEulerExplicitModified(OdeEq
m_name = "Ode Solver Linear Euler Explicit Modified";
}
-void OdeSolverLinearEulerExplicitModified::solve(double dt, const OdeState& currentState, OdeState* newState)
+void OdeSolverLinearEulerExplicitModified::solve(double dt, const OdeState& currentState, OdeState* newState,
+ bool computeCompliance)
{
if (!m_initialized)
{
- OdeSolverEulerExplicitModified::solve(dt, currentState, newState);
+ // The compliance matrix is constant and used in all following calls, so we force its calculation on 1st pass.
+ OdeSolverEulerExplicitModified::solve(dt, currentState, newState, true);
m_initialized = true;
}
else
{
- Vector& f = m_equation.computeF(currentState);
- currentState.applyBoundaryConditionsToVector(&f);
- Vector deltaV = m_compliance * (f);
+ m_equation.updateFMDK(currentState, ODEEQUATIONUPDATE_F);
+
+ const Vector& f = m_equation.getF();
+ Vector deltaV = m_equation.applyCompliance(currentState, f);
newState->getVelocities() = currentState.getVelocities() + deltaV;
newState->getPositions() = currentState.getPositions() + dt * newState->getVelocities();
diff --git a/SurgSim/Math/OdeSolverLinearEulerExplicitModified.h b/SurgSim/Math/OdeSolverLinearEulerExplicitModified.h
index ddce1ca..557aeda 100644
--- a/SurgSim/Math/OdeSolverLinearEulerExplicitModified.h
+++ b/SurgSim/Math/OdeSolverLinearEulerExplicitModified.h
@@ -34,7 +34,9 @@ public:
/// \param equation The ode equation to be solved
explicit OdeSolverLinearEulerExplicitModified(OdeEquation* equation);
- virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+ /// The parameter computeCompliance is irrelevant for any Linear solver as it is a constant matrix.
+ /// It will be precomputed on the first call and use in the following calls, no matter what the parameter value is.
+ void solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance = true) override;
private:
/// Has the solver been initialized
diff --git a/SurgSim/Math/OdeSolverLinearEulerImplicit.cpp b/SurgSim/Math/OdeSolverLinearEulerImplicit.cpp
index 281d6bb..961cd14 100644
--- a/SurgSim/Math/OdeSolverLinearEulerImplicit.cpp
+++ b/SurgSim/Math/OdeSolverLinearEulerImplicit.cpp
@@ -13,6 +13,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "SurgSim/Framework/Log.h"
#include "SurgSim/Math/OdeSolverLinearEulerImplicit.h"
#include "SurgSim/Math/OdeState.h"
@@ -26,22 +27,36 @@ OdeSolverLinearEulerImplicit::OdeSolverLinearEulerImplicit(OdeEquation* equation
: OdeSolverEulerImplicit(equation), m_initialized(false)
{
m_name = "Ode Solver Linear Euler Implicit";
+
+ // The system being linear, only 1 iteration is necessary to find the exact solution.
+ setNewtonRaphsonMaximumIteration(1);
}
-void OdeSolverLinearEulerImplicit::solve(double dt, const OdeState& currentState, OdeState* newState)
+void OdeSolverLinearEulerImplicit::setNewtonRaphsonMaximumIteration(size_t maximumIteration)
+{
+ OdeSolverEulerImplicit::setNewtonRaphsonMaximumIteration(maximumIteration);
+ SURGSIM_LOG_IF(maximumIteration != 1, SurgSim::Framework::Logger::getLogger("OdeSolver"), WARNING) <<
+ "OdeSolverLinearEulerImplicit should have a maximum number of iteration of 1 for the Newton-Raphson. " <<
+ "As the model is (supposed to be) linear, a single iteration will find the exact solution.";
+}
+
+void OdeSolverLinearEulerImplicit::solve(double dt, const OdeState& currentState, OdeState* newState,
+ bool computeCompliance)
{
if (!m_initialized)
{
- OdeSolverEulerImplicit::solve(dt, currentState, newState);
- m_constantK = m_equation.computeK(currentState);
+ // The compliance matrix is constant and used in all following calls, so we force its calculation on 1st pass.
+ OdeSolverEulerImplicit::solve(dt, currentState, newState, true);
+ m_constantK = m_equation.getK();
m_initialized = true;
}
else
{
- Vector& f = m_equation.computeF(currentState);
- f -= m_constantK * currentState.getVelocities() * dt;
- currentState.applyBoundaryConditionsToVector(&f);
- Vector deltaV = m_compliance * f;
+ m_equation.updateFMDK(currentState, ODEEQUATIONUPDATE_F);
+
+ Vector f = m_equation.getF();
+ f -= m_constantK * (currentState.getVelocities() * dt);
+ Vector deltaV = m_equation.applyCompliance(currentState, f);
newState->getVelocities() = currentState.getVelocities() + deltaV;
newState->getPositions() = currentState.getPositions() + dt * newState->getVelocities();
diff --git a/SurgSim/Math/OdeSolverLinearEulerImplicit.h b/SurgSim/Math/OdeSolverLinearEulerImplicit.h
index 9d9d346..54328cd 100644
--- a/SurgSim/Math/OdeSolverLinearEulerImplicit.h
+++ b/SurgSim/Math/OdeSolverLinearEulerImplicit.h
@@ -17,6 +17,7 @@
#define SURGSIM_MATH_ODESOLVERLINEAREULERIMPLICIT_H
#include "SurgSim/Math/OdeSolverEulerImplicit.h"
+#include "SurgSim/Math/SparseMatrix.h"
namespace SurgSim
{
@@ -27,6 +28,9 @@ 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.
+/// \note If the matrices are all constant, the problem to solve becomes linear,
+/// \note therefore the Newton-Raphson algorithm will be exact in only 1 iteration.
+/// \note Therefore this solver uses a number of maximum iteration of 1 unless overriden.
class OdeSolverLinearEulerImplicit : public OdeSolverEulerImplicit
{
public:
@@ -34,11 +38,15 @@ public:
/// \param equation The ode equation to be solved
explicit OdeSolverLinearEulerImplicit(OdeEquation* equation);
- virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+ void setNewtonRaphsonMaximumIteration(size_t maximumIteration) override;
+
+ /// The parameter computeCompliance is irrelevant for any Linear solver as it is a constant matrix.
+ /// It will be precomputed on the first call and use in the following calls, no matter what the parameter value is.
+ void solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance = true) override;
private:
/// The constant stiffness matrix
- Matrix m_constantK;
+ SparseMatrix m_constantK;
/// Has the solver been initialized
bool m_initialized;
diff --git a/SurgSim/Math/OdeSolverLinearRungeKutta4.cpp b/SurgSim/Math/OdeSolverLinearRungeKutta4.cpp
index 52a7c8f..6f76b1e 100644
--- a/SurgSim/Math/OdeSolverLinearRungeKutta4.cpp
+++ b/SurgSim/Math/OdeSolverLinearRungeKutta4.cpp
@@ -24,16 +24,18 @@ namespace Math
OdeSolverLinearRungeKutta4::OdeSolverLinearRungeKutta4(OdeEquation* equation)
: OdeSolverRungeKutta4(equation),
- m_initialized(false)
+ m_initialized(false)
{
m_name = "Ode Solver Linear Runge Kutta 4";
}
-void OdeSolverLinearRungeKutta4::solve(double dt, const OdeState& currentState, OdeState* newState)
+void OdeSolverLinearRungeKutta4::solve(double dt, const OdeState& currentState, OdeState* newState,
+ bool computeCompliance)
{
if (!m_initialized)
{
- OdeSolverRungeKutta4::solve(dt, currentState, newState);
+ // The compliance matrix is constant and used in all following calls, so we force its calculation on 1st pass.
+ OdeSolverRungeKutta4::solve(dt, currentState, newState, true);
m_initialized = true;
}
else
@@ -53,32 +55,37 @@ void OdeSolverLinearRungeKutta4::solve(double dt, const OdeState& currentState,
// 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;
+ // Reminder: m_complianceMatrix = dt.M^-1 (including 0 compliance for all boundary conditions)
+ m_equation.updateFMDK(currentState, ODEEQUATIONUPDATE_F);
+ m_k1.acceleration = m_equation.applyCompliance(currentState, m_equation.getF() / 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;
+ m_equation.updateFMDK(*newState, ODEEQUATIONUPDATE_F);
+ m_k2.acceleration = m_equation.applyCompliance(*newState, m_equation.getF() / 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;
+ m_equation.updateFMDK(*newState, ODEEQUATIONUPDATE_F);
+ m_k3.acceleration = m_equation.applyCompliance(*newState, m_equation.getF() / 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;
+ m_equation.updateFMDK(*newState, ODEEQUATIONUPDATE_F);
+ m_k4.acceleration = m_equation.applyCompliance(*newState, m_equation.getF() / 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;
+ (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;
+ (m_k1.acceleration + m_k4.acceleration + 2.0 *
+ (m_k2.acceleration + m_k3.acceleration)) * dt / 6.0;
}
}
diff --git a/SurgSim/Math/OdeSolverLinearRungeKutta4.h b/SurgSim/Math/OdeSolverLinearRungeKutta4.h
index 8a18088..f24a823 100644
--- a/SurgSim/Math/OdeSolverLinearRungeKutta4.h
+++ b/SurgSim/Math/OdeSolverLinearRungeKutta4.h
@@ -34,7 +34,9 @@ public:
/// \param equation The ode equation to be solved
explicit OdeSolverLinearRungeKutta4(OdeEquation* equation);
- virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+ /// The parameter computeCompliance is irrelevant for any Linear solver as it is a constant matrix.
+ /// It will be precomputed on the first call and use in the following calls, no matter what the parameter value is.
+ void solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance = true) override;
private:
bool m_initialized;
diff --git a/SurgSim/Math/OdeSolverLinearStatic.cpp b/SurgSim/Math/OdeSolverLinearStatic.cpp
index cae441d..38fa0cf 100644
--- a/SurgSim/Math/OdeSolverLinearStatic.cpp
+++ b/SurgSim/Math/OdeSolverLinearStatic.cpp
@@ -28,18 +28,19 @@ OdeSolverLinearStatic::OdeSolverLinearStatic(OdeEquation* equation)
m_name = "Ode Solver Linear Static";
}
-void OdeSolverLinearStatic::solve(double dt, const OdeState& currentState, OdeState* newState)
+void OdeSolverLinearStatic::solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance)
{
if (!m_initialized)
{
- OdeSolverStatic::solve(dt, currentState, newState);
+ OdeSolverStatic::solve(dt, currentState, newState, computeCompliance);
m_initialized = true;
}
else
{
- Vector& f = m_equation.computeF(currentState);
- currentState.applyBoundaryConditionsToVector(&f);
- Vector deltaX = m_compliance * f;
+ m_equation.updateFMDK(currentState, ODEEQUATIONUPDATE_F);
+
+ const Vector& f = m_equation.getF();
+ Vector deltaX = m_equation.applyCompliance(currentState, f);
// Compute the new state using the static scheme:
newState->getPositions() = currentState.getPositions() + deltaX;
diff --git a/SurgSim/Math/OdeSolverLinearStatic.h b/SurgSim/Math/OdeSolverLinearStatic.h
index 086f73e..54b0d6f 100644
--- a/SurgSim/Math/OdeSolverLinearStatic.h
+++ b/SurgSim/Math/OdeSolverLinearStatic.h
@@ -33,7 +33,9 @@ public:
/// \param equation The ode equation to be solved
explicit OdeSolverLinearStatic(OdeEquation* equation);
- virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+ /// The parameter computeCompliance is irrelevant for any Linear solver as it is a constant matrix.
+ /// It will be precomputed on the first call and use in the following calls, no matter what the parameter value is.
+ void solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance = true) override;
private:
/// Has the solver been initialized
diff --git a/SurgSim/Math/OdeSolverRungeKutta4.cpp b/SurgSim/Math/OdeSolverRungeKutta4.cpp
index 6d2e917..4c59bf5 100644
--- a/SurgSim/Math/OdeSolverRungeKutta4.cpp
+++ b/SurgSim/Math/OdeSolverRungeKutta4.cpp
@@ -28,7 +28,7 @@ OdeSolverRungeKutta4::OdeSolverRungeKutta4(OdeEquation* equation)
m_name = "Ode Solver Runge Kutta 4";
}
-void OdeSolverRungeKutta4::solve(double dt, const OdeState& currentState, OdeState* newState)
+void OdeSolverRungeKutta4::solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance)
{
// 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:
@@ -36,6 +36,8 @@ void OdeSolverRungeKutta4::solve(double dt, const OdeState& currentState, OdeSta
// (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)))
+ // On the velocity level, we can write M/dt.deltaV = F
+ // Therefore the system matrix is M/dt
// 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) )
@@ -43,41 +45,39 @@ void OdeSolverRungeKutta4::solve(double dt, const OdeState& currentState, OdeSta
// 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);
+ // Assemble the linear system systemMatrix.solution = rhs
+ assembleLinearSystem(dt, currentState, *newState);
// 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.
+ m_k1.acceleration = m_linearSolver->solve(m_rhs / 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);
+ m_equation.updateFMDK(*newState, ODEEQUATIONUPDATE_F);
+ Vector f = m_equation.getF();
+ m_k2.acceleration =
+ m_linearSolver->solve(*currentState.applyBoundaryConditionsToVector(&f) / 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);
+ m_equation.updateFMDK(*newState, ODEEQUATIONUPDATE_F);
+ f = m_equation.getF();
+ m_k3.acceleration =
+ m_linearSolver->solve(*currentState.applyBoundaryConditionsToVector(&f) / 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);
+ m_equation.updateFMDK(*newState, ODEEQUATIONUPDATE_F);
+ f = m_equation.getF();
+ m_k4.acceleration =
+ m_linearSolver->solve(*currentState.applyBoundaryConditionsToVector(&f) / dt);
// Compute the new state using Runge Kutta 4 integration scheme:
newState->getPositions() = currentState.getPositions() +
@@ -85,9 +85,30 @@ void OdeSolverRungeKutta4::solve(double dt, const OdeState& currentState, OdeSta
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;
+ if (computeCompliance)
+ {
+ computeComplianceMatrixFromSystemMatrix(currentState);
+ }
+}
+
+void OdeSolverRungeKutta4::assembleLinearSystem(double dt, const OdeState& state, const OdeState& newState,
+ bool computeRHS)
+{
+ m_equation.updateFMDK(state, ODEEQUATIONUPDATE_F | ODEEQUATIONUPDATE_M);
+
+ // Computes the LHS systemMatrix
+ m_systemMatrix = m_equation.getM() / dt;
+ state.applyBoundaryConditionsToMatrix(&m_systemMatrix);
+
+ // Feed the systemMatrix to the linear solver, so it can be used after this call to solve or inverse the matrix
+ m_linearSolver->setMatrix(m_systemMatrix);
+
+ // Computes the RHS vector
+ if (computeRHS)
+ {
+ m_rhs = m_equation.getF();
+ state.applyBoundaryConditionsToVector(&m_rhs);
+ }
}
}; // namespace Math
diff --git a/SurgSim/Math/OdeSolverRungeKutta4.h b/SurgSim/Math/OdeSolverRungeKutta4.h
index 4c88427..2089c80 100644
--- a/SurgSim/Math/OdeSolverRungeKutta4.h
+++ b/SurgSim/Math/OdeSolverRungeKutta4.h
@@ -26,20 +26,32 @@ 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)
+/// Runge Kutta 4 ode solver (See http://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods)
+/// solves the following \f$2^{nd}\f$ order ode
+/// \f$M(x(t), v(t)).a(t) = f(t, x(t), v(t))\f$.
+/// This ode is solved as an ode of order 1 by defining the state vector
+/// \f$y = \left(\begin{array}{c}x\\v\end{array}\right)\f$:
+/// \f[
+/// y' = \left(\begin{array}{c} x' \\ v' \end{array}\right) =
+/// \left(\begin{array}{c} v \\ M(x, v)^{-1}.f(t,x, v) \end{array}\right) =
+/// f(t, y)
+/// \f]
+/// After integrating this equation, we get:
+/// \f[ y(t+dt) - y(t) = \int_t^{t+dt} f(t,y) dt \f]
+/// Runge Kutta 4 evaluates the integral term using 4 dependents evaluations of \f$f\f$ at different times and states:
+/// \f[
+/// \begin{array}{l}
+/// y(t+dt) = y(t) + \frac{dt}{6} (k_1 + 2k_2 + 2k_3 + k_4)
+/// \\ \text{with:}
+/// \\ \quad
+/// \begin{array}{lllllll}
+/// k_1 &=& f(& t &,& y(t) &)
+/// \\ k_2 &=& f(& t+\frac{dt}{2} &,& y(t) + \frac{dt}{2} k_1 &)
+/// \\ k_3 &=& f(& t+\frac{dt}{2} &,& y(t) + \frac{dt}{2} k_2 &)
+/// \\ k_4 &=& f(& t+dt &,& y(t) + dt k_3 &)
+/// \end{array}
+/// \end{array}
+/// \f]
class OdeSolverRungeKutta4 : public OdeSolver
{
public:
@@ -47,15 +59,11 @@ public:
/// \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;
+ void solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance = true) override;
protected:
- /// Temporary vectors to store the 4 intermediates evaluations
- Vector m_force;
+ void assembleLinearSystem(double dt, const OdeState& state, const OdeState& newState,
+ bool computeRHS = true) override;
/// Internal structure to hold the 4 temporary evaluations
struct RungeKuttaDerivedState
@@ -65,8 +73,11 @@ protected:
Vector velocity;
Vector acceleration;
};
- /// Runge kutta 4 intermediate system evaluations
+
+ ///@{
+ /// Runge kutta 4 intermediate system evaluation
RungeKuttaDerivedState m_k1, m_k2, m_k3, m_k4;
+ ///@}
};
}; // namespace Math
diff --git a/SurgSim/Math/OdeSolverStatic.cpp b/SurgSim/Math/OdeSolverStatic.cpp
index 0fdf074..5112c97 100644
--- a/SurgSim/Math/OdeSolverStatic.cpp
+++ b/SurgSim/Math/OdeSolverStatic.cpp
@@ -28,36 +28,58 @@ OdeSolverStatic::OdeSolverStatic(OdeEquation* equation)
m_name = "Ode Solver Static";
}
-void OdeSolverStatic::solve(double dt, const OdeState& currentState, OdeState* newState)
+void OdeSolverStatic::solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance)
{
// General equation to solve:
- // K.deltaX = Fext + Fint(t)
+ // K.deltaX = f(t) = 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
+ // systemMatrix . solution = rhs
+ // Therefore systemMatrix = K, solution = deltaX, rhs = f(t)
- // 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);
+ // Assemble the linear system systemMatrix.solution = rhs
+ assembleLinearSystem(dt, currentState, *newState);
- // 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);
+ // Solve the linear system to find solution = deltaX
+ m_solution = m_linearSolver->solve(m_rhs);
// Compute the new state using the static scheme:
- newState->getPositions() = currentState.getPositions() + deltaX;
+ newState->getPositions() = currentState.getPositions() + m_solution;
// Velocities are null in static mode (no time dependency)
newState->getVelocities().setZero();
+
+ if (computeCompliance)
+ {
+ computeComplianceMatrixFromSystemMatrix(currentState);
+ }
+}
+
+void OdeSolverStatic::assembleLinearSystem(double dt, const OdeState& state, const OdeState& newState, bool computeRHS)
+{
+ // 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
+ // systemMatrix . solution = rhs
+ // Therefore systemMatrix = K, solution = deltaX, rhs = f(t)
+
+ m_equation.updateFMDK(state, ODEEQUATIONUPDATE_F | ODEEQUATIONUPDATE_K);
+
+ // Computes the LHS systemMatrix
+ m_systemMatrix = m_equation.getK();
+ state.applyBoundaryConditionsToMatrix(&m_systemMatrix);
+
+ // Feed the systemMatrix to the linear solver, so it can be used after this call to solve or inverse the matrix
+ m_linearSolver->setMatrix(m_systemMatrix);
+
+ // Computes the RHS vector
+ if (computeRHS)
+ {
+ m_rhs = m_equation.getF();
+ state.applyBoundaryConditionsToVector(&m_rhs);
+ }
}
}; // namespace Math
diff --git a/SurgSim/Math/OdeSolverStatic.h b/SurgSim/Math/OdeSolverStatic.h
index 5f18acf..6bbc2c0 100644
--- a/SurgSim/Math/OdeSolverStatic.h
+++ b/SurgSim/Math/OdeSolverStatic.h
@@ -24,11 +24,14 @@ 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)
+/// Static ode solver solves the following \f$2^{nd}\f$ order ode \f$M(x(t), v(t)).a(t) = f(t, x(t), v(t))\f$. <br>
+/// This ode equation is solved w.r.t. \f$x\f$, by discarding all time derived variables (i.e. \f$v\f$, \f$a\f$)
+/// reducing the equation to solve to:
+/// \f[
+/// 0 = f(t, x(t)) = f_{ext} + f_{int}(t, x(t)) = f_{ext} - K(x).(x - x0)
+/// \f]
+/// \note This solver does not solve the resulting non-linear equations, but their linearization:
+/// \f$f_{ext} - K.(x - x0)=0\f$
class OdeSolverStatic : public OdeSolver
{
public:
@@ -36,7 +39,11 @@ public:
/// \param equation The ode equation to be solved
explicit OdeSolverStatic(OdeEquation* equation);
- virtual void solve(double dt, const OdeState& currentState, OdeState* newState) override;
+ void solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance = true) override;
+
+protected:
+ void assembleLinearSystem(double dt, const OdeState& state, const OdeState& newState,
+ bool computeRHS = true) override;
};
}; // namespace Math
diff --git a/SurgSim/Math/OdeState.cpp b/SurgSim/Math/OdeState.cpp
index 3cb743d..e34f6d0 100644
--- a/SurgSim/Math/OdeState.cpp
+++ b/SurgSim/Math/OdeState.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -39,7 +39,7 @@ bool OdeState::operator ==(const OdeState& state) const
bool OdeState::operator !=(const OdeState& state) const
{
- return ! ((*this) == state);
+ return !((*this) == state);
}
void OdeState::reset()
@@ -113,7 +113,8 @@ const SurgSim::Math::Vector3d OdeState::getVelocity(size_t nodeId) const
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.";
+ "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)
{
@@ -124,10 +125,11 @@ void OdeState::addBoundaryCondition(size_t nodeId)
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.";
+ "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;
+ "Invalid nodeDofId " << nodeDofId << " number of dof per node is " << m_numDofPerNode;
size_t globalDofId = nodeId * m_numDofPerNode + nodeDofId;
if (! m_boundaryConditionsPerDof[globalDofId])
@@ -155,11 +157,11 @@ bool OdeState::isBoundaryCondition(size_t dof) const
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";
+ << "Invalid vector to apply boundary conditions on";
- for (std::vector<size_t>::const_iterator it = getBoundaryConditions().cbegin();
- it != getBoundaryConditions().cend();
- ++it)
+ for (auto it = getBoundaryConditions().cbegin();
+ it != getBoundaryConditions().cend();
+ ++it)
{
(*vector)[*it] = 0.0;
}
@@ -169,10 +171,9 @@ Vector* OdeState::applyBoundaryConditionsToVector(Vector* vector) const
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()
+ SURGSIM_ASSERT(matrix != nullptr && static_cast<size_t>(matrix->rows()) == getNumDof()
&& static_cast<size_t>(matrix->cols()) == getNumDof())
- << "Invalid matrix to apply boundary conditions on";
+ << "Invalid matrix to apply boundary conditions on";
double complianceValue = 0.0;
@@ -181,9 +182,9 @@ void OdeState::applyBoundaryConditionsToMatrix(Matrix* matrix, bool hasComplianc
complianceValue = 1.0;
}
- for (std::vector<size_t>::const_iterator it = getBoundaryConditions().cbegin();
- it != getBoundaryConditions().cend();
- ++it)
+ for (auto it = getBoundaryConditions().cbegin();
+ it != getBoundaryConditions().cend();
+ ++it)
{
(*matrix).middleRows(*it, 1).setZero();
(*matrix).middleCols(*it, 1).setZero();
@@ -191,9 +192,51 @@ void OdeState::applyBoundaryConditionsToMatrix(Matrix* matrix, bool hasComplianc
}
}
+void OdeState::applyBoundaryConditionsToMatrix(SparseMatrix* matrix, bool hasCompliance) const
+{
+ SURGSIM_ASSERT(matrix != nullptr && 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 (auto it = getBoundaryConditions().cbegin();
+ it != getBoundaryConditions().cend();
+ ++it)
+ {
+ Math::zeroRow((*it), matrix);
+ Math::zeroColumn(static_cast<SparseMatrix::Index>((*it)), matrix);
+ (*matrix).coeffRef(static_cast<SparseMatrix::Index>(*it),
+ static_cast<SparseMatrix::Index>(*it)) = complianceValue;
+ }
+}
+
bool OdeState::isValid() const
{
- return SurgSim::Math::isValid(getPositions()) && SurgSim::Math::isValid(getVelocities());
+ using SurgSim::Math::isValid;
+
+ /// http://steve.hollasch.net/cgindex/coding/ieeefloat.html
+ /// We use the IEEE754 standard stipulating that any arithmetic operation with a NaN operand will produce NaN
+ /// and any sum of +-INF with a finite number or +-INF will produce +-INF.
+ /// Therefore, testing if a vector contains only finite numbers can be achieve easily by summing all the values
+ /// and testing if the result is a finite number or not.
+ return isValid(getPositions().sum()) && isValid(getVelocities().sum());
+}
+
+OdeState OdeState::interpolate(const OdeState& other, double t) const
+{
+ auto result = OdeState(*this);
+ if (t != 0)
+ {
+ result.m_v += (other.m_v - m_v) * t;
+ result.m_x += (other.m_x - m_x) * t;
+ }
+ return result;
}
}; // namespace Math
diff --git a/SurgSim/Math/OdeState.h b/SurgSim/Math/OdeState.h
index b3e7547..93f5847 100644
--- a/SurgSim/Math/OdeState.h
+++ b/SurgSim/Math/OdeState.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -19,6 +19,7 @@
#include <memory>
#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/SparseMatrix.h"
#include "SurgSim/Math/Vector.h"
namespace SurgSim
@@ -27,10 +28,13 @@ 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) )
+/// The state \f$y\f$ of an ode of 2nd order of the form \f$M(x,v).a = F(x, v)\f$ with boundary conditions.
+/// This ode equation is solved as an ode of order 1 by defining the state vector
+/// \f$y = \left(\begin{array}{c}x\\v\end{array}\right)\f$:
+/// \f[
+/// y' = \left(\begin{array}{c} x' \\ v' \end{array}\right) =
+/// \left(\begin{array}{c} v \\ M(x, v)^{-1}.F(x, v) \end{array}\right)
+/// \f]
class OdeState
{
public:
@@ -38,7 +42,7 @@ public:
OdeState();
/// Destructor
- ~OdeState();
+ virtual ~OdeState();
/// Comparison operator (equality test)
/// \param state The state to compare it to
@@ -52,13 +56,13 @@ public:
/// Resets the state
/// \note Simply set all positions/velocities to 0 and remove all boundary conditions
- void reset();
+ virtual 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);
+ virtual void setNumDof(size_t numDofPerNode, size_t numNodes);
/// Retrieves the number of degrees of freedom
/// \return The number of DOF for this representation
@@ -121,20 +125,37 @@ public:
/// 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)
+ /// \return The parameter vector. This enables chained use like
+ /// \f$U = K^1 * \text{applyBoundaryConditionsToVector}(x)\f$
Vector* applyBoundaryConditionsToVector(Vector* vector) const;
/// Apply boundary conditions to a given matrix
- /// \param matrix The matrix to apply the boundary conditions on
+ /// \param matrix The dense 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;
+ /// Apply boundary conditions to a given matrix
+ /// \param matrix The sparse 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(SparseMatrix* 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;
+ virtual bool isValid() const;
+
+ /// Returns the linear interpolated ODE state between this and other at parameter t
+ /// \param other the end point for the linear interpolation
+ /// \param t the interpolation time
+ /// \return the interpolated state = this + (other - this) * t;
+ /// \note All dof are independently linearly interpolated (This will not work correctly
+ /// on rotation vectors where a slerp will be required.)
+ OdeState interpolate(const OdeState& other, double t) const;
private:
/// Default public copy constructor and assignment operator are being used on purpose
diff --git a/SurgSim/Math/ParticlesShape-inl.h b/SurgSim/Math/ParticlesShape-inl.h
new file mode 100644
index 0000000..11d35b9
--- /dev/null
+++ b/SurgSim/Math/ParticlesShape-inl.h
@@ -0,0 +1,45 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_PARTICLESSHAPE_INL_H
+#define SURGSIM_MATH_PARTICLESSHAPE_INL_H
+
+namespace SurgSim
+{
+namespace Math
+{
+
+template <class V>
+ParticlesShape::ParticlesShape(const SurgSim::DataStructures::Vertices<V>& other) :
+ DataStructures::Vertices<DataStructures::EmptyData>(other),
+ m_radius(0.0)
+{
+ update();
+}
+
+template <class V>
+ParticlesShape& ParticlesShape::operator=(const SurgSim::DataStructures::Vertices<V>& other)
+{
+ DataStructures::Vertices<DataStructures::EmptyData>::operator=(other);
+ update();
+ return *this;
+}
+
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif
diff --git a/SurgSim/Math/ParticlesShape.cpp b/SurgSim/Math/ParticlesShape.cpp
new file mode 100644
index 0000000..592d121
--- /dev/null
+++ b/SurgSim/Math/ParticlesShape.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/Math/ParticlesShape.h"
+
+#include "SurgSim/DataStructures/AabbTree.h"
+#include "SurgSim/DataStructures/AabbTreeData.h"
+
+
+namespace SurgSim
+{
+namespace Math
+{
+
+SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::ParticlesShape, ParticlesShape);
+
+ParticlesShape::ParticlesShape(double radius) :
+ m_radius(radius)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(ParticlesShape, double, Radius, getRadius, setRadius);
+ update();
+}
+
+ParticlesShape::ParticlesShape(const ParticlesShape& other) :
+ DataStructures::Vertices<DataStructures::EmptyData>(other),
+ m_aabbTree(other.m_aabbTree),
+ m_radius(other.getRadius()),
+ m_center(other.getCenter()),
+ m_volume(other.getVolume()),
+ m_secondMomentOfVolume(other.getSecondMomentOfVolume())
+{
+}
+
+int ParticlesShape::getType() const
+{
+ return SHAPE_TYPE_PARTICLES;
+}
+
+double ParticlesShape::getVolume() const
+{
+ return m_volume;
+}
+
+Vector3d ParticlesShape::getCenter() const
+{
+ return m_center;
+}
+
+Matrix33d ParticlesShape::getSecondMomentOfVolume() const
+{
+ return m_secondMomentOfVolume;
+}
+
+bool ParticlesShape::isValid() const
+{
+ return m_radius >= 0.0;
+}
+
+double ParticlesShape::getRadius() const
+{
+ return m_radius;
+}
+
+void ParticlesShape::setRadius(double radius)
+{
+ m_radius = radius;
+}
+
+bool ParticlesShape::doUpdate()
+{
+ const double numParticles = static_cast<double>(getVertices().size());
+ const Vector3d radius = Vector3d::Constant(m_radius);
+
+ std::list<DataStructures::AabbTreeData::Item> items;
+ Vector3d totalPosition = Vector3d::Zero();
+ Matrix33d totalDisplacementSkewSquared = Matrix33d::Zero();
+ size_t id = 0;
+ for (auto const& vertex : getVertices())
+ {
+ totalPosition += vertex.position;
+
+ Matrix33d skewOfDisplacement = makeSkewSymmetricMatrix(vertex.position);
+ totalDisplacementSkewSquared += skewOfDisplacement * skewOfDisplacement;
+
+ SurgSim::Math::Aabbd aabb(vertex.position - radius, vertex.position + radius);
+ items.emplace_back(std::make_pair(std::move(aabb), id));
+
+ id++;
+ }
+
+ m_center = totalPosition / numParticles;
+
+ double sphereVolume = (4.0 / 3.0) * M_PI * m_radius * m_radius * m_radius;
+ m_volume = sphereVolume * numParticles;
+
+ // Parallel Axis Theorem
+ m_secondMomentOfVolume = Matrix33d::Identity() * (2.0 / 5.0) * sphereVolume * m_radius * m_radius * numParticles;
+ m_secondMomentOfVolume -= sphereVolume * totalDisplacementSkewSquared;
+
+ m_aabbTree = std::make_shared<SurgSim::DataStructures::AabbTree>();
+ m_aabbTree->set(std::move(items));
+
+ return true;
+}
+
+std::shared_ptr<Shape> ParticlesShape::getTransformed(const RigidTransform3d& pose) const
+{
+ auto transformed = std::make_shared<ParticlesShape>(*this);
+ transformed->transform(pose);
+ transformed->update();
+ return transformed;
+}
+
+const std::shared_ptr<const SurgSim::DataStructures::AabbTree> ParticlesShape::getAabbTree() const
+{
+ return m_aabbTree;
+}
+
+bool ParticlesShape::isTransformable() const
+{
+ return true;
+}
+
+const Math::Aabbd& ParticlesShape::getBoundingBox() const
+{
+ if (m_aabbTree != nullptr)
+ {
+ return m_aabbTree->getAabb();
+ }
+ else
+ {
+ return m_aabb;
+ }
+}
+
+};
+};
diff --git a/SurgSim/Math/ParticlesShape.h b/SurgSim/Math/ParticlesShape.h
new file mode 100644
index 0000000..4475461
--- /dev/null
+++ b/SurgSim/Math/ParticlesShape.h
@@ -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.
+
+#ifndef SURGSIM_MATH_PARTICLESSHAPE_H
+#define SURGSIM_MATH_PARTICLESSHAPE_H
+
+#include <memory>
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace DataStructures
+{
+class AabbTree;
+};
+
+namespace Math
+{
+
+SURGSIM_STATIC_REGISTRATION(ParticlesShape);
+
+/// Particles Shape: A shape consisting of a group of particles of equal radius
+class ParticlesShape : public Shape, public SurgSim::DataStructures::Vertices<DataStructures::EmptyData>
+{
+public:
+ /// Constructor
+ /// \param radius The particles' radius (in m)
+ explicit ParticlesShape(double radius = 0.0);
+
+ /// Copy constructor
+ /// \param other The ParticleShape to be copied from
+ explicit ParticlesShape(const ParticlesShape& other);
+
+ /// Copy constructor from another Vertices type
+ /// \tparam V type of data stored in the other vertices
+ /// \param other The vertices to be copied from. Vertex data will not be copied
+ template <class V>
+ explicit ParticlesShape(const SurgSim::DataStructures::Vertices<V>& other);
+
+ /// Assignment when the template data is a different type
+ /// \tparam V type of data stored in the other Vertices
+ /// \param other the Vertices to copy from
+ template <class V>
+ ParticlesShape& operator=(const Vertices<V>& other);
+
+ SURGSIM_CLASSNAME(SurgSim::Math::ParticlesShape);
+
+ /// Get the AabbTree
+ /// \return The object's associated AabbTree
+ const std::shared_ptr<const SurgSim::DataStructures::AabbTree> getAabbTree() const;
+
+ /// Set the particles' radius
+ /// \param radius the radius being set to all particles
+ void setRadius(double radius);
+
+ /// Get the radius of the particles
+ /// \return The particles' radius
+ double getRadius() const;
+
+ int getType() const override;
+
+ double getVolume() const override;
+
+ Vector3d getCenter() const override;
+
+ Matrix33d getSecondMomentOfVolume() const override;
+
+ std::shared_ptr<Shape> getTransformed(const RigidTransform3d& pose) const override;
+
+ bool isValid() const override;
+
+ bool isTransformable() const override;
+
+ const Math::Aabbd& getBoundingBox() const override;
+
+private:
+ bool doUpdate() override;
+
+ /// The aabb tree of the ParticlesShape
+ std::shared_ptr<SurgSim::DataStructures::AabbTree> m_aabbTree;
+
+ /// Particles' radius
+ double m_radius;
+
+ /// Volumetric center of particles
+ Vector3d m_center;
+
+ /// Total volume of particles
+ double m_volume;
+
+ /// Second moment of volume
+ Matrix33d m_secondMomentOfVolume;
+};
+
+};
+};
+
+#include "SurgSim/Math/ParticlesShape-inl.h"
+
+#endif // SURGSIM_MATH_PARTICLESSHAPE_H
diff --git a/SurgSim/Math/PlaneShape.cpp b/SurgSim/Math/PlaneShape.cpp
index 0b1ec1c..66e062c 100644
--- a/SurgSim/Math/PlaneShape.cpp
+++ b/SurgSim/Math/PlaneShape.cpp
@@ -25,7 +25,7 @@ PlaneShape::PlaneShape()
{
}
-int PlaneShape::getType()
+int PlaneShape::getType() const
{
return SHAPE_TYPE_PLANE;
}
diff --git a/SurgSim/Math/PlaneShape.h b/SurgSim/Math/PlaneShape.h
index 0178985..1db750a 100644
--- a/SurgSim/Math/PlaneShape.h
+++ b/SurgSim/Math/PlaneShape.h
@@ -43,20 +43,20 @@ public:
SURGSIM_CLASSNAME(SurgSim::Math::PlaneShape);
/// \return the type of the shape
- virtual int getType() override;
+ int getType() const override;
/// Get the volume of the shape
/// \return The volume of the shape (in m-3)
- virtual double getVolume() const override;
+ double getVolume() const override;
/// Get the volumetric center of the shape
/// \return The center of the shape
- virtual Vector3d getCenter() const override;
+ 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;
+ Matrix33d getSecondMomentOfVolume() const override;
/// Gets the d of the plane equation.
/// \return The value of d (always 0).
@@ -68,7 +68,7 @@ public:
/// A PlaneShape is always valid.
/// \return True.
- virtual bool isValid() const override;
+ bool isValid() const override;
};
}; // Math
diff --git a/SurgSim/Math/PointTriangleCcdContactCalculation-inl.h b/SurgSim/Math/PointTriangleCcdContactCalculation-inl.h
new file mode 100644
index 0000000..c273f59
--- /dev/null
+++ b/SurgSim/Math/PointTriangleCcdContactCalculation-inl.h
@@ -0,0 +1,121 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_POINTTRIANGLECCDCONTACTCALCULATION_INL_H
+#define SURGSIM_MATH_POINTTRIANGLECCDCONTACTCALCULATION_INL_H
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "SurgSim/Math/CubicSolver.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// Check if a point belongs to a triangle at a given time of their motion
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param time The time of coplanarity of the 4 points (P(t), A(t), B(t), C(t) are expected to be coplanar)
+/// \param P the point motion (from first to second)
+/// \param A, B, C The triangle points motion (from first to second)
+/// \param[out] barycentricCoordinates The barycentric coordinates of P(t) in ABC(t)
+/// \return true if P(t) is inside the triangle ABC(t)
+template <class T, int MOpt>
+bool isPointInsideTriangle(
+ T time,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& P,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& A,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& B,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& C,
+ Eigen::Matrix<T, 3, 1, MOpt>* barycentricCoordinates)
+{
+ Eigen::Matrix<T, 3, 1, MOpt> Pt = interpolate(P.first, P.second, time);
+ Eigen::Matrix<T, 3, 1, MOpt> At = interpolate(A.first, A.second, time);
+ Eigen::Matrix<T, 3, 1, MOpt> Bt = interpolate(B.first, B.second, time);
+ Eigen::Matrix<T, 3, 1, MOpt> Ct = interpolate(C.first, C.second, time);
+
+ bool result = Math::barycentricCoordinates(Pt, At, Bt, Ct, barycentricCoordinates);
+
+ for (int i = 0; i < 3; i++)
+ {
+ if (std::abs((*barycentricCoordinates)[i]) < Math::Geometry::ScalarEpsilon)
+ {
+ (*barycentricCoordinates)[i] = 0.0;
+ }
+ if (std::abs(1.0 - (*barycentricCoordinates)[i]) < Math::Geometry::ScalarEpsilon)
+ {
+ (*barycentricCoordinates)[i] = 1.0;
+ }
+ }
+
+ return (result &&
+ (*barycentricCoordinates)[0] >= 0.0 &&
+ (*barycentricCoordinates)[1] >= 0.0 &&
+ (*barycentricCoordinates)[2] >= 0.0);
+}
+
+/// Continuous collision detection between a point P and a triangle ABC
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param P the point motion (from first to second) to calculate the ccd with
+/// \param A, B, C The triangle points motion (from first to second) to calculate the ccd with
+/// \param[out] timeOfImpact The time of impact within [0..1] if a collision is found
+/// \param[out] tv01Param, tv02Param The barycentric coordinate of the contact point in the triangle
+/// i.e. ContactPoint(timeOfImpact) = A(timeOfImpact) + tv01Param.AB(timeOfImpact) + tv02Param.AC(timeOfImpact)
+/// \return True if the given point/triangle motions intersect, False otherwise
+/// \note Simple cubic-solver https://graphics.stanford.edu/courses/cs468-02-winter/Papers/Collisions_vetements.pdf
+/// \note Optimized method http://www.robotics.sei.ecnu.edu.cn/CCDLY/GMOD15.pdf
+/// \note Optimized method https://www.cs.ubc.ca/~rbridson/docs/brochu-siggraph2012-ccd.pdf
+template <class T, int MOpt> inline
+bool calculateCcdContactPointTriangle(
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& P,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& A,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& B,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& C,
+ T* timeOfImpact, T* tv01Param, T* tv02Param)
+{
+ std::array<T, 3> roots;
+ int numberOfRoots = timesOfCoplanarityInRange01(P, A, B, C, &roots);
+
+ // The roots are all in [0..1] and ordered ascendingly
+ for (int rootId = 0; rootId < numberOfRoots; ++rootId)
+ {
+ Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
+ if (isPointInsideTriangle(roots[rootId], P, A, B, C, &baryCoords))
+ {
+ // The point P is in the triangle plane at time t, and is inside the triangle
+ *timeOfImpact = roots[rootId];
+ *tv01Param = baryCoords[1];
+ *tv02Param = baryCoords[2];
+
+ // None of these assertion should be necessary, but just to double check
+ SURGSIM_ASSERT(*timeOfImpact >= 0.0 && *timeOfImpact <= 1.0);
+ SURGSIM_ASSERT(*tv01Param >= 0.0);
+ SURGSIM_ASSERT(*tv02Param >= 0.0);
+ SURGSIM_ASSERT(*tv01Param + *tv02Param <= 1.0 + Geometry::ScalarEpsilon);
+
+ return true;
+ }
+ }
+
+ return false;
+}
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_POINTTRIANGLECCDCONTACTCALCULATION_INL_H
diff --git a/SurgSim/Math/Polynomial-inl.h b/SurgSim/Math/Polynomial-inl.h
new file mode 100644
index 0000000..662c4c6
--- /dev/null
+++ b/SurgSim/Math/Polynomial-inl.h
@@ -0,0 +1,699 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_POLYNOMIAL_INL_H
+#define SURGSIM_MATH_POLYNOMIAL_INL_H
+
+#include <stdlib.h>
+#include <algorithm>
+
+#include "SurgSim/Math/IntervalArithmetic.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+template <typename T>
+bool isNearZero(const T& value, const T& epsilon)
+{
+ return (value + epsilon >= 0 && value - epsilon <= 0);
+}
+
+// Polynomial of degree 0
+
+template <class T>
+Polynomial<T, 0>::Polynomial() : m_a0(static_cast<T>(0))
+{
+}
+
+template <class T>
+Polynomial<T, 0>::Polynomial(const T& a0) : m_a0(a0)
+{
+}
+
+template <class T>
+T Polynomial<T, 0>::evaluate(const T& x) const
+{
+ return m_a0;
+}
+
+template <class T>
+T Polynomial<T, 0>::operator()(const T& x) const
+{
+ return evaluate(x);
+}
+
+template <class T>
+T& Polynomial<T, 0>::operator[](const size_t i)
+{
+ return const_cast<T&>(const_cast<const Polynomial<T, 0>*>(this)->operator[](i));
+}
+
+template <class T>
+const T& Polynomial<T, 0>::operator[](const size_t i) const
+{
+ SURGSIM_ASSERT(i <= 0) << "Attempting to set a coefficient greater than the polynomial degree";
+ return m_a0;
+}
+
+template <class T>
+Polynomial<T, 0> Polynomial<T, 0>::operator- () const
+{
+ return Polynomial(-m_a0);
+}
+
+template <class T>
+Polynomial<T, 0> Polynomial<T, 0>::operator+ (const Polynomial<T, 0>& rhs) const
+{
+ return Polynomial(m_a0 + rhs.m_a0);
+}
+
+template <class T>
+Polynomial<T, 0>& Polynomial<T, 0>::operator+= (const Polynomial<T, 0>& rhs)
+{
+ m_a0 += rhs.m_a0;
+ return *this;
+}
+
+template <class T>
+Polynomial<T, 0> Polynomial<T, 0>::operator- (const Polynomial<T, 0>& rhs) const
+{
+ return Polynomial(m_a0 - rhs.m_a0);
+}
+
+template <class T>
+Polynomial<T, 0>& Polynomial<T, 0>::operator-= (const Polynomial<T, 0>& rhs)
+{
+ m_a0 -= rhs.m_a0;
+ return *this;
+}
+
+template <class T>
+bool Polynomial<T, 0>::isNearZero(const T& epsilon) const
+{
+ return SurgSim::Math::isNearZero(m_a0, epsilon);
+}
+
+template <class T>
+bool Polynomial<T, 0>::isApprox(const Polynomial<T, 0>& p, const T& epsilon) const
+{
+ return ((*this) - p).isNearZero(epsilon);
+}
+
+template <class T>
+T Polynomial<T, 0>::getCoefficient(size_t i) const
+{
+ switch (i)
+ {
+ case 0:
+ return m_a0;
+ default:
+ return 0;
+ }
+}
+
+template <class T>
+void Polynomial<T, 0>::setCoefficient(size_t i, const T& value)
+{
+ SURGSIM_ASSERT(i <= 0) << "Attempting to set a coefficient greater than the polynomial degree";
+ m_a0 = value;
+}
+
+// Polynomial of degree 1
+
+template <class T>
+Polynomial<T, 1>::Polynomial() : m_a0(static_cast<T>(0)), m_a1(static_cast<T>(0))
+{
+}
+
+template <class T>
+Polynomial<T, 1>::Polynomial(const T& a0, const T& a1) : m_a0(a0), m_a1(a1)
+{
+}
+
+template <class T>
+T Polynomial<T, 1>::evaluate(const T& x) const
+{
+ return m_a1 * x + m_a0;
+}
+
+template <class T>
+T Polynomial<T, 1>::operator()(const T& x) const
+{
+ return evaluate(x);
+}
+
+template <class T>
+T& Polynomial<T, 1>::operator[](const size_t i)
+{
+ return const_cast<T&>(const_cast<const Polynomial<T, 1>*>(this)->operator[](i));
+}
+
+template <class T>
+const T& Polynomial<T, 1>::operator[](const size_t i) const
+{
+ SURGSIM_ASSERT(i <= 1) << "Attempting to set a coefficient greater than the polynomial degree";
+ switch (i)
+ {
+ case 0:
+ {
+ return m_a0;
+ }
+ default:
+ {
+ return m_a1;
+ }
+ }
+}
+
+template <class T>
+Polynomial<T, 1> Polynomial<T, 1>::operator- () const
+{
+ return Polynomial(-m_a0, -m_a1);
+}
+
+template <class T>
+Polynomial<T, 1> Polynomial<T, 1>::operator+ (const Polynomial<T, 1>& rhs) const
+{
+ return Polynomial(m_a0 + rhs.m_a0, m_a1 + rhs.m_a1);
+}
+
+template <class T>
+Polynomial<T, 1>& Polynomial<T, 1>::operator+= (const Polynomial<T, 1>& rhs)
+{
+ m_a0 += rhs.m_a0;
+ m_a1 += rhs.m_a1;
+ return *this;
+}
+
+template <class T>
+Polynomial<T, 1> Polynomial<T, 1>::operator- (const Polynomial<T, 1>& rhs) const
+{
+ return Polynomial(m_a0 - rhs.m_a0, m_a1 - rhs.m_a1);
+}
+
+template <class T>
+Polynomial<T, 1>& Polynomial<T, 1>::operator-= (const Polynomial<T, 1>& rhs)
+{
+ m_a0 -= rhs.m_a0;
+ m_a1 -= rhs.m_a1;
+ return *this;
+}
+
+template <class T>
+Polynomial<T, 0> Polynomial<T, 1>::derivative() const
+{
+ return Polynomial<T, 0>(m_a1);
+}
+
+template <class T>
+bool Polynomial<T, 1>::isNearZero(const T& epsilon) const
+{
+ return SurgSim::Math::isNearZero(m_a0, epsilon) && SurgSim::Math::isNearZero(m_a1, epsilon);
+}
+
+template <class T>
+bool Polynomial<T, 1>::isApprox(const Polynomial<T, 1>& p, const T& epsilon) const
+{
+ return ((*this) - p).isNearZero(epsilon);
+}
+
+template <class T>
+T Polynomial<T, 1>::getCoefficient(size_t i) const
+{
+ switch (i)
+ {
+ case 0:
+ {
+ return m_a0;
+ }
+ case 1:
+ {
+ return m_a1;
+ }
+ default:
+ {
+ return 0;
+ }
+ }
+}
+
+template <class T>
+void Polynomial<T, 1>::setCoefficient(size_t i, const T& value)
+{
+ SURGSIM_ASSERT(i <= 1) << "Attempting to set a coefficient greater than the polynomial degree";
+ switch (i)
+ {
+ case 0:
+ {
+ m_a0 = value;
+ break;
+ }
+ case 1:
+ {
+ m_a1 = value;
+ break;
+ }
+ }
+}
+
+// Polynomial of degree 2
+
+template <class T>
+Polynomial<T, 2>::Polynomial() : m_a0(static_cast<T>(0)), m_a1(static_cast<T>(0)), m_a2(static_cast<T>(0))
+{
+}
+
+template <class T>
+Polynomial<T, 2>::Polynomial(const T& a0, const T& a1, const T& a2) : m_a0(a0), m_a1(a1), m_a2(a2)
+{
+}
+
+template <class T>
+T Polynomial<T, 2>::discriminant() const
+{
+ return m_a1 * m_a1 - static_cast<T>(4) * m_a0 * m_a2;
+}
+
+template <class T>
+T Polynomial<T, 2>::evaluate(const T& x) const
+{
+ return (m_a2 * x + m_a1) * x + m_a0;
+}
+
+template <class T>
+T Polynomial<T, 2>::operator()(const T& x) const
+{
+ return evaluate(x);
+}
+
+template <class T>
+T& Polynomial<T, 2>::operator[](const size_t i)
+{
+ return const_cast<T&>(const_cast<const Polynomial<T, 2>*>(this)->operator[](i));
+}
+
+template <class T>
+const T& Polynomial<T, 2>::operator[](const size_t i) const
+{
+ SURGSIM_ASSERT(i <= 2) << "Attempting to set a coefficient greater than the polynomial degree";
+ switch (i)
+ {
+ case 0:
+ {
+ return m_a0;
+ }
+ case 1:
+ {
+ return m_a1;
+ }
+ default:
+ {
+ return m_a2;
+ }
+ }
+}
+
+template <class T>
+Polynomial<T, 2> Polynomial<T, 2>::operator- () const
+{
+ return Polynomial(-m_a0, -m_a1, -m_a2);
+}
+
+template <class T>
+Polynomial<T, 2> Polynomial<T, 2>::operator+ (const Polynomial<T, 2>& rhs) const
+{
+ return Polynomial(m_a0 + rhs.m_a0, m_a1 + rhs.m_a1, m_a2 + rhs.m_a2);
+}
+
+template <class T>
+Polynomial<T, 2>& Polynomial<T, 2>::operator+= (const Polynomial<T, 2>& rhs)
+{
+ m_a0 += rhs.m_a0;
+ m_a1 += rhs.m_a1;
+ m_a2 += rhs.m_a2;
+ return *this;
+}
+
+template <class T>
+Polynomial<T, 2> Polynomial<T, 2>::operator- (const Polynomial<T, 2>& rhs) const
+{
+ return Polynomial(m_a0 - rhs.m_a0, m_a1 - rhs.m_a1, m_a2 - rhs.m_a2);
+}
+
+template <class T>
+Polynomial<T, 2>& Polynomial<T, 2>::operator-= (const Polynomial<T, 2>& rhs)
+{
+ m_a0 -= rhs.m_a0;
+ m_a1 -= rhs.m_a1;
+ m_a2 -= rhs.m_a2;
+ return *this;
+}
+
+template <class T>
+Polynomial<T, 1> Polynomial<T, 2>::derivative() const
+{
+ return Polynomial<T, 1>(m_a1, 2 * m_a2);
+}
+
+template <class T>
+bool Polynomial<T, 2>::isNearZero(const T& epsilon) const
+{
+ return SurgSim::Math::isNearZero(m_a0, epsilon) &&
+ SurgSim::Math::isNearZero(m_a1, epsilon) &&
+ SurgSim::Math::isNearZero(m_a2, epsilon);
+}
+
+template <class T>
+bool Polynomial<T, 2>::isApprox(const Polynomial<T, 2>& p, const T& epsilon) const
+{
+ return ((*this) - p).isNearZero(epsilon);
+}
+
+template <class T>
+T Polynomial<T, 2>::getCoefficient(size_t i) const
+{
+ switch (i)
+ {
+ case 0:
+ {
+ return m_a0;
+ }
+ case 1:
+ {
+ return m_a1;
+ }
+ case 2:
+ {
+ return m_a2;
+ }
+ default:
+ {
+ return 0;
+ }
+ }
+}
+
+template <class T>
+void Polynomial<T, 2>::setCoefficient(size_t i, const T& value)
+{
+ SURGSIM_ASSERT(i <= 2) << "Attempting to set a coefficient greater than the polynomial degree";
+ switch (i)
+ {
+ case 0:
+ {
+ m_a0 = value;
+ break;
+ }
+ case 1:
+ {
+ m_a1 = value;
+ break;
+ }
+ case 2:
+ {
+ m_a2 = value;
+ break;
+ }
+ }
+}
+
+// Polynomial of degree 3
+
+template <class T>
+Polynomial<T, 3>::Polynomial() :
+ m_a0(static_cast<T>(0)),
+ m_a1(static_cast<T>(0)),
+ m_a2(static_cast<T>(0)),
+ m_a3(static_cast<T>(0))
+{
+}
+
+template <class T>
+Polynomial<T, 3>::Polynomial(const T& a0, const T& a1, const T& a2, const T& a3) :
+ m_a0(a0),
+ m_a1(a1),
+ m_a2(a2),
+ m_a3(a3)
+{
+}
+
+template <class T>
+T Polynomial<T, 3>::evaluate(const T& x) const
+{
+ return ((m_a3 * x + m_a2) * x + m_a1) * x + m_a0;
+}
+
+template <class T>
+T Polynomial<T, 3>::operator()(const T& x) const
+{
+ return evaluate(x);
+}
+
+template <class T>
+T& Polynomial<T, 3>::operator[](const size_t i)
+{
+ return const_cast<T&>(const_cast<const Polynomial<T, 3>*>(this)->operator[](i));
+}
+
+template <class T>
+const T& Polynomial<T, 3>::operator[](const size_t i) const
+{
+ SURGSIM_ASSERT(i <= 3) << "Attempting to set or access a coefficient greater than the polynomial degree";
+ switch (i)
+ {
+ case 0:
+ {
+ return m_a0;
+ }
+ case 1:
+ {
+ return m_a1;
+ }
+ case 2:
+ {
+ return m_a2;
+ }
+ default:
+ {
+ return m_a3;
+ }
+ }
+}
+
+template <class T>
+Polynomial<T, 3> Polynomial<T, 3>::operator- () const
+{
+ return Polynomial(-m_a0, -m_a1, -m_a2, -m_a3);
+}
+
+template <class T>
+Polynomial<T, 3> Polynomial<T, 3>::operator+ (const Polynomial<T, 3>& rhs) const
+{
+ return Polynomial(m_a0 + rhs.m_a0, m_a1 + rhs.m_a1, m_a2 + rhs.m_a2, m_a3 + rhs.m_a3);
+}
+
+template <class T>
+Polynomial<T, 3>& Polynomial<T, 3>::operator+= (const Polynomial<T, 3>& rhs)
+{
+ m_a0 += rhs.m_a0;
+ m_a1 += rhs.m_a1;
+ m_a2 += rhs.m_a2;
+ m_a3 += rhs.m_a3;
+ return *this;
+}
+
+template <class T>
+Polynomial<T, 3> Polynomial<T, 3>::operator- (const Polynomial<T, 3>& rhs) const
+{
+ return Polynomial(m_a0 - rhs.m_a0, m_a1 - rhs.m_a1, m_a2 - rhs.m_a2, m_a3 - rhs.m_a3);
+}
+
+template <class T>
+Polynomial<T, 3>& Polynomial<T, 3>::operator-= (const Polynomial<T, 3>& rhs)
+{
+ m_a0 -= rhs.m_a0;
+ m_a1 -= rhs.m_a1;
+ m_a2 -= rhs.m_a2;
+ m_a3 -= rhs.m_a3;
+ return *this;
+}
+
+template <class T>
+Polynomial<T, 2> Polynomial<T, 3>::derivative() const
+{
+ return Polynomial<T, 2>(m_a1, 2 * m_a2, 3 * m_a3);
+}
+
+template <class T>
+bool Polynomial<T, 3>::isNearZero(const T& epsilon) const
+{
+ return SurgSim::Math::isNearZero(m_a0, epsilon) &&
+ SurgSim::Math::isNearZero(m_a1, epsilon) &&
+ SurgSim::Math::isNearZero(m_a2, epsilon) &&
+ SurgSim::Math::isNearZero(m_a3, epsilon);
+}
+
+template <class T>
+bool Polynomial<T, 3>::isApprox(const Polynomial<T, 3>& p, const T& epsilon) const
+{
+ return ((*this) - p).isNearZero(epsilon);
+}
+
+template <class T>
+T Polynomial<T, 3>::getCoefficient(size_t i) const
+{
+ switch (i)
+ {
+ case 0:
+ {
+ return m_a0;
+ }
+ case 1:
+ {
+ return m_a1;
+ }
+ case 2:
+ {
+ return m_a2;
+ }
+ case 3:
+ {
+ return m_a3;
+ }
+ default:
+ {
+ return 0;
+ }
+ }
+}
+
+template <class T>
+void Polynomial<T, 3>::setCoefficient(size_t i, const T& value)
+{
+ SURGSIM_ASSERT(i <= 3) << "Attempting to set a coefficient greater than the polynomial degree";
+ switch (i)
+ {
+ case 0:
+ {
+ m_a0 = value;
+ break;
+ }
+ case 1:
+ {
+ m_a1 = value;
+ break;
+ }
+ case 2:
+ {
+ m_a2 = value;
+ break;
+ }
+ case 3:
+ {
+ m_a3 = value;
+ break;
+ }
+ }
+}
+
+// Operators
+
+template <typename T, int N, int M>
+Polynomial < T, N + M > operator*(const Polynomial<T, N>& p, const Polynomial<T, M>& q)
+{
+ Polynomial < T, N + M > result;
+ for (int i = 0; i <= N + M; ++i)
+ {
+ T coeff = 0;
+ int jMin = std::max(0, i - M);
+ int jMax = std::min(i, N);
+ for (int j = jMin; j <= jMax; ++j)
+ {
+ coeff += p.getCoefficient(j) * q.getCoefficient(i - j);
+ }
+ result.setCoefficient(i, coeff);
+ }
+ return result;
+}
+
+template <typename T>
+Polynomial<T, 2> operator*(const Polynomial<T, 1>& p, const Polynomial<T, 1>& q)
+{
+ const T p0 = p.getCoefficient(0);
+ const T p1 = p.getCoefficient(1);
+ const T q0 = q.getCoefficient(0);
+ const T q1 = q.getCoefficient(1);
+ return Polynomial<T, 2>(p0 * q0, p0 * q1 + p1 * q0, p1 * q1);
+}
+
+template <typename T>
+Polynomial<T, 3> operator*(const Polynomial<T, 2>& p, const Polynomial<T, 1>& q)
+{
+ const T p0 = p.getCoefficient(0);
+ const T p1 = p.getCoefficient(1);
+ const T p2 = p.getCoefficient(2);
+ const T q0 = q.getCoefficient(0);
+ const T q1 = q.getCoefficient(1);
+ return Polynomial<T, 3>(p0 * q0, p0 * q1 + p1 * q0, p1 * q1 + p2 * q0, p2 * q1);
+}
+
+template <typename T>
+Polynomial<T, 3> operator*(const Polynomial<T, 1>& p, const Polynomial<T, 2>& q)
+{
+ const T p0 = p.getCoefficient(0);
+ const T p1 = p.getCoefficient(1);
+ const T q0 = q.getCoefficient(0);
+ const T q1 = q.getCoefficient(1);
+ const T q2 = q.getCoefficient(2);
+ return Polynomial<T, 3>(p0 * q0, p0 * q1 + p1 * q0, p0 * q2 + p1 * q1, p1 * q2);
+}
+
+template <typename T>
+Polynomial<T, 0> square(const Polynomial<T, 0>& p)
+{
+ const T c = p.getCoefficient(0);
+ return Polynomial<T, 0>(c * c);
+}
+
+template <typename T>
+Polynomial<T, 2> square(const Polynomial<T, 1>& p)
+{
+ const T p0 = p.getCoefficient(0);
+ const T p1 = p.getCoefficient(1);
+ return Polynomial<T, 2>(p0 * p0, 2 * p1 * p0, p1 * p1);
+}
+
+template <typename T, int N>
+inline std::ostream& operator<<(std::ostream& stream, const Polynomial<T, N>& p)
+{
+ stream << "(";
+ for (int i = N; i > 1; --i)
+ {
+ stream << p.getCoefficient(i) << "*x^" << i << " + ";
+ }
+ if (N >= 1)
+ {
+ stream << p.getCoefficient(1) << "*x + ";
+ }
+ stream << p.getCoefficient(0) << ")";
+ return stream;
+}
+
+}; // Math
+}; // SurgSim
+
+#endif // SURGSIM_MATH_POLYNOMIAL_INL_H
diff --git a/SurgSim/Math/Polynomial.h b/SurgSim/Math/Polynomial.h
new file mode 100644
index 0000000..86031f5
--- /dev/null
+++ b/SurgSim/Math/Polynomial.h
@@ -0,0 +1,388 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_POLYNOMIAL_H
+#define SURGSIM_MATH_POLYNOMIAL_H
+
+#include <iostream>
+
+namespace SurgSim
+{
+namespace Math
+{
+
+namespace
+{
+double polynomial_epsilon = 1.0e-09;
+}
+
+/// Define an utility function for comparing individual coefficients to 0.
+///
+/// \tparam T underlying data type used as either the scalar or as the data type
+/// for the interval
+/// \param value the value to compare
+/// \param epsilon the tolerance
+/// \return true if the value is within epsilon of 0
+template <typename T>
+bool isNearZero(const T& value, const T& epsilon = static_cast<T>(polynomial_epsilon));
+
+/// Polynomial<T, N> defines the concept of an N degree polynomial with type T
+/// coefficients and provides operations on them including arithmetic operations,
+/// construction, and IO.
+///
+/// \tparam T underlying data type over which the interval is defined.
+/// \tparam N degree of the Polynomial
+template <typename T, int N> class Polynomial
+{
+ static_assert(N >= 0, "Polynomials must have degree >= 0.");
+ static_assert(N <= 3, "Polynomials of degree > 3 are not yet supported.");
+};
+
+/// Polynomial<T, 0> specializes the Polynomial class for degree 0 (constant polynomials)
+/// \sa Polynomial<T, N>
+template <typename T>
+class Polynomial<T, 0>
+{
+public:
+ /// Constructor
+ Polynomial();
+
+ /// Constructor
+ /// \param a0 coefficient of the 0 degree term
+ explicit Polynomial(const T& a0);
+
+ /// Evaluate the polynomial at a point
+ /// \param x point at which to evaluate the polynomial
+ /// \return the value of the polynomial at x
+ T evaluate(const T& x) const;
+
+ /// Evaluate the polynomial at a point
+ /// \param x point at which to evaluate the polynomial
+ /// \return the value of the polynomial at x
+ T operator()(const T& x) const;
+
+ /// @{
+ /// Standard arithmetic operators extended to intervals
+ T& operator[](const size_t i);
+ const T& operator[](const size_t i) const;
+ Polynomial<T, 0> operator- () const;
+ Polynomial<T, 0> operator+ (const Polynomial<T, 0>& rhs) const;
+ Polynomial<T, 0>& operator+= (const Polynomial<T, 0>& rhs);
+ Polynomial<T, 0> operator- (const Polynomial<T, 0>& rhs) const;
+ Polynomial<T, 0>& operator-= (const Polynomial<T, 0>& rhs);
+ /// @}
+
+ /// \param epsilon the closeness parameter
+ /// \return true if all coefficients of the polynomial are within an epsilon of 0
+ bool isNearZero(const T& epsilon = static_cast<T>(polynomial_epsilon)) const;
+
+ /// \param p the test polynomial
+ /// \param epsilon the closeness parameter
+ /// \return true if all the coefficients of the current polynomial are within an epsilon of
+ /// the corresponding coefficient in p
+ bool isApprox(const Polynomial<T, 0>& p, const T& epsilon) const;
+
+ /// \param i index of the desired coefficient
+ /// \return the value of coefficient i
+ T getCoefficient(size_t i) const;
+
+ /// Set a specified coefficient to a desired value
+ /// \param i index of the desired coefficient
+ /// \param value the value to be set in the coefficient
+ /// \exception if i is greater than the polynomial degree
+ void setCoefficient(size_t i, const T& value);
+
+private:
+ /// @{
+ /// Coefficient of the polynomial
+ T m_a0;
+ /// @}
+};
+
+/// Polynomial<T, 1> specializes the Polynomial class for degree 1 (linear polynomials)
+/// \sa Polynomial<T, N>
+template <typename T>
+class Polynomial<T, 1>
+{
+public:
+ /// Constructor
+ Polynomial();
+
+ /// Constructor
+ /// \param a0 coefficient of the 0 degree term
+ /// \param a1 coefficient of the 1 degree term
+ Polynomial(const T& a0, const T& a1);
+
+ /// Evaluate the polynomial at a point
+ /// \param x point at which to evaluate the polynomial
+ /// \return the value of the polynomial at x
+ T evaluate(const T& x) const;
+
+ /// Evaluate the polynomial at a point
+ /// \param x point at which to evaluate the polynomial
+ /// \return the value of the polynomial at x
+ T operator()(const T& x) const;
+
+ /// @{
+ /// Standard arithmetic operators extended to intervals
+ T& operator[](const size_t i);
+ const T& operator[](const size_t i) const;
+ Polynomial<T, 1> operator- () const;
+ Polynomial<T, 1> operator+ (const Polynomial<T, 1>& rhs) const;
+ Polynomial<T, 1>& operator+= (const Polynomial<T, 1>& rhs);
+ Polynomial<T, 1> operator- (const Polynomial<T, 1>& rhs) const;
+ Polynomial<T, 1>& operator-= (const Polynomial<T, 1>& rhs);
+ /// @}
+
+ /// \return the derivative of the polynomial
+ Polynomial<T, 0> derivative() const;
+
+ /// \param epsilon the closeness parameter
+ /// \return true if all coefficients of the polynomial are within an epsilon of 0
+ bool isNearZero(const T& epsilon = static_cast<T>(polynomial_epsilon)) const;
+
+ /// \param p the test polynomial
+ /// \param epsilon the closeness parameter
+ /// \return true if all the coefficients of the current polynomial are within an epsilon of
+ /// the corresponding coefficient in p
+ bool isApprox(const Polynomial<T, 1>& p, const T& epsilon) const;
+
+ /// \param i index of the desired coefficient
+ /// \return the value of coefficient i
+ T getCoefficient(size_t i) const;
+
+ /// Set a specified coefficient to a desired value
+ /// \param i index of the desired coefficient
+ /// \param value the value to be set in the coefficient
+ /// \exception if i is greater than the polynomial degree
+ void setCoefficient(size_t i, const T& value);
+
+private:
+ /// @{
+ /// Coefficient of the polynomial
+ T m_a0;
+ T m_a1;
+ /// @}
+};
+
+/// Polynomial<T, 2> specializes the Polynomial class for degree 2 (quadratic polynomials)
+/// \sa Polynomial<T, N>
+template <typename T>
+class Polynomial<T, 2>
+{
+public:
+ /// Constructor
+ Polynomial();
+
+ /// Constructor
+ /// \param a0 coefficient of the 0 degree term
+ /// \param a1 coefficient of the 1 degree term
+ /// \param a2 coefficient of the 2 degree term
+ Polynomial(const T& a0, const T& a1, const T& a2);
+
+ /// Evaluate the discriminant of a quadratic polynomial
+ /// \return the discriminant (b^2 - a4c)
+ T discriminant() const;
+
+ /// Evaluate the polynomial at a point
+ /// \param x point at which to evaluate the polynomial
+ /// \return the value of the polynomial at x
+ T evaluate(const T& x) const;
+
+ /// Evaluate the polynomial at a point
+ /// \param x point at which to evaluate the polynomial
+ /// \return the value of the polynomial at x
+ T operator()(const T& x) const;
+
+ /// @{
+ /// Standard arithmetic operators extended to intervals
+ T& operator[](const size_t i);
+ const T& operator[](const size_t i) const;
+ Polynomial<T, 2> operator- () const;
+ Polynomial<T, 2> operator+ (const Polynomial<T, 2>& rhs) const;
+ Polynomial<T, 2>& operator+= (const Polynomial<T, 2>& rhs);
+ Polynomial<T, 2> operator- (const Polynomial<T, 2>& rhs) const;
+ Polynomial<T, 2>& operator-= (const Polynomial<T, 2>& rhs);
+ /// @}
+
+ /// \return the derivative of the polynomial
+ Polynomial<T, 1> derivative() const;
+
+ /// \param epsilon the closeness parameter
+ /// \return true if all coefficients of the polynomial are within an epsilon of 0
+ bool isNearZero(const T& epsilon = static_cast<T>(polynomial_epsilon)) const;
+
+ /// \param p the test polynomial
+ /// \param epsilon the closeness parameter
+ /// \return true if all the coefficients of the current polynomial are within an epsilon of
+ /// the corresponding coefficient in p
+ bool isApprox(const Polynomial<T, 2>& p, const T& epsilon) const;
+
+ /// \param i index of the desired coefficient
+ /// \return the value of coefficient i
+ T getCoefficient(size_t i) const;
+
+ /// Set a specified coefficient to a desired value
+ /// \param i index of the desired coefficient
+ /// \param value the value to be set in the coefficient
+ /// \exception if i is greater than the polynomial degree
+ void setCoefficient(size_t i, const T& value);
+
+private:
+ /// @{
+ /// Coefficient of the polynomial
+ T m_a0;
+ T m_a1;
+ T m_a2;
+ /// @}
+};
+
+/// Polynomial<T, 3> specializes the Polynomial class for degree 3 (cubic polynomials)
+/// \sa Polynomial<T, N>
+template <typename T>
+class Polynomial<T, 3>
+{
+public:
+ /// Constructor
+ Polynomial();
+
+ /// Constructor
+ /// \param a0 coefficient of the 0 degree term
+ /// \param a1 coefficient of the 1 degree term
+ /// \param a2 coefficient of the 2 degree term
+ /// \param a3 coefficient of the 3 degree term
+ Polynomial(const T& a0, const T& a1, const T& a2, const T& a3);
+
+ /// Evaluate the polynomial at a point
+ /// \param x point at which to evaluate the polynomial
+ /// \return the value of the polynomial at x
+ T evaluate(const T& x) const;
+
+ /// Evaluate the polynomial at a point
+ /// \param x point at which to evaluate the polynomial
+ /// \return the value of the polynomial at x
+ T operator()(const T& x) const;
+
+ /// @{
+ /// Standard arithmetic operators extended to intervals
+ T& operator[](const size_t i);
+ const T& operator[](const size_t i) const;
+ Polynomial<T, 3> operator- () const;
+ Polynomial<T, 3> operator+ (const Polynomial<T, 3>& rhs) const;
+ Polynomial<T, 3>& operator+= (const Polynomial<T, 3>& rhs);
+ Polynomial<T, 3> operator- (const Polynomial<T, 3>& rhs) const;
+ Polynomial<T, 3>& operator-= (const Polynomial<T, 3>& rhs);
+ /// @}
+
+ /// \return the derivative of the polynomial
+ Polynomial<T, 2> derivative() const;
+
+ /// \param epsilon the closeness parameter
+ /// \return true if all coefficients of the polynomial are within an epsilon of 0
+ bool isNearZero(const T& epsilon = static_cast<T>(polynomial_epsilon)) const;
+
+ /// \param p the test polynomial
+ /// \param epsilon the closeness parameter
+ /// \return true if all the coefficients of the current polynomial are within an epsilon of
+ /// the corresponding coefficient in p
+ bool isApprox(const Polynomial<T, 3>& p, const T& epsilon) const;
+
+ /// \param i index of the desired coefficient
+ /// \return the value of coefficient i
+ T getCoefficient(size_t i) const;
+
+ /// Set a specified coefficient to a desired value
+ /// \param i index of the desired coefficient
+ /// \param value the value to be set in the coefficient
+ /// \exception if i is greater than the polynomial degree
+ void setCoefficient(size_t i, const T& value);
+
+private:
+ /// @{
+ /// Coefficient of the polynomial
+ T m_a0;
+ T m_a1;
+ T m_a2;
+ T m_a3;
+ /// @}
+};
+
+// Operators
+
+/// Multiply two polynomials of arbitrary degree. This current implementation is limited to
+/// a resulting polynomial of no more than degree 3 (i.e. N + M <= 3) because of limits in the
+/// underlying polynomial representations.
+/// \tparam N degree of the first polynomial
+/// \tparam M degree of the second polynomial
+/// \tparam T underlying data type over which the interval is defined.
+/// \param p first polynomial of degree N
+/// \param q second polynomial of degree M
+/// \return p * q as a polynomial of degree N + M
+template <typename T, int N, int M>
+Polynomial < T, N + M > operator*(const Polynomial<T, N>& p, const Polynomial<T, M>& q);
+
+/// Multiply two polynomials of degree 1.
+/// \tparam T underlying data type over which the interval is defined.
+/// \param p first polynomial of degree 1
+/// \param q second polynomial of degree 1
+/// \return p * q as a polynomial of degree 2
+template <typename T>
+Polynomial<T, 2> operator*(const Polynomial<T, 1>& p, const Polynomial<T, 1>& q);
+
+/// Multiply two polynomials of degree 2 and 1 respectively.
+/// \tparam T underlying data type over which the interval is defined.
+/// \param p first polynomial of degree 2
+/// \param q second polynomial of degree 1
+/// \return p * q as a polynomial of degree 3
+template <typename T>
+Polynomial<T, 3> operator*(const Polynomial<T, 2>& p, const Polynomial<T, 1>& q);
+
+/// Multiply two polynomials of degree 1 and 2 respectively.
+/// \tparam T underlying data type over which the interval is defined.
+/// \param p first polynomial of degree 1
+/// \param q second polynomial of degree 2
+/// \return p * q as a polynomial of degree 3
+template <typename T>
+Polynomial<T, 3> operator*(const Polynomial<T, 1>& p, const Polynomial<T, 2>& q);
+
+/// Square a degree 0 polynomial
+/// \tparam T underlying data type over which the interval is defined.
+/// \param p polynomial of degree 0
+/// \return p^2 as a polynomial of degree 0
+template <typename T>
+Polynomial<T, 0> square(const Polynomial<T, 0>& p);
+
+/// Square a degree 1 polynomial
+/// \tparam T underlying data type over which the interval is defined.
+/// \param p polynomial of degree 1
+/// \return p^2 as a polynomial of degree 2
+template <typename T>
+Polynomial<T, 2> square(const Polynomial<T, 1>& p);
+
+/// Write a textual version of a Polynomial to an output stream
+/// \tparam T underlying type of the polynomial coefficients
+/// \tparam N degree of the polynomial
+/// \param stream the ostream to be written to
+/// \param p the polynomial to write
+/// \return the active stream
+template <typename T, int N>
+std::ostream& operator<<(std::ostream& stream, const Polynomial<T, N>& p);
+
+}; // Math
+}; // SurgSim
+
+#include "SurgSim/Math/Polynomial-inl.h"
+
+#endif // SURGSIM_MATH_POLYNOMIAL_H
diff --git a/SurgSim/Math/PolynomialRoots-inl.h b/SurgSim/Math/PolynomialRoots-inl.h
new file mode 100644
index 0000000..0bf3f80
--- /dev/null
+++ b/SurgSim/Math/PolynomialRoots-inl.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_MATH_POLYNOMIALROOTS_INL_H
+#define SURGSIM_MATH_POLYNOMIALROOTS_INL_H
+
+namespace SurgSim
+{
+namespace Math
+{
+
+// PolynomialRootsCommon
+template <typename T, int N>
+bool PolynomialRootsCommon<T, N>::isDegenerate() const
+{
+ return m_numRoots == DEGENERATE;
+}
+
+template <typename T, int N>
+int PolynomialRootsCommon<T, N>::getNumRoots() const
+{
+ return m_numRoots;
+}
+
+template <typename T, int N>
+T PolynomialRootsCommon<T, N>::operator[](const int i) const
+{
+ SURGSIM_ASSERT((m_numRoots > i) && (i >= 0)) <<
+ "Requesting a root beyond the number of roots available for this polynomial, " <<
+ "or a root with a negative index.";
+ return m_roots[i];
+}
+
+// roots of an degree-1 polynomial (linear)
+template <typename T>
+PolynomialRoots<T, 1>::PolynomialRoots(const Polynomial<T, 1>& p, const T& epsilon)
+{
+ solve<T, 1>(p.getCoefficient(1), p.getCoefficient(0), static_cast<T>(epsilon),
+ &(this->m_numRoots), &(this->m_roots));
+}
+
+// roots of an degree-2 polynomial (quadratic)
+template <typename T>
+PolynomialRoots<T, 2>::PolynomialRoots(const Polynomial<T, 2>& p, const T& epsilon)
+{
+ solve<T, 2>(p.getCoefficient(2), p.getCoefficient(1), p.getCoefficient(0), static_cast<T>(epsilon),
+ &(this->m_numRoots), &(this->m_roots));
+}
+
+// Utilities: Solve for roots of linear equation a * x + b = y
+template <typename T, int N>
+void solve(const T& a, const T& b, const T& epsilon, int* numRoots, std::array<T, N>* roots)
+{
+ static_assert(N >= 1, "Root array is not large enough to hold the roots of the polynomial");
+ if (isNearZero(a, epsilon))
+ {
+ // The "1-st degree polynomial" is really close to a constant.
+ // If the constant is zero, there are infinitely many solutions; otherwise there are zero.
+ if (isNearZero(b, epsilon))
+ {
+ *numRoots = PolynomialRootsCommon<T, N>::DEGENERATE; // infinitely many solutions
+ }
+ else
+ {
+ *numRoots = 0;
+ }
+ }
+ else
+ {
+ *numRoots = 1;
+ (*roots)[0] = -b / a;
+ }
+}
+
+// Utilities: Solve for roots of quadratic equation a * x^2 + b * x + c = y
+template <typename T, int N>
+void solve(const T& a, const T& b, const T& c, const T& epsilon, int* numRoots, std::array<T, N>* roots)
+{
+ static_assert(N >= 2, "Root array is not large enough to hold the roots of the polynomial");
+
+ if (isNearZero(a, epsilon))
+ {
+ // The "2nd degree polynomial" is really (close to) 1st degree or less.
+ // We can delegate the solution in this case.
+ solve<T, N>(b, c, epsilon, numRoots, roots);
+ return;
+ }
+
+ T discriminant = b * b - 4.0 * a * c;
+ if (discriminant > epsilon)
+ {
+ *numRoots = 2;
+ T sqrtDiscriminant = sqrt(discriminant);
+ (*roots)[0] = (-b - sqrtDiscriminant) / (2 * a);
+ (*roots)[1] = (-b + sqrtDiscriminant) / (2 * a);
+ if ((*roots)[0] > (*roots)[1])
+ {
+ std::swap((*roots)[0], (*roots)[1]);
+ }
+ }
+ else if (discriminant > -epsilon)
+ {
+ *numRoots = 1;
+ (*roots)[0] = -b / (2 * a);
+ }
+ else
+ {
+ *numRoots = 0;
+ }
+}
+
+}; // Math
+}; // SurgSim
+
+#endif // SURGSIM_MATH_POLYNOMIALROOTS_INL_H
diff --git a/SurgSim/Math/PolynomialRoots.h b/SurgSim/Math/PolynomialRoots.h
new file mode 100644
index 0000000..a1a308b
--- /dev/null
+++ b/SurgSim/Math/PolynomialRoots.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_MATH_POLYNOMIALROOTS_H
+#define SURGSIM_MATH_POLYNOMIALROOTS_H
+
+#include <iostream>
+#include <algorithm>
+
+#include "SurgSim/Math/Polynomial.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// The (algebraic) roots of a Polynomial<N,T>.
+/// If all coefficients of a polynomial are 0, the roots are degenerate: the polynomial equals 0 for any x.
+/// Otherwise, there may be anywhere between 0 and N roots.
+///
+/// \tparam T type of the coefficients and computations
+/// \tparam N degree of the polynomial for which roots are being calculated
+/// \sa PolynomialRootsCommon<T, N>
+template <typename T, int N> class PolynomialRoots;
+
+/// The common base class for PolynomialRoots specializations for various N.
+///
+/// \tparam T type of the coefficients and computations
+/// \tparam N degree of the polynomial for which roots are being calculated
+/// \sa PolynomialRoots<T, N>
+template <typename T, int N>
+class PolynomialRootsCommon
+{
+public:
+ /// Indicator for a degenerate polynomial (infinite number of roots).
+ static const int DEGENERATE = -1;
+
+ /// \return true in the polynomial is degenerate
+ bool isDegenerate() const;
+
+ /// \return the number of available roots or DEGENERATE if there are infinitely many
+ int getNumRoots() const;
+
+ /// Read only access to the roots of the polynomial
+ /// \param i is the number of the root to return
+ /// \return the value of the ith root
+ /// \exception if there is no root of rank i
+ /// \note The roots are ordered ascendingly, so PolynomialRootsCommon[0] < PolynomialRootsCommon[1] < ...
+ T operator[](int i) const;
+
+private:
+ /// @{
+ /// Prohibit copying and assignment.
+ PolynomialRootsCommon(const PolynomialRootsCommon&);
+ PolynomialRootsCommon& operator=(const PolynomialRootsCommon&);
+ /// @}
+
+protected:
+ /// Constructor. Since the constructor must define the roots, only allow construction from a derived class
+ PolynomialRootsCommon() {}
+
+ /// The number of roots available for the polynomial, or DEGENERATE if there are infinite roots
+ int m_numRoots;
+
+ /// An array of up to N roots for a degree N polynomial ordered ascendingly
+ std::array<T, N> m_roots;
+};
+
+/// PolynomialRoots<T, 1> specializes the PolynomialRoots class for degree 1 (linear polynomials)
+/// \sa PolynomialRoots<T, N>
+template <typename T>
+class PolynomialRoots<T, 1> : public PolynomialRootsCommon<T, 1>
+{
+public:
+ /// Constructor
+ /// \param p the degree 1 polynomial for which the roots are to be calculated
+ /// \param epsilon tolerance parameter for determining the number of valid, unique roots
+ explicit PolynomialRoots(const Polynomial<T, 1>& p, const T& epsilon = 1.0e-09);
+};
+
+/// PolynomialRoots<T, 2> specializes the PolynomialRoots class for degree 2 (quadratic polynomials)
+/// \sa PolynomialRoots<T, N>
+template <typename T>
+class PolynomialRoots<T, 2> : public PolynomialRootsCommon<T, 2>
+{
+public:
+ /// Constructor
+ /// \param p the degree 2 polynomial for which the roots are to be calculated
+ /// \param epsilon tolerance parameter for determining the number of valid, unique roots
+ explicit PolynomialRoots(const Polynomial<T, 2>& p, const T& epsilon = 1.0e-09);
+};
+
+/// Specialized solve routine for linear polynomials (2 coefficients)
+/// \tparam N maximum number of roots that can be stored
+/// \tparam T type of the coefficients and computations
+/// \param a coefficient of the linear term
+/// \param b coefficient of the constant term
+/// \param epsilon tolerance parameter for determining the number of valid, unique roots
+/// \param numRoots [out] number of roots calculated, or DEGENERATE if there are infinitely many
+/// \param roots [out] array containing the calculated roots ordered ascendingly
+/// \exception if there are more than N roots
+template <typename T, int N>
+void solve(const T& a, const T& b, const T& epsilon, int* numRoots, std::array<T, N>* roots);
+
+/// Specialized solve routine for quadratic polynomials (3 coefficients)
+/// \tparam N maximum number of roots that can be stored
+/// \tparam T type of the coefficients and computations
+/// \param a coefficient of the square term
+/// \param b coefficient of the linear term
+/// \param c coefficient of the constant term
+/// \param epsilon tolerance parameter for determining the number of valid, unique roots
+/// \param numRoots [out] number of roots calculated, or DEGENERATE if there are infinitely many
+/// \param roots [out] array containing the calculated roots ordered ascendingly
+/// \exception if there are more than N roots
+template <typename T, int N>
+void solve(const T& a, const T& b, const T& c, const T& epsilon, int* numRoots, std::array<T, N>* roots);
+
+}; // Math
+}; // SurgSim
+
+#include "SurgSim/Math/PolynomialRoots-inl.h"
+
+#endif // SURGSIM_MATH_POLYNOMIALROOTS_H
diff --git a/SurgSim/Math/PolynomialValues-inl.h b/SurgSim/Math/PolynomialValues-inl.h
new file mode 100644
index 0000000..10a051f
--- /dev/null
+++ b/SurgSim/Math/PolynomialValues-inl.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_MATH_POLYNOMIALVALUES_INL_H
+#define SURGSIM_MATH_POLYNOMIALVALUES_INL_H
+
+namespace SurgSim
+{
+namespace Math
+{
+
+template <class T>
+PolynomialValues<T, 0>::PolynomialValues(const Polynomial<T, 0>& p) : m_polynomial(p)
+{
+}
+
+template <class T>
+const Polynomial<T, 0>& PolynomialValues<T, 0>::getPolynomial() const
+{
+ return m_polynomial;
+}
+
+template <class T>
+Interval<T> PolynomialValues<T, 0>::valuesOverInterval(const Interval<T>&) const
+{
+ return Interval<T>(m_polynomial.evaluate(0), m_polynomial.evaluate(0));
+}
+
+template <class T>
+PolynomialValues<T, 1>::PolynomialValues(const Polynomial<T, 1>& p) : m_polynomial(p) {}
+
+template <class T>
+const Polynomial<T, 1>& PolynomialValues<T, 1>::getPolynomial() const
+{
+ return m_polynomial;
+}
+
+template <class T>
+Interval<T> PolynomialValues<T, 1>::valuesOverInterval(const Interval<T>& interval) const
+{
+ return Interval<T>::minToMax(m_polynomial.evaluate(interval.getMin()),
+ m_polynomial.evaluate(interval.getMax()));
+}
+
+template <class T>
+PolynomialValues<T, 2>::PolynomialValues(const Polynomial<T, 2>& p) : m_polynomial(p),
+ m_derivative(m_polynomial.derivative()),
+ m_locationOfExtremum(m_derivative)
+{
+}
+
+template <class T>
+const Polynomial<T, 2>& PolynomialValues<T, 2>::getPolynomial() const
+{
+ return m_polynomial;
+}
+
+template <class T>
+const Polynomial<T, 1>& PolynomialValues<T, 2>::getDerivative() const
+{
+ return m_derivative;
+}
+
+template <class T>
+const PolynomialRoots<T, 1>& PolynomialValues<T, 2>::getLocationsOfExtrema() const
+{
+ return m_locationOfExtremum;
+}
+
+template <class T>
+Interval<T> PolynomialValues<T, 2>::valuesOverInterval(const Interval<T>& interval) const
+{
+ // Always consider the endpoints.
+ Interval<T> result = Interval<T>::minToMax(m_polynomial.evaluate(interval.getMin()),
+ m_polynomial.evaluate(interval.getMax()));
+
+ if (m_locationOfExtremum.getNumRoots() > 0)
+ {
+ // There is an extremum (min or max)...
+ if (interval.contains(m_locationOfExtremum[0]))
+ {
+ //...and it occurs somewhere in the middle of the interval.
+ // The value at the extremum needs to be made a part of the result interval.
+ result.extendToInclude(m_polynomial.evaluate(m_locationOfExtremum[0]));
+ }
+ }
+ return result;
+}
+
+template <class T>
+PolynomialValues<T, 3>::PolynomialValues(const Polynomial<T, 3>& p) : m_polynomial(p),
+ m_derivative(m_polynomial.derivative()),
+ m_locationOfExtremum(m_derivative)
+{
+}
+
+template <class T>
+const Polynomial<T, 3>& PolynomialValues<T, 3>::getPolynomial() const
+{
+ return m_polynomial;
+}
+
+template <class T>
+const Polynomial<T, 2>& PolynomialValues<T, 3>::getDerivative() const
+{
+ return m_derivative;
+}
+
+template <class T>
+const PolynomialRoots<T, 2>& PolynomialValues<T, 3>::getLocationsOfExtrema() const
+{
+ return m_locationOfExtremum;
+}
+
+template <class T>
+Interval<T> PolynomialValues<T, 3>::valuesOverInterval(const Interval<T>& interval) const
+{
+ // Always consider the endpoints.
+ Interval<T> result = Interval<T>::minToMax(m_polynomial.evaluate(interval.getMin()),
+ m_polynomial.evaluate(interval.getMax()));
+
+ for (int i = 0; i < m_locationOfExtremum.getNumRoots(); ++i)
+ {
+ // There is an extremum (min or max)...
+ if (interval.contains(m_locationOfExtremum[i]))
+ {
+ //...and it occurs somewhere in the middle of the interval.
+ // The value at the extremum needs to be made a part of the result interval.
+ result.extendToInclude(m_polynomial.evaluate(m_locationOfExtremum[i]));
+ }
+ }
+ return result;
+}
+
+template <class T, int N>
+Interval<T> valuesOverInterval(const Polynomial<T, N>& p, const Interval<T>& interval)
+{
+ return PolynomialValues<T, N>(p).valuesOverInterval(interval);
+}
+
+}; // Math
+}; // SurgSim
+
+#endif // SURGSIM_MATH_POLYNOMIALVALUES_INL_H
diff --git a/SurgSim/Math/PolynomialValues.h b/SurgSim/Math/PolynomialValues.h
new file mode 100644
index 0000000..e4b1d5e
--- /dev/null
+++ b/SurgSim/Math/PolynomialValues.h
@@ -0,0 +1,167 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_POLYNOMIALVALUES_H
+#define SURGSIM_MATH_POLYNOMIALVALUES_H
+
+#include "SurgSim/Math/Polynomial.h"
+#include "SurgSim/Math/PolynomialRoots.h"
+#include "SurgSim/Math/IntervalArithmetic.h"
+#include "SurgSim/Math/MinMax.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// Class to manage polynomial based calculations of interval boundaries.
+///
+/// \tparam T type of the coefficients and computations
+/// \tparam N degree of the polynomial being managed
+template <typename T, int N> class PolynomialValues;
+
+/// PolynomialValues<T, 0> specializes the PolynomialValues class for degree 0 (constant polynomials)
+/// \sa PolynomialValues<T, N>
+template <class T>
+class PolynomialValues<T, 0>
+{
+public:
+ /// Constructor. Initialize based on the polynomial p
+ /// \param p polynomial on which the value calculations are based
+ explicit PolynomialValues(const Polynomial<T, 0>& p);
+
+ /// \return the polynomial basis of this calculation
+ const Polynomial<T, 0>& getPolynomial() const;
+
+ /// \param interval an interval on the independent variable over which the
+ /// values are to be calculated
+ /// \return the minimum and maximum polynomial values over interval
+ Interval<T> valuesOverInterval(const Interval<T>& interval) const;
+
+private:
+ /// The polynomial under consideration
+ Polynomial<T, 0> m_polynomial;
+};
+
+/// PolynomialValues<T, 1> specializes the PolynomialValues class for degree 1 (linear polynomials)
+/// \sa PolynomialValues<T, N>
+template <class T>
+class PolynomialValues<T, 1>
+{
+public:
+ /// Constructor. Initialize based on the polynomial p
+ /// \param p polynomial on which the value calculations are based
+ explicit PolynomialValues(const Polynomial<T, 1>& p);
+
+ /// \return the polynomial basis of this calculation
+ const Polynomial<T, 1>& getPolynomial() const;
+
+ /// \param interval an interval on the independent variable over which the
+ /// values are to be calculated
+ /// \return the minimum and maximum polynomial values over interval
+ Interval<T> valuesOverInterval(const Interval<T>& interval) const;
+
+private:
+ /// The polynomial under consideration
+ Polynomial<T, 1> m_polynomial;
+};
+
+/// PolynomialValues<T, 2> specializes the PolynomialValues class for degree 2 (quadratic polynomials)
+/// \sa PolynomialValues<T, N>
+template <class T>
+class PolynomialValues<T, 2>
+{
+public:
+ /// Constructor. Initialize based on the polynomial p
+ /// \param p polynomial on which the value calculations are based
+ explicit PolynomialValues(const Polynomial<T, 2>& p);
+
+ /// \return the polynomial basis of this calculation
+ const Polynomial<T, 2>& getPolynomial() const;
+
+ /// \return the derivative of the polynomial basis for this calculation
+ const Polynomial<T, 1>& getDerivative() const;
+
+ /// \return the locations of the extrema for the polynomial
+ const PolynomialRoots<T, 1>& getLocationsOfExtrema() const;
+
+ /// \param interval an interval on the independent variable over which the
+ /// values are to be calculated
+ /// \return the minimum and maximum polynomial values over interval
+ Interval<T> valuesOverInterval(const Interval<T>& interval) const;
+
+private:
+ /// The polynomial under consideration
+ Polynomial<T, 2> m_polynomial;
+
+ /// Cached version of the derivative of the polynomial
+ Polynomial<T, 1> m_derivative;
+
+ /// Cached version of the locations of the extrema
+ PolynomialRoots<T, 1> m_locationOfExtremum;
+};
+
+/// PolynomialValues<T, 3> specializes the PolynomialValues class for degree 3 (cubic polynomials)
+/// \sa PolynomialValues<T, N>
+template <class T>
+class PolynomialValues<T, 3>
+{
+public:
+ /// Constructor. Initialize based on the polynomial p
+ /// \param p polynomial on which the value calculations are based
+ explicit PolynomialValues(const Polynomial<T, 3>& p);
+
+ /// \return the polynomial basis of this calculation
+ const Polynomial<T, 3>& getPolynomial() const;
+
+ /// \return the derivative of the polynomial basis for this calculation
+ const Polynomial<T, 2>& getDerivative() const;
+
+ /// \return the locations of the extrema for the polynomial
+ const PolynomialRoots<T, 2>& getLocationsOfExtrema() const;
+
+ /// \param interval an interval on the independent variable over which the
+ /// values are to be calculated
+ /// \return the minimum and maximum polynomial values over interval
+ Interval<T> valuesOverInterval(const Interval<T>& interval) const;
+
+private:
+ /// The polynomial under consideration
+ Polynomial<T, 3> m_polynomial;
+
+ /// Cached version of the derivative of the polynomial
+ Polynomial<T, 2> m_derivative;
+
+ /// Cached version of the locations of the extrema
+ PolynomialRoots<T, 2> m_locationOfExtremum;
+};
+
+/// Calculate the minimum and maximum values of the dependent variable over a specified
+/// range of the independent variable
+/// \tparam N degree of the polynomial being managed
+/// \tparam T type of the coefficients and computations
+/// \param p polynomial on which the value calculations are based
+/// \param interval an interval on the independent variable over which the
+/// values are to be calculated
+/// \return the minimum and maximum polynomial values over interval
+template <class T, int N>
+Interval<T> valuesOverInterval(const Polynomial<T, N>& p, const Interval<T>& interval);
+
+}; // Math
+}; // SurgSim
+
+#include "SurgSim/Math/PolynomialValues-inl.h"
+
+#endif // SURGSIM_MATH_POLYNOMIALVALUES_H
diff --git a/SurgSim/Math/Scalar-inl.h b/SurgSim/Math/Scalar-inl.h
new file mode 100644
index 0000000..0f93f4b
--- /dev/null
+++ b/SurgSim/Math/Scalar-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_SCALAR_INL_H
+#define SURGSIM_MATH_SCALAR_INL_H
+
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+template <class T>
+T clamp(T value, T min, T max, T epsilon)
+{
+ return (value >= (max - epsilon) ? max : (value <= (min + epsilon) ? min : value));
+}
+
+template <typename T>
+const T clampOperator<T>::operator()(const T& x) const
+{
+ return clamp(x, m_min, m_max, m_epsilon);
+}
+
+};
+};
+
+#endif // SURGSIM_MATH_SCALAR_INL_H
+
diff --git a/SurgSim/Math/Scalar.h b/SurgSim/Math/Scalar.h
new file mode 100644
index 0000000..707d91a
--- /dev/null
+++ b/SurgSim/Math/Scalar.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_SCALAR_H
+#define SURGSIM_MATH_SCALAR_H
+
+namespace SurgSim
+{
+namespace Math
+{
+/// Clamp any values within an epsilon window to a maximum or minimum value.
+/// \note max is tested first, so if the value < min + epsilon and value >
+/// max - epsilon, value will be set to max.
+/// \tparam T underlying type
+/// \param value [in/out] the value to be clamped
+/// \param min the minimum value for the clamp
+/// \param max the maximum value for the clamp
+/// \param epsilon definition of the epsilon window.
+template <class T>
+T clamp(T value, T min, T max, T epsilon);
+
+/// define a custom template unary functor to execute the clamp operation
+/// over an Eigen matrix structure. The operation clamps based on an epsilon
+/// interval. For values outside the interval min to max, the clamp proceeds
+/// as expected. However, values within the interval that lie within epsilon
+/// of an endpoint are also clamped to th endpoint.
+///
+/// \tparam T the type over which the operator is defined.
+template<typename T>
+class clampOperator
+{
+public:
+ /// Constructor.
+ ///
+ /// \param min minimum value for the clamp interval
+ /// \param max maximum value for the clamp interval
+ /// \param epsilon values within epsilon of an interval endpoint
+ /// are clamped to the interval regardless of if they are within the interval or not
+ clampOperator(const T& min, const T& max, const T& epsilon) : m_min(min), m_max(max),
+ m_epsilon(epsilon) {}
+
+ /// Execute the clamp operator
+ /// \param x the value to clamp based
+ /// \return the x clamped using the minimum, maximum and epsilon specified in he class constructor
+ const T operator()(const T& x) const;
+
+private:
+ /// The minimum value of the interval
+ T m_min;
+
+ /// The maximum value of the interval
+ T m_max;
+
+ /// The closeness parameter for the clamp. Values within epsilon of an interval endpoint
+ /// are clamped to the interval regardless of if they are within the interval or not
+ T m_epsilon;
+};
+
+};
+};
+
+#include "SurgSim/Math/Scalar-inl.h"
+
+#endif // SURGSIM_MATH_SCALAR_H
diff --git a/SurgSim/Math/SegmentMeshShape-inl.h b/SurgSim/Math/SegmentMeshShape-inl.h
new file mode 100644
index 0000000..239c16e
--- /dev/null
+++ b/SurgSim/Math/SegmentMeshShape-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_MATH_SEGMENTMESHSHAPE_INL_H
+#define SURGSIM_MATH_SEGMENTMESHSHAPE_INL_H
+
+namespace SurgSim
+{
+namespace Math
+{
+
+template <class VertexData, class EdgeData>
+SegmentMeshShape::SegmentMeshShape(
+ const SurgSim::DataStructures::SegmentMesh<VertexData, EdgeData>& mesh,
+ double radius) : SurgSim::DataStructures::SegmentMeshPlain(mesh)
+{
+ setRadius(radius);
+ updateAabbTree();
+}
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_SEGMENTMESHSHAPE_INL_H
diff --git a/SurgSim/Math/SegmentMeshShape.cpp b/SurgSim/Math/SegmentMeshShape.cpp
new file mode 100644
index 0000000..38c043a
--- /dev/null
+++ b/SurgSim/Math/SegmentMeshShape.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/Math/SegmentMeshShape.h"
+
+#include "SurgSim/DataStructures/AabbTree.h"
+#include "SurgSim/DataStructures/AabbTreeData.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/SegmentMeshShapePlyReaderDelegate.h"
+
+
+namespace SurgSim
+{
+namespace Math
+{
+SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::SegmentMeshShape, SegmentMeshShape);
+
+SegmentMeshShape::SegmentMeshShape()
+{
+ setRadius(0.001);
+ updateAabbTree();
+}
+
+SegmentMeshShape::SegmentMeshShape(const SegmentMeshShape& other) :
+ DataStructures::SegmentMeshPlain(other)
+{
+ setRadius(other.m_radius);
+ updateAabbTree();
+}
+
+int SegmentMeshShape::getType() const
+{
+ return SHAPE_TYPE_SEGMENTMESH;
+}
+
+double SegmentMeshShape::getVolume() const
+{
+ SURGSIM_ASSERT("SegmentMeshShape: Volume calculation not implemented.");
+ return std::numeric_limits<double>::quiet_NaN();
+}
+
+Vector3d SegmentMeshShape::getCenter() const
+{
+ SURGSIM_ASSERT("SegmentMeshShape: Center calculation not implemented.");
+ return Vector3d::Constant(std::numeric_limits<double>::quiet_NaN());
+}
+
+Matrix33d SegmentMeshShape::getSecondMomentOfVolume() const
+{
+ SURGSIM_ASSERT("SegmentMeshShape: Second Moment Of Volume calculation not implemented.");
+ return Matrix33d::Constant(std::numeric_limits<double>::quiet_NaN());
+}
+
+bool SegmentMeshShape::isValid() const
+{
+ // If the radius is less than DistanceEpsilon, the collision detection (which assumes the segments are capsules),
+ /// will not work correctly.
+ return (m_radius >= Geometry::DistanceEpsilon &&
+ m_segmentEndBoundingBoxHalfExtent.isApprox(Vector3d(m_radius, m_radius, m_radius)));
+}
+
+void SegmentMeshShape::setRadius(double radius)
+{
+ m_radius = radius;
+ m_segmentEndBoundingBoxHalfExtent = Vector3d(m_radius, m_radius, m_radius);
+}
+
+double SegmentMeshShape::getRadius() const
+{
+ return m_radius;
+}
+
+bool SegmentMeshShape::doUpdate()
+{
+ updateAabbTree();
+ return true;
+}
+
+bool SegmentMeshShape::doLoad(const std::string& fileName)
+{
+ DataStructures::PlyReader reader(fileName);
+ if (!reader.isValid())
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger())
+ << "'" << fileName << "' is an invalid .ply file.";
+ return false;
+ }
+
+ auto delegate = std::make_shared<SegmentMeshShapePlyReaderDelegate>(
+ std::dynamic_pointer_cast<SegmentMeshShape>(this->shared_from_this()));
+ if (!reader.parseWithDelegate(delegate))
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger())
+ << "The input file '" << fileName << "' does not have the property required by segment mesh.";
+ return false;
+ }
+
+ return true;
+}
+
+std::shared_ptr<const DataStructures::AabbTree> SegmentMeshShape::getAabbTree() const
+{
+ return m_aabbTree;
+}
+
+std::shared_ptr<Shape> SegmentMeshShape::getTransformed(const RigidTransform3d& pose) const
+{
+ auto transformed = std::make_shared<SegmentMeshShape>(*this);
+ transformed->transform(pose);
+ transformed->update();
+ return transformed;
+}
+
+void SegmentMeshShape::updateAabbTree()
+{
+ m_aabbTree = std::make_shared<DataStructures::AabbTree>();
+
+ std::list<DataStructures::AabbTreeData::Item> items;
+
+ auto const& edges = getEdges();
+
+ for (size_t id = 0; id < edges.size(); ++id)
+ {
+ if (edges[id].isValid)
+ {
+ const auto& vertices = getEdgePositions(id);
+ Aabbd aabb((vertices[0] - m_segmentEndBoundingBoxHalfExtent).eval());
+ aabb.extend((vertices[0] + m_segmentEndBoundingBoxHalfExtent).eval());
+ aabb.extend((vertices[1] - m_segmentEndBoundingBoxHalfExtent).eval());
+ aabb.extend((vertices[1] + m_segmentEndBoundingBoxHalfExtent).eval());
+ items.emplace_back(aabb, id);
+ }
+ }
+ m_aabbTree->set(std::move(items));
+ m_aabb = m_aabbTree->getAabb();
+}
+
+bool SegmentMeshShape::isTransformable() const
+{
+ return true;
+}
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/SegmentMeshShape.h b/SurgSim/Math/SegmentMeshShape.h
new file mode 100644
index 0000000..a21e37f
--- /dev/null
+++ b/SurgSim/Math/SegmentMeshShape.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_SEGMENTMESHSHAPE_H
+#define SURGSIM_MATH_SEGMENTMESHSHAPE_H
+
+#include "SurgSim/DataStructures/AabbTree.h"
+#include "SurgSim/DataStructures/SegmentMesh.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+SURGSIM_STATIC_REGISTRATION(SegmentMeshShape);
+
+/// SegmentMeshShape defines a shape based on a mesh, like MeshShape.
+/// But, unlike MeshShape, the mesh does not have any triangle topology. It only consists of edges.
+///
+/// \sa MeshShape
+class SegmentMeshShape : public Shape, public DataStructures::SegmentMeshPlain
+{
+public:
+ /// Constructor
+ SegmentMeshShape();
+
+ /// Copy constructor
+ /// \param other The SegmentMeshShape to be copied from
+ explicit SegmentMeshShape(const SegmentMeshShape& other);
+
+ /// Constructor
+ /// \param mesh The segment mesh to build the shape from
+ /// \param radius The radius associated to this surface mesh
+ /// \note The default radius is positive EPSILON to be relevant in collision detection calculations.
+ template <class VertexData, class EdgeData>
+ explicit SegmentMeshShape(const DataStructures::SegmentMesh<VertexData, EdgeData>& mesh,
+ double radius = Geometry::DistanceEpsilon);
+
+ SURGSIM_CLASSNAME(SurgSim::Math::SegmentMeshShape);
+
+ int getType() const override;
+ double getVolume() const override;
+ Vector3d getCenter() const override;
+ Matrix33d getSecondMomentOfVolume() const override;
+ bool isValid() const override;
+
+ /// \param radius The radius of the segments.
+ void setRadius(double radius);
+
+ /// \return The radius of the segments.
+ double getRadius() const;
+
+ /// \return The object's associated AabbTree
+ std::shared_ptr<const DataStructures::AabbTree> getAabbTree() const;
+
+ bool isTransformable() const override;
+
+ std::shared_ptr<Shape> getTransformed(const RigidTransform3d& pose) const override;
+
+protected:
+ bool doUpdate() override;
+ bool doLoad(const std::string& fileName) override;
+
+ /// Update the AabbTree, which is an axis-aligned bounding box r-tree used to accelerate spatial searches
+ void updateAabbTree();
+
+private:
+ /// Segment radius
+ double m_radius;
+
+ /// The aabb tree used to accelerate collision detection against the mesh
+ std::shared_ptr<DataStructures::AabbTree> m_aabbTree;
+
+ /// Half extent of the AABB of the sphere at one of the segment end.
+ Vector3d m_segmentEndBoundingBoxHalfExtent;
+};
+
+} // Math
+} // SurgSim
+
+#include "SurgSim/Math/SegmentMeshShape-inl.h"
+
+#endif // SURGSIM_MATH_SEGMENTMESHSHAPE_H
diff --git a/SurgSim/Math/SegmentMeshShapePlyReaderDelegate.cpp b/SurgSim/Math/SegmentMeshShapePlyReaderDelegate.cpp
new file mode 100644
index 0000000..bbce69b
--- /dev/null
+++ b/SurgSim/Math/SegmentMeshShapePlyReaderDelegate.cpp
@@ -0,0 +1,84 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/SegmentMeshShapePlyReaderDelegate.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+SegmentMeshShapePlyReaderDelegate::SegmentMeshShapePlyReaderDelegate() :
+ m_radius(0.0)
+{
+
+}
+
+SegmentMeshShapePlyReaderDelegate::SegmentMeshShapePlyReaderDelegate(
+ const std::shared_ptr<Math::SegmentMeshShape>& shape) :
+ TriangleMeshPlyReaderDelegate<Math::SegmentMeshShape>(shape),
+ m_hasRadius(false),
+ m_radius(0.0),
+ m_shape(shape)
+{
+ SURGSIM_ASSERT(m_shape != nullptr) << "Shape can't be nullptr.";
+}
+
+SegmentMeshShapePlyReaderDelegate::~SegmentMeshShapePlyReaderDelegate()
+{
+
+}
+
+void* SegmentMeshShapePlyReaderDelegate::beginRadius(const std::string&, size_t value)
+{
+ SURGSIM_ASSERT(value <= 1) << "Can't process more than 1 radius";
+ return &m_radius;
+}
+
+void SegmentMeshShapePlyReaderDelegate::processRadius(const std::string&)
+{
+ m_shape->setRadius(m_radius);
+}
+
+bool SegmentMeshShapePlyReaderDelegate::registerDelegate(DataStructures::PlyReader* reader)
+{
+ bool result = TriangleMeshPlyReaderDelegate<Math::SegmentMeshShape>::registerDelegate(reader);
+
+ if (m_hasRadius)
+ {
+ reader->requestElement("radius",
+ std::bind(&SegmentMeshShapePlyReaderDelegate::beginRadius, this,
+ std::placeholders::_1, std::placeholders::_2),
+ std::bind(&SegmentMeshShapePlyReaderDelegate::processRadius,
+ this, std::placeholders::_1),
+ nullptr);
+ reader->requestScalarProperty("radius", "value", DataStructures::PlyReader::TYPE_DOUBLE, 0);
+ }
+
+ return result;
+}
+
+bool SegmentMeshShapePlyReaderDelegate::fileIsAcceptable(const DataStructures::PlyReader& reader)
+{
+ bool result = TriangleMeshPlyReaderDelegate<Math::SegmentMeshShape>::fileIsAcceptable(reader);
+
+ m_hasRadius = reader.hasProperty("radius", "value") && reader.isScalar("radius", "value");
+
+ return result;
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Math/SegmentMeshShapePlyReaderDelegate.h b/SurgSim/Math/SegmentMeshShapePlyReaderDelegate.h
new file mode 100644
index 0000000..227fdb9
--- /dev/null
+++ b/SurgSim/Math/SegmentMeshShapePlyReaderDelegate.h
@@ -0,0 +1,65 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_SEGMENTMESHSHAPEPLYREADERDELEGATE_H
+#define SURGSIM_MATH_SEGMENTMESHSHAPEPLYREADERDELEGATE_H
+
+#include "SurgSim/DataStructures/TriangleMeshPlyReaderDelegate.h"
+#include "SurgSim/Math/SegmentMeshShape.h"
+
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+/// Implementation of ply reader for segment meshes, enable to read the radius from the ply file
+class SegmentMeshShapePlyReaderDelegate : public DataStructures::TriangleMeshPlyReaderDelegate<SegmentMeshShape>
+{
+public:
+ /// Constructor
+ SegmentMeshShapePlyReaderDelegate();
+
+ explicit SegmentMeshShapePlyReaderDelegate(const std::shared_ptr<Math::SegmentMeshShape>& shape);
+
+ /// Destructor
+ ~SegmentMeshShapePlyReaderDelegate();
+
+ /// Delegate function to begin radius processing
+ /// \param element the name of the element being processed 'radius' in this case, it is ignored
+ /// \param value the number of radius entries should be == 1
+ /// \return the address for ply to deposit the radius data
+ void* beginRadius(const std::string& element, size_t value);
+
+ /// Callback function to process the radius
+ /// \param element the name of element being processed 'radius' in this case, it is ignored
+ void processRadius(const std::string& element);
+
+ bool registerDelegate(DataStructures::PlyReader* reader) override;
+
+ bool fileIsAcceptable(const DataStructures::PlyReader& reader) override;
+
+private:
+ bool m_hasRadius;
+ double m_radius;
+
+ std::shared_ptr<Math::SegmentMeshShape> m_shape;
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Math/SegmentSegmentCcdContactCalculation-inl.h b/SurgSim/Math/SegmentSegmentCcdContactCalculation-inl.h
new file mode 100644
index 0000000..5beab25
--- /dev/null
+++ b/SurgSim/Math/SegmentSegmentCcdContactCalculation-inl.h
@@ -0,0 +1,143 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_SEGMENTSEGMENTCCDCONTACTCALCULATION_INL_H
+#define SURGSIM_MATH_SEGMENTSEGMENTCCDCONTACTCALCULATION_INL_H
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "SurgSim/Math/CubicSolver.h"
+#include "SurgSim/Math/Scalar.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// Check if 2 segments intersect at a given time of their motion
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param time The time of coplanarity of the 4 points (A(t), B(t), C(t), D(t) are expected to be coplanar)
+/// \param A, B The 1st segment motion (each point's motion is from first to second)
+/// \param C, D The 2nd segment motion (each point's motion is from first to second)
+/// \param[out] barycentricCoordinates The barycentric coordinates of the intersection in AB(t) and CD(t)
+/// i.e. P(t) = A + barycentricCoordinates[0].AB(t) = C + barycentricCoordinates[1].CD
+/// \return True if AB(t) is intersecting CD(t), False otherwise
+template <class T, int MOpt>
+bool areSegmentsIntersecting(
+ T time,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& A,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& B,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& C,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& D,
+ Eigen::Matrix<T, 2, 1, MOpt>* barycentricCoordinates)
+{
+ Eigen::Matrix<T, 3, 1, MOpt> At = interpolate(A.first, A.second, time);
+ Eigen::Matrix<T, 3, 1, MOpt> Bt = interpolate(B.first, B.second, time);
+ Eigen::Matrix<T, 3, 1, MOpt> Ct = interpolate(C.first, C.second, time);
+ Eigen::Matrix<T, 3, 1, MOpt> Dt = interpolate(D.first, D.second, time);
+
+ // P = A + alpha.AB and P = C + beta.CD
+ // A + alpha.AB = C + beta.CD
+ // (AB -CD).(alpha) = (AC) which is a 3x2 linear system A.x = b
+ // (beta )
+ // Let's solve it using (A^t.A)^-1.(A^t.A) = Id2x2
+ // (A^t.A)^-1.A^t.(A.x) = (A^t.A)^-1.A^t.b
+ // x = (A^t.A)^-1.A^t.b
+
+ Eigen::Matrix<T, 3, 2> matrixA;
+ matrixA.col(0) = Bt - At;
+ matrixA.col(1) = -(Dt - Ct);
+
+ Eigen::Matrix<T, 3, 1, MOpt> b = Ct - At;
+ Eigen::Matrix<T, 2, 2> inv;
+ bool invertible;
+ (matrixA.transpose() * matrixA).computeInverseWithCheck(inv, invertible);
+ if (!invertible)
+ {
+ return false;
+ }
+
+ *barycentricCoordinates = inv * matrixA.transpose() * b;
+
+ for (int i = 0; i < 2; i++)
+ {
+ if (std::abs((*barycentricCoordinates)[i]) < Math::Geometry::ScalarEpsilon)
+ {
+ (*barycentricCoordinates)[i] = 0.0;
+ }
+ if (std::abs(1.0 - (*barycentricCoordinates)[i]) < Math::Geometry::ScalarEpsilon)
+ {
+ (*barycentricCoordinates)[i] = 1.0;
+ }
+ }
+
+ return
+ (*barycentricCoordinates)[0] >= 0.0 && (*barycentricCoordinates)[0] <= 1.0 &&
+ (*barycentricCoordinates)[1] >= 0.0 && (*barycentricCoordinates)[1] <= 1.0;
+}
+
+/// Continuous collision detection between two segments AB and CD
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param A, B The 1st segment motion (each point's motion is from first to second)
+/// \param C, D The 2nd segment motion (each point's motion is from first to second)
+/// \param[out] timeOfImpact The time of impact within [0..1] if an collision is found
+/// \param[out] s0p1Factor, s1p1Factor The barycentric coordinate of the contact point on the 2 segments
+/// i.e. ContactPoint(timeOfImpact) = A(timeOfImpact) + s0p1Factor.AB(timeOfImpact)
+/// i.e. ContactPoint(timeOfImpact) = C(timeOfImpact) + s1p1Factor.CD(timeOfImpact)
+/// \return True if the given segment/segment motions intersect, False otherwise
+/// \note Simple cubic-solver https://graphics.stanford.edu/courses/cs468-02-winter/Papers/Collisions_vetements.pdf
+/// \note Optimized method http://www.robotics.sei.ecnu.edu.cn/CCDLY/GMOD15.pdf
+/// \note Optimized method https://www.cs.ubc.ca/~rbridson/docs/brochu-siggraph2012-ccd.pdf
+template <class T, int MOpt> inline
+bool calculateCcdContactSegmentSegment(
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& A,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& B,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& C,
+ const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& D,
+ T* timeOfImpact, T* s0p1Factor, T* s1p1Factor)
+{
+ std::array<T, 3> roots;
+ int numberOfRoots = timesOfCoplanarityInRange01(A, B, C, D, &roots);
+
+ // The roots are all in [0..1] and ordered ascendingly
+ for (int rootId = 0; rootId < numberOfRoots; ++rootId)
+ {
+ Eigen::Matrix<T, 2, 1, MOpt> barycentricCoordinates;
+ if (areSegmentsIntersecting(roots[rootId], A, B, C, D, &barycentricCoordinates))
+ {
+ // The segments AB and CD are coplanar at time t, and they intersect
+ *timeOfImpact = roots[rootId];
+ *s0p1Factor = barycentricCoordinates[0];
+ *s1p1Factor = barycentricCoordinates[1];
+
+ // None of these assertion should be necessary, but just to double check
+ SURGSIM_ASSERT(*timeOfImpact >= 0.0 && *timeOfImpact <= 1.0);
+ SURGSIM_ASSERT(*s0p1Factor >= 0.0 && *s0p1Factor <= 1.0);
+ SURGSIM_ASSERT(*s1p1Factor >= 0.0 && *s1p1Factor <= 1.0);
+
+ return true;
+ }
+ }
+
+ return false;
+}
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_SEGMENTSEGMENTCCDCONTACTCALCULATION_INL_H
diff --git a/SurgSim/Math/Shape.cpp b/SurgSim/Math/Shape.cpp
index f1b5ddd..8b3c86c 100644
--- a/SurgSim/Math/Shape.cpp
+++ b/SurgSim/Math/Shape.cpp
@@ -23,24 +23,39 @@ namespace SurgSim
namespace Math
{
+Shape::Shape()
+{
+ m_aabb.setEmpty();
+}
+
Shape::~Shape()
{
}
-Shape::FactoryType& Shape::getFactory()
+bool Shape::isTransformable() const
{
- static FactoryType factory;
- return factory;
+ return false;
+}
+
+std::shared_ptr<Shape> Shape::getTransformed(const RigidTransform3d& pose) const
+{
+ SURGSIM_FAILURE() << "getTransformed not implemented for " << getClassName();
+ return nullptr;
}
/// 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.";
+ "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";
}
+const Math::Aabbd& Shape::getBoundingBox() const
+{
+ return m_aabb;
+}
+
} // namespace Math
-} // namespace SurgSim
\ No newline at end of file
+} // namespace SurgSim
diff --git a/SurgSim/Math/Shape.h b/SurgSim/Math/Shape.h
index 36c94d9..42413a4 100644
--- a/SurgSim/Math/Shape.h
+++ b/SurgSim/Math/Shape.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -18,8 +18,11 @@
#include "SurgSim/Framework/Accessible.h"
#include "SurgSim/Framework/ObjectFactory.h"
-#include "SurgSim/Math/Vector.h"
#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/Aabb.h"
+#include "SurgSim/DataStructures/OptionalValue.h"
namespace SurgSim
{
@@ -47,26 +50,32 @@ typedef enum
SHAPE_TYPE_DOUBLESIDEDPLANE,
SHAPE_TYPE_MESH,
SHAPE_TYPE_OCTREE,
+ SHAPE_TYPE_PARTICLES,
SHAPE_TYPE_PLANE,
SHAPE_TYPE_SPHERE,
SHAPE_TYPE_SURFACEMESH,
+ SHAPE_TYPE_SEGMENTMESH,
+ SHAPE_TYPE_COMPOUNDSHAPE,
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
+class Shape : virtual public SurgSim::Framework::Accessible, public Framework::FactoryBase<Shape>
{
public:
typedef ::SurgSim::Math::Vector3d Vector3d;
typedef ::SurgSim::Math::Matrix33d Matrix33d;
- // Destructor
+ /// Constructor
+ Shape();
+
+ /// Destructor
virtual ~Shape();
/// \return the type of shape
- virtual int getType() = 0;
+ virtual int getType() const = 0;
/// Get the volume of the shape
/// \return The volume of the shape (in m-3)
@@ -81,10 +90,13 @@ public:
/// \return The 3x3 symmetric second moment matrix
virtual Matrix33d getSecondMomentOfVolume() const = 0;
- typedef SurgSim::Framework::ObjectFactory<SurgSim::Math::Shape> FactoryType;
+ /// \return true if the the shape can be transformed
+ virtual bool isTransformable() const;
- /// \return The static class factory that is being used in the conversion.
- static FactoryType& getFactory();
+ /// Get a copy of this shape with an applied rigid transform
+ /// \param pose The pose to transform the shape by
+ /// \return the posed shape
+ virtual std::shared_ptr<Shape> getTransformed(const RigidTransform3d& pose) const;
/// Get class name
virtual std::string getClassName() const;
@@ -92,6 +104,58 @@ public:
/// Check if the shape is valid
/// \return True if shape is valid; Otherwise, false.
virtual bool isValid() const = 0;
+
+ /// \return the bounding box for the shape
+ virtual const Math::Aabbd& getBoundingBox() const;
+
+protected:
+ Math::Aabbd m_aabb;
+};
+
+/// PosedShape is a transformed shape with a record of the pose used to transform it.
+template <class T>
+struct PosedShape
+{
+ PosedShape()
+ {
+ pose = Math::RigidTransform3d::Identity();
+ }
+ PosedShape(const T& shapeInput, const Math::RigidTransform3d& poseInput) : shape(shapeInput), pose(poseInput) {}
+
+ void invalidate()
+ {
+ shape = nullptr;
+ }
+ const T& getShape() const
+ {
+ return shape;
+ }
+ const Math::RigidTransform3d& getPose() const
+ {
+ return pose;
+ }
+
+protected:
+ T shape;
+ Math::RigidTransform3d pose;
+};
+
+/// PosedShapeMotion is embedding the motion of a PosedShape, providing a posed shape at 2 different instant.
+template <class T>
+struct PosedShapeMotion : public std::pair<PosedShape<T>, PosedShape<T>>
+{
+ PosedShapeMotion() {}
+ PosedShapeMotion(const PosedShape<T>& posedShapeFirst, const PosedShape<T>& posedShapeSecond)
+ {
+ this->first = posedShapeFirst;
+ this->second = posedShapeSecond;
+ }
+
+ void invalidate()
+ {
+ this->first.invalidate();
+ this->second.invalidate();
+ }
};
}; // Math
diff --git a/SurgSim/Math/Shapes.h b/SurgSim/Math/Shapes.h
index 1835586..800afaf 100644
--- a/SurgSim/Math/Shapes.h
+++ b/SurgSim/Math/Shapes.h
@@ -19,11 +19,13 @@
/// This file includes all the shapes
#include "SurgSim/Math/BoxShape.h"
#include "SurgSim/Math/CapsuleShape.h"
+#include "SurgSim/Math/CompoundShape.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/ParticlesShape.h"
+#include "SurgSim/Math/PlaneShape.h"
#include "SurgSim/Math/SphereShape.h"
#include "SurgSim/Math/SurfaceMeshShape.h"
diff --git a/SurgSim/Math/SparseMatrix.h b/SurgSim/Math/SparseMatrix.h
new file mode 100644
index 0000000..5a5d21c
--- /dev/null
+++ b/SurgSim/Math/SparseMatrix.h
@@ -0,0 +1,395 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file SparseMatrix.h
+/// Definitions of useful sparse matrix functions
+
+#ifndef SURGSIM_MATH_SPARSEMATRIX_H
+#define SURGSIM_MATH_SPARSEMATRIX_H
+
+#include <Eigen/Sparse>
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Math/Matrix.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+/// A sparse matrix
+typedef Eigen::SparseMatrix<double> SparseMatrix;
+
+/// Helper class to run operation a column/row of a matrix to a chunk of memory where the size is dynamically defined
+/// \tparam DerivedSub The class of the matrix from which the data are copied
+/// \tparam SparseType The type of the SparseVector/SparseMatrix containing the chunk of memory
+/// \tparam StorageOrder The storage option of the SparseType
+template < class DerivedSub, class SparseType,
+ int StorageOrder =
+ ((SparseType::Flags & Eigen::RowMajorBit) == Eigen::RowMajorBit) ? Eigen::RowMajor : Eigen::ColMajor >
+class Operation
+{
+public :
+ typedef typename SparseType::Scalar T;
+ typedef typename SparseType::Index Index;
+
+ /// Do the assignment of a row/column of a matrix to a chunk of memory
+ /// \param ptr The chunk of memory
+ /// \param start Where the assignment starts in the chunk of memory
+ /// \param n, m The size of the block (n rows, m columns)
+ /// \param subMatrix The matrix from which the row/column is copied
+ /// \param colRowId The column or row id depending on the template parameter Opt
+ void assign(T* ptr, size_t start, size_t n, Index m, const DerivedSub& subMatrix, size_t colRowId) {}
+
+ /// Do the assignment of a single matrix element (operator =)
+ /// \param ptr The matrix element to be assigned
+ /// \param value The value to assign to
+ void assign(T* ptr, const T& value) {}
+
+ /// Do the addition of a row/column of a matrix to a chunk of memory
+ /// \param ptr The chunk of memory
+ /// \param start Where the addition starts in the chunk of memory
+ /// \param n, m The size of the block (n rows, m columns)
+ /// \param subMatrix The matrix from which the row/column is added
+ /// \param colRowId The column or row id depending on the template parameter Opt
+ void add(T* ptr, size_t start, size_t n, size_t m, const DerivedSub& subMatrix, size_t colRowId) {}
+
+ /// Do the addition of a single matrix element (operator +=)
+ /// \param ptr The matrix element to be increased
+ /// \param value The value to add
+ void add(T* ptr, const T& value) {}
+};
+
+/// Specialization for column major storage
+template <class DerivedSub, class SparseType>
+class Operation<DerivedSub, SparseType, Eigen::ColMajor>
+{
+public:
+ typedef typename SparseType::Scalar T;
+ typedef typename SparseType::Index Index;
+
+ void assign(T* ptr, size_t start, size_t n, size_t m, const DerivedSub& subMatrix, size_t colId)
+ {
+ typedef Eigen::Matrix < T, Eigen::Dynamic, 1, Eigen::DontAlign | Eigen::ColMajor > ColVector;
+ typedef typename Eigen::Matrix < T, Eigen::Dynamic, 1, Eigen::DontAlign | Eigen::ColMajor >::Index IndexVector;
+ typedef typename DerivedSub::Index IndexSub;
+
+ // ptr[start] is the 1st element in the column
+ // The elements exists and are contiguous in memory, we use Eigen::Map functionality to optimize the operation
+ Eigen::Map<ColVector>(&ptr[start], static_cast<IndexVector>(n)).operator = (
+ subMatrix.col(static_cast<IndexSub>(colId)).segment(0, static_cast<IndexSub>(n)));
+ }
+
+ void assign(T* ptr, const T& value)
+ {
+ *ptr = value;
+ }
+
+ void add(T* ptr, size_t start, size_t n, size_t m, const DerivedSub& subMatrix, size_t colId)
+ {
+ typedef Eigen::Matrix < T, Eigen::Dynamic, 1, Eigen::DontAlign | Eigen::ColMajor > ColVector;
+ typedef typename Eigen::Matrix < T, Eigen::Dynamic, 1, Eigen::DontAlign | Eigen::ColMajor >::Index IndexVector;
+ typedef typename DerivedSub::Index IndexSub;
+
+ // ptr[start] is the 1st element in the column
+ // The elements exists and are contiguous in memory, we use Eigen::Map functionality to optimize the operation
+ Eigen::Map<ColVector>(&ptr[start], static_cast<IndexVector>(n)).operator += (
+ subMatrix.col(static_cast<IndexSub>(colId)).segment(0, static_cast<IndexSub>(n)));
+ }
+
+ void add(T* ptr, const T& value)
+ {
+ *ptr += value;
+ }
+};
+
+/// Specialization for row major storage
+template <class DerivedSub, class SparseType>
+class Operation<DerivedSub, SparseType, Eigen::RowMajor>
+{
+public:
+ typedef typename SparseType::Scalar T;
+ typedef typename SparseType::Index Index;
+
+ void assign(T* ptr, size_t start, size_t n, size_t m, const DerivedSub& subMatrix, size_t rowId)
+ {
+ typedef Eigen::Matrix < T, 1, Eigen::Dynamic, Eigen::DontAlign | Eigen::RowMajor > RowVector;
+ typedef typename Eigen::Matrix < T, 1, Eigen::Dynamic, Eigen::DontAlign | Eigen::RowMajor >::Index IndexVector;
+ typedef typename DerivedSub::Index IndexSub;
+
+ // ptr[start] is the 1st element in the row
+ // The elements exists and are contiguous in memory, we use Eigen::Map functionality to optimize the operation
+ Eigen::Map<RowVector>(&ptr[start], static_cast<IndexVector>(m)).operator = (
+ subMatrix.row(static_cast<IndexSub>(rowId)).segment(0, static_cast<IndexSub>(m)));
+ }
+
+ void assign(T* ptr, const T& value)
+ {
+ *ptr = value;
+ }
+
+ void add(T* ptr, size_t start, size_t n, size_t m, const DerivedSub& subMatrix, size_t rowId)
+ {
+ typedef Eigen::Matrix < T, 1, Eigen::Dynamic, Eigen::DontAlign | Eigen::RowMajor > RowVector;
+ typedef typename Eigen::Matrix < T, 1, Eigen::Dynamic, Eigen::DontAlign | Eigen::RowMajor >::Index IndexVector;
+ typedef typename DerivedSub::Index IndexSub;
+
+ // ptr[start] is the 1st element in the row
+ // The elements exists and are contiguous in memory, we use Eigen::Map functionality to optimize the operation
+ Eigen::Map<RowVector>(&ptr[start], static_cast<IndexVector>(m)).operator += (
+ subMatrix.row(static_cast<IndexSub>(rowId)).segment(0, static_cast<IndexSub>(m)));
+ }
+
+ void add(T* ptr, const T& value)
+ {
+ *ptr += value;
+ }
+};
+
+/// Runs a given operation on a SparseMatrix block(i, j, n, m) from a (n x m) 'subMatrix' with searching for the
+/// block 1st element. <br>
+/// It supposes: <br>
+/// + that the SparseMatrix already contains all the elements within the block (no insertion necessary) <br>
+/// + that both the SparseMatrix and the 'subMatrix' are using the same Scalar type, double. <br>
+/// This function will not change anything to the structure of the SparseMatrix, only change the values of the
+/// corresponding coefficients.
+/// \tparam Opt, Index Type parameters defining the output matrix type SparseMatrix<double, Opt, Index>
+/// \param subMatrix The sub matrix that will be copied into the SparseMatrix block
+/// \param rowStart, columnStart The row and column indices to indicate where the block in the SparseMatrix starts
+/// \param n, m The block size (Derived may be bigger but cannot be smaller in both dimension)
+/// \param[in,out] matrix The sparse matrix in which the block needs to be set by 'subMatrix'
+/// \param op The operation to run on the block
+/// \exception SurgSim::Framework::AssertionFailure If one of the following conditions is met: <br>
+/// * if 'subMatrix' is smaller than (n x m) in any dimension <br>
+/// * if 'matrix' is nullptr or smaller than (n x m) in any dimension <br>
+/// * if the requested block is out of range in 'matrix'. <br>
+/// * if 'matrix' does not fulfill the requirement (i.e. is missing elements within the block). <br>
+/// \note The receiving SparseMatrix must have a structure like the following: <br>
+/// (xx x0 x) <br>
+/// (xx 0x x) <br>
+/// (x0[xx]x) -> The block must already contain all the coefficients but these rows and columns may <br>
+/// (0x[xx]0) -> contains more coefficients before and after the block. <br>
+/// (xx 00 x) <br>
+template <int Opt, typename Index>
+void blockWithSearch(const Eigen::Ref<const Matrix>& subMatrix, size_t rowStart, size_t columnStart, size_t n, size_t m,
+ Eigen::SparseMatrix<double, Opt, Index>* matrix,
+ void (Operation<Matrix, Eigen::SparseMatrix<double, Opt, Index>>::*op)
+ (double*, size_t, size_t, size_t, const Matrix&, size_t))
+{
+ static Operation<Matrix, Eigen::SparseMatrix<double, Opt, Index>> operation;
+ SURGSIM_ASSERT(nullptr != matrix) << "Invalid recipient matrix, nullptr found";
+
+ SURGSIM_ASSERT(subMatrix.rows() >= static_cast<Matrix::Index>(n)) << "subMatrix doesn't have enough rows";
+ SURGSIM_ASSERT(subMatrix.cols() >= static_cast<Matrix::Index>(m)) << "subMatrix doesn't have enough columns";
+
+ SURGSIM_ASSERT(matrix->rows() >= static_cast<Index>(rowStart + n)) << "The block is out of range in matrix";
+ SURGSIM_ASSERT(matrix->cols() >= static_cast<Index>(columnStart + m)) << "The block is out of range in matrix";
+ SURGSIM_ASSERT(matrix->valuePtr() != nullptr) << "The matrix is not initialized correctly, null pointer to values";
+
+ double* ptr = matrix->valuePtr();
+ const Index* innerIndices = matrix->innerIndexPtr();
+ const Index* outerIndices = matrix->outerIndexPtr();
+
+ Index outerStart = static_cast<Index>(Opt == Eigen::ColMajor ? columnStart : rowStart);
+ Index innerStart = static_cast<Index>(Opt == Eigen::ColMajor ? rowStart : columnStart);
+ Index outerSize = static_cast<Index>(Opt == Eigen::ColMajor ? m : n);
+ Index innerSize = static_cast<Index>(Opt == Eigen::ColMajor ? n : m);
+
+ for (Index outerLoop = 0; outerLoop < outerSize; ++outerLoop)
+ {
+ // outerIndices[outerStart + outerLoop] is the index in ptr and innerIndices of the first non-zero element in
+ // the outer element (outerStart + outerLoop)
+ const Index innerStartIdInCurrentOuter = outerIndices[outerStart + outerLoop];
+ const Index innerStartIdInNextOuter = outerIndices[outerStart + outerLoop + 1];
+
+ // Look for the index of innerStart in this outer (the column/row may contain elements before)
+ Index innerFirstElement;
+ if (innerIndices[innerStartIdInCurrentOuter] == innerStart)
+ {
+ innerFirstElement = innerStartIdInCurrentOuter;
+ }
+ else
+ {
+ innerFirstElement = static_cast<Index>(matrix->data().searchLowerIndex(
+ innerStartIdInCurrentOuter, innerStartIdInNextOuter - 1, innerStart));
+ }
+
+ // Make sure we actually found the 1st element of the block in this outer
+ SURGSIM_ASSERT(innerIndices[innerFirstElement] == innerStart) <<
+ "matrix is missing an element of the block (1st element on a row/column)";
+
+ // Make sure that we are not going to write out of the range...
+ // i.e. The column/row (starting at the beginning of the block) has at least innerSize elements
+ SURGSIM_ASSERT(static_cast<Index>(innerSize) <= innerStartIdInNextOuter - innerFirstElement) <<
+ "matrix is missing elements of the block (but not the 1st element on a row/column)";
+
+ // Make sure that the last element corresponding to the block size has the expected index
+ SURGSIM_ASSERT(innerStart + static_cast<Index>(innerSize) - 1 == \
+ innerIndices[innerFirstElement + static_cast<Index>(innerSize) - 1]) <<
+ "The last element of the block does not have the expected index. " <<
+ "The matrix is missing elements of the block (but not the 1st element on a row/column)";
+
+ (operation.*op)(ptr, innerFirstElement, n, m, subMatrix, outerLoop);
+ }
+}
+
+/// Runs a given operation on a SparseMatrix block(i, j, n, m) from a (n x m) 'subMatrix' with searching for the
+/// block 1st element. <br>
+/// It supposes: <br>
+/// + that both the SparseMatrix and the 'subMatrix' are using the same Scalar type, double. <br>
+/// This function can be used to initialize the form of a sparse matrix as it will add entries when appropriate
+/// entries do not exist.
+/// \tparam Opt, Index Type parameters defining the output matrix type SparseMatrix<double, Opt, Index>
+/// \param subMatrix The sub matrix that will be copied into the SparseMatrix block
+/// \param rowStart, columnStart The row and column indices to indicate where the block in the SparseMatrix starts
+/// \param[in,out] matrix The sparse matrix in which the block needs to be set by 'subMatrix'
+/// \param op The operation to run on the block
+/// \exception SurgSim::Framework::AssertionFailure If one of the following conditions is met: <br>
+/// * if 'subMatrix' is smaller than (n x m) in any dimension <br>
+/// * if 'matrix' is nullptr or smaller than (n x m) in any dimension <br>
+/// * if the requested block is out of range in 'matrix'. <br>
+/// * if 'matrix' does not fulfill the requirement (i.e. is missing elements within the block). <br>
+/// \note The receiving SparseMatrix must have a structure like the following: <br>
+/// (xx x0 x) <br>
+/// (xx 0x x) <br>
+/// (x0[xx]x) -> The block must already contain all the coefficients but these rows and columns may <br>
+/// (0x[xx]0) -> contains more coefficients before and after the block. <br>
+/// (xx 00 x) <br>
+template <int Opt, typename Index>
+void blockOperation(const Eigen::Ref<const Matrix>& subMatrix, size_t rowStart, size_t columnStart,
+ Eigen::SparseMatrix<double, Opt, Index>* matrix,
+ void (Operation<Matrix, Eigen::SparseMatrix<double, Opt, Index>>::*op)(double*, const double&))
+{
+ static Operation<Matrix, Eigen::SparseMatrix<double, Opt, Index>> operation;
+ SURGSIM_ASSERT(nullptr != matrix) << "Invalid recipient matrix, nullptr found";
+
+ Index n = static_cast<Index>(subMatrix.rows());
+ Index m = static_cast<Index>(subMatrix.cols());
+ SURGSIM_ASSERT(matrix->rows() >= static_cast<Index>(rowStart) + n) << "The block is out of range in matrix";
+ SURGSIM_ASSERT(matrix->cols() >= static_cast<Index>(columnStart) + m) << "The block is out of range in matrix";
+
+ for (Index row = 0; row < n; ++row)
+ {
+ for (Index column = 0; column < m; ++column)
+ {
+ (operation.*op)(
+ &matrix->coeffRef(static_cast<Index>(rowStart) + row, static_cast<Index>(columnStart) + column),
+ subMatrix(row, column));
+ }
+ }
+}
+
+/// Helper method to add a sub-matrix into a matrix, for the sake of clarity
+/// \tparam Opt, Index Type parameters defining the output matrix type SparseMatrix<double, Opt, Index>
+/// \param subMatrix The sub-matrix
+/// \param blockIdRow, blockIdCol The block indices in matrix
+/// \param[in,out] matrix The matrix to add the sub-matrix into
+/// \param initialize Option parameter, default=true. If true, the matrix form is assumed to be undefined
+/// and is initialized when necessary. If false, the matrix form is assumed to be previously defined.
+/// \note This is a specialization of addSubMatrix for sparse matrices.
+template <int Opt, typename Index>
+void addSubMatrix(const Eigen::Ref<const Matrix>& subMatrix, size_t blockIdRow, size_t blockIdCol,
+ Eigen::SparseMatrix<double, Opt, Index>* matrix, bool initialize = true)
+{
+ if (initialize)
+ {
+ blockOperation(subMatrix, static_cast<size_t>(subMatrix.rows() * blockIdRow),
+ static_cast<size_t>(subMatrix.cols() * blockIdCol), matrix,
+ &Operation<Matrix, Eigen::SparseMatrix<double, Opt, Index>>::add);
+ }
+ else
+ {
+ blockWithSearch(subMatrix, static_cast<size_t>(subMatrix.rows() * blockIdRow),
+ static_cast<size_t>(subMatrix.cols() * blockIdCol),
+ static_cast<size_t>(subMatrix.rows()), static_cast<size_t>(subMatrix.cols()), matrix,
+ &Operation<Matrix, Eigen::SparseMatrix<double, Opt, Index>>::add);
+ }
+}
+
+/// Helper method to assign a sub-matrix into a matrix, for the sake of clarity
+/// \tparam Opt, Index Type parameters defining the output matrix type SparseMatrix<double, Opt, Index>
+/// \param subMatrix The sub-matrix
+/// \param blockIdRow, blockIdCol The block indices in matrix
+/// \param[in,out] matrix The matrix to assign the sub-matrix into
+/// \param initialize Option parameter, default=true. If true, the matrix form is assumed to be undefined
+/// and is initialized when necessary. If false, the matrix form is assumed to be previously defined.
+template <int Opt, typename Index>
+void assignSubMatrix(const Eigen::Ref<const Matrix>& subMatrix, size_t blockIdRow, size_t blockIdCol,
+ Eigen::SparseMatrix<double, Opt, Index>* matrix, bool initialize = true)
+{
+ if (initialize)
+ {
+ blockOperation(subMatrix, (subMatrix.rows() * blockIdRow),
+ (subMatrix.cols() * blockIdCol), matrix,
+ &Operation<Matrix, Eigen::SparseMatrix<double, Opt, Index>>::assign);
+ }
+ else
+ {
+ blockWithSearch(subMatrix, (subMatrix.rows() * blockIdRow),
+ (subMatrix.cols() * blockIdCol),
+ subMatrix.rows(), subMatrix.cols(), matrix,
+ &Operation<Matrix, Eigen::SparseMatrix<double, Opt, Index>>::assign);
+ }
+}
+
+/// Helper method to zero a row of a matrix specialized for Sparse Matrices
+/// \param row The row to set to zero
+/// \param[in,out] matrix The matrix to set the zero row on.
+template <typename T, int Opt, typename Index>
+void zeroRow(size_t row, Eigen::SparseMatrix<T, Opt, Index>* matrix)
+{
+ for (Index column = 0; column < matrix->cols(); ++column)
+ {
+ if (matrix->coeff(static_cast<Index>(row), column))
+ {
+ matrix->coeffRef(static_cast<Index>(row), column) = 0;
+ }
+ }
+}
+
+/// Helper method to zero a column of a matrix specialized for Sparse Matrices
+/// \param column The column to set to zero
+/// \param[in,out] matrix The matrix to set the zero column on.
+template <typename T, int Opt, typename Index>
+inline void zeroColumn(size_t column, Eigen::SparseMatrix<T, Opt, Index>* matrix)
+{
+ for (Index row = 0; row < matrix->rows(); ++row)
+ {
+ if (matrix->coeff(row, static_cast<Index>(column)))
+ {
+ matrix->coeffRef(row, static_cast<Index>(column)) = 0;
+ }
+ }
+}
+
+/// Helper method to zero all entries of a matrix specialized for Sparse Matrices. This
+/// allows the preservation of the the matrix form while still allowing the reset of
+/// the matrix entries to zero.
+/// \param[in,out] matrix The matrix to set to zero
+template <typename T, int Opt, typename Index>
+inline void clearMatrix(Eigen::SparseMatrix<T, Opt, Index>* matrix)
+{
+ SURGSIM_ASSERT(matrix->isCompressed()) << "Invalid matrix. Matrix must be in compressed form.";
+ T* ptr = matrix->valuePtr();
+ for (Index value = 0; value < matrix->nonZeros(); ++value)
+ {
+ *ptr = 0;
+ ++ptr;
+ }
+}
+
+}; // namespace Math
+}; // namespace SurgSim
+
+#endif // SURGSIM_MATH_SPARSEMATRIX_H
diff --git a/SurgSim/Math/SphereShape.cpp b/SurgSim/Math/SphereShape.cpp
index 1000cb6..4c0b77c 100644
--- a/SurgSim/Math/SphereShape.cpp
+++ b/SurgSim/Math/SphereShape.cpp
@@ -24,9 +24,10 @@ 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);
+ updateAabb();
}
-int SphereShape::getType()
+int SphereShape::getType() const
{
return SHAPE_TYPE_SPHERE;
}
@@ -39,6 +40,14 @@ double SphereShape::getRadius() const
void SphereShape::setRadius(double radius)
{
m_radius = radius;
+ updateAabb();
+}
+
+void SphereShape::updateAabb()
+{
+ m_aabb.setEmpty();
+ m_aabb.extend(Vector3d(-m_radius, -m_radius, -m_radius));
+ m_aabb.extend(Vector3d(m_radius, m_radius, m_radius));
}
double SphereShape::getVolume() const
@@ -69,5 +78,6 @@ bool SphereShape::isValid() const
return m_radius >= 0;
}
+
}; // namespace Math
}; // namespace SurgSim
diff --git a/SurgSim/Math/SphereShape.h b/SurgSim/Math/SphereShape.h
index 61d89f8..8222e1e 100644
--- a/SurgSim/Math/SphereShape.h
+++ b/SurgSim/Math/SphereShape.h
@@ -37,7 +37,7 @@ public:
SURGSIM_CLASSNAME(SurgSim::Math::SphereShape);
/// \return the type of the shape
- virtual int getType() override;
+ int getType() const override;
/// Get the sphere radius
/// \return The sphere radius
@@ -45,19 +45,19 @@ public:
/// Get the volume of the shape
/// \return The volume of the shape (in m-3)
- virtual double getVolume() const override;
+ double getVolume() const override;
/// Get the volumetric center of the shape
/// \return The center of the shape
- virtual Vector3d getCenter() const override;
+ 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;
+ Matrix33d getSecondMomentOfVolume() const override;
/// \return True if radius is bigger than or equal to 0; Otherwise, false.
- virtual bool isValid() const override;
+ bool isValid() const override;
protected:
// Setters in 'protected' sections are for serialization purpose only.
@@ -66,9 +66,11 @@ protected:
/// \param radius The sphere radius
void setRadius(double radius);
+ void updateAabb();
private:
/// Sphere radius
double m_radius;
+
};
}; // Math
diff --git a/SurgSim/Math/SurfaceMeshShape-inl.h b/SurgSim/Math/SurfaceMeshShape-inl.h
index 2720129..c297ace 100644
--- a/SurgSim/Math/SurfaceMeshShape-inl.h
+++ b/SurgSim/Math/SurfaceMeshShape-inl.h
@@ -24,14 +24,9 @@ namespace Math
template <class VertexData, class EdgeData, class TriangleData>
SurfaceMeshShape::SurfaceMeshShape(
- const SurgSim::DataStructures::TriangleMeshBase<VertexData, EdgeData, TriangleData>& mesh,
- double thickness) : m_thickness(thickness)
+ const SurgSim::DataStructures::TriangleMesh<VertexData, EdgeData, TriangleData>& mesh,
+ double thickness) : MeshShape(mesh), 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();
}
diff --git a/SurgSim/Math/SurfaceMeshShape.cpp b/SurgSim/Math/SurfaceMeshShape.cpp
index 5cd1122..8f15f75 100644
--- a/SurgSim/Math/SurfaceMeshShape.cpp
+++ b/SurgSim/Math/SurfaceMeshShape.cpp
@@ -15,9 +15,10 @@
//
#include "SurgSim/Math/SurfaceMeshShape.h"
-#include "SurgSim/DataStructures/TriangleMeshUtilities.h"
+
#include "SurgSim/Framework/Log.h"
+
namespace
{
const double epsilon = 1e-10;
@@ -29,64 +30,15 @@ namespace Math
{
SURGSIM_REGISTER(SurgSim::Math::Shape, SurgSim::Math::SurfaceMeshShape, SurfaceMeshShape);
-SurfaceMeshShape::SurfaceMeshShape() : m_volume(0.0), m_thickness(1e-2)
+SurfaceMeshShape::SurfaceMeshShape() : m_thickness(1e-2)
{
- SURGSIM_ADD_SERIALIZABLE_PROPERTY(SurfaceMeshShape, std::string, FileName, getFileName, setFileName);
}
-int SurfaceMeshShape::getType()
+int SurfaceMeshShape::getType() const
{
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:
@@ -128,16 +80,16 @@ void SurfaceMeshShape::computeVolumeIntegrals()
Eigen::VectorXd integral(10);
integral.setZero();
- for (auto const& triangle : m_mesh->getTriangles())
+ for (auto const& triangle : 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]);
+ auto A = getVertexPosition(triangle.verticesId[0]);
+ auto B = getVertexPosition(triangle.verticesId[1]);
+ auto C = 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;
@@ -190,7 +142,7 @@ void SurfaceMeshShape::computeVolumeIntegrals()
bool SurfaceMeshShape::isValid() const
{
- return (nullptr != m_mesh) && (m_thickness > 1e-5) && (m_mesh->isValid());
+ return (m_thickness > 1e-5) && MeshShape::isValid();
}
}; // namespace Math
diff --git a/SurgSim/Math/SurfaceMeshShape.h b/SurgSim/Math/SurfaceMeshShape.h
index ccf5b4d..8d7955a 100644
--- a/SurgSim/Math/SurfaceMeshShape.h
+++ b/SurgSim/Math/SurfaceMeshShape.h
@@ -17,8 +17,8 @@
#define SURGSIM_MATH_SURFACEMESHSHAPE_H
#include "SurgSim/DataStructures/TriangleMesh.h"
-#include "SurgSim/DataStructures/TriangleMeshBase.h"
#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Math/MeshShape.h"
#include "SurgSim/Math/Shape.h"
namespace SurgSim
@@ -50,7 +50,7 @@ SURGSIM_STATIC_REGISTRATION(SurfaceMeshShape);
/// \note SurgSim::Physics::DcdCollision so far.
///
/// \sa MeshShape
-class SurfaceMeshShape : public Shape
+class SurfaceMeshShape : public MeshShape
{
public:
/// Constructor
@@ -62,71 +62,27 @@ public:
/// \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,
+ const SurgSim::DataStructures::TriangleMesh<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;
+ int getType() const override;
/// 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:
+ bool isValid() const override;
+protected:
/// 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;
+ void computeVolumeIntegrals() override;
+private:
/// Surface mesh thickness
double m_thickness;
-
- /// File name of the external file which contains the triangle mesh.
- std::string m_fileName;
};
}; // Math
diff --git a/SurgSim/Math/TriangleCapsuleContactCalculation-inl.h b/SurgSim/Math/TriangleCapsuleContactCalculation-inl.h
new file mode 100644
index 0000000..88ceed0
--- /dev/null
+++ b/SurgSim/Math/TriangleCapsuleContactCalculation-inl.h
@@ -0,0 +1,579 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_TRIANGLECAPSULECONTACTCALCULATION_INL_H
+#define SURGSIM_MATH_TRIANGLECAPSULECONTACTCALCULATION_INL_H
+
+#include "SurgSim/Math/Valid.h"
+#include "SurgSim/DataStructures/OptionalValue.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+namespace TriangleCapsuleContactCalculation
+{
+
+/// Find the point on (positive X side of) ellipse parallel to the given tangent.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+/// \param tangent The given tangent to this ellipse, whose corresponding point is to be found
+/// \param center Center of the ellipse.
+/// \param majorAxis, minorAxis The major/minor axes of the ellipse, both of unit length
+/// \param majorRadius, minorRadius Major/minor radii of the ellipse
+/// \note majorAxis and minorAxis are assumed to be orthogonal to each other.
+/// \return The point on the ellipse (in positive x direction) which has the given tangent.
+template <class T, int MOpt>
+Eigen::Matrix<T, 3, 1, MOpt> pointWithTangentOnEllipse(const Eigen::Matrix<T, 3, 1, MOpt>& center,
+ const Eigen::Matrix<T, 3, 1, MOpt>& majorAxis,
+ const Eigen::Matrix<T, 3, 1, MOpt>& minorAxis,
+ const double majorRadius, const double minorRadius,
+ const Eigen::Matrix<T, 3, 1, MOpt>& tangent)
+{
+ Eigen::Transform<T, 3, Eigen::Isometry> transform;
+ transform.translation() = center;
+ transform.linear().col(0) = majorAxis;
+ transform.linear().col(1) = minorAxis;
+ transform.linear().col(2) = majorAxis.cross(minorAxis);
+
+ // tangent in local coordinates.
+ Eigen::Matrix<T, 3, 1, MOpt> localTangent = transform.inverse().linear() * tangent;
+
+ // Slope of this tangent
+ T m = localTangent[1] / localTangent[0];
+
+ // Ellipse equation: x*x/a*a + y*y/b*b = 1
+ // Rewriting ellipse equation in the form, y = f(x): y = sqrt(a*a - x*x) * b / a
+ // Slope of ellipse: y' = -x*b*b/a*a*y
+ // This slope is equal to the slope of the localTangent. So, we can solve for x and y.
+ T x = std::sqrt((m * m * majorRadius * majorRadius * majorRadius * majorRadius) /
+ (minorRadius * minorRadius + m * m * majorRadius * majorRadius));
+ T y = (minorRadius / majorRadius) *
+ std::sqrt(majorRadius * majorRadius - x * x) * ((m > 0.0) ? -1.0 : 1.0);
+
+ // Transforming this point into world coordinates.
+ return transform * Eigen::Matrix<T, 3, 1, MOpt>(x, y, static_cast<T>(0));
+}
+
+/// Class used to find the intersection between a triangle and a capsule.
+/// \tparam T Accuracy of the calculation, can usually be inferred.
+/// \tparam MOpt Eigen Matrix options, can usually be inferred.
+template <class T, int MOpt>
+class TriangleCapsuleContactCalculation
+{
+ typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
+ typedef Eigen::Matrix<T, 2, 1, MOpt> Vector2;
+ typedef Eigen::Transform<T, 3, Eigen::Isometry> RigidTransform3;
+
+public:
+ /// Constructor.
+ /// \param tv0,tv1,tv2 Vertices of the triangle.
+ /// \param tn Normal of the triangle, should be normalized.
+ /// \param cv0,cv1 Ends of the capsule axis.
+ /// \param cr Capsule radius.
+ TriangleCapsuleContactCalculation(
+ const Vector3& tv0, const Vector3& tv1, const Vector3& tv2,
+ const Vector3& tn,
+ const Vector3& cv0, const Vector3& cv1,
+ double cr)
+ : m_tv0(tv0), m_tv1(tv1), m_tv2(tv2), m_tn(tn),
+ m_cvTop(cv0), m_cvBottom(cv1), m_cr(cr)
+ {
+ m_epsilon = static_cast<T>(Geometry::DistanceEpsilon);
+ m_distance = distanceSegmentTriangle(cv0, cv1, m_tv0, m_tv1, m_tv2, m_tn,
+ &m_penetrationPointCapsuleAxis, &m_penetrationPointTriangle);
+ m_cAxis = m_cvBottom - m_cvTop;
+ m_cLength = m_cAxis.norm();
+ m_cAxis = m_cAxis / m_cLength;
+ if (m_cAxis.dot(tn) > 0.0)
+ {
+ m_cvTop = cv1;
+ m_cvBottom = cv0;
+ m_cAxis = -m_cAxis;
+ }
+ }
+
+ /// \return Whether there is an intersection.
+ bool isIntersecting()
+ {
+ return m_distance < (m_cr - m_epsilon);
+ }
+
+ /// Calculate the contact info.
+ /// \param [out] penetrationDepth The depth of penetration.
+ /// \param [out] penetrationPointTriangle The contact point on triangle.
+ /// \param [out] penetrationPointCapsule The contact point on capsule.
+ /// \param [out] contactNormal The contact normal that points from capsule to triangle.
+ /// \param [out] penetrationPointCapsuleAxis The point on the capsule axis closest to the triangle.
+ void calculateContact(T* penetrationDepth, Vector3* penetrationPointTriangle, Vector3* penetrationPointCapsule,
+ Vector3* contactNormal, Vector3* penetrationPointCapsuleAxis)
+ {
+ if (isIntersecting())
+ {
+ bool result =
+ axisAwayFromTriangle() ||
+ axisPerpendicularToTriangle() ||
+ axisTouchingTriangle() ||
+ axisThroughTriangle();
+
+ SURGSIM_ASSERT(result) << "At this point, there has to be an intersection.";
+
+ *penetrationDepth = m_penetrationDepth;
+ *penetrationPointTriangle = m_penetrationPointTriangle;
+ *penetrationPointCapsule = m_penetrationPointCapsule;
+ *contactNormal = m_contactNormal;
+ *penetrationPointCapsuleAxis = m_penetrationPointCapsuleAxis;
+ }
+ }
+
+private:
+ /// This function handles the contact data calculation for the case where there is an intersection between the
+ /// capsule and the triangle, but the capsule axis does not intersect the triangle.
+ /// \return True, if the axis of the capsule is away from the triangle.
+ /// \note This function presupposes that isIntersecting() returned true.
+ bool axisAwayFromTriangle()
+ {
+ if (m_distance > m_epsilon)
+ {
+ m_contactNormal = m_penetrationPointTriangle - m_penetrationPointCapsuleAxis;
+ m_contactNormal.normalize();
+ m_penetrationPointCapsule = m_penetrationPointCapsuleAxis + (m_contactNormal * m_cr);
+ m_penetrationDepth = m_cr - m_distance;
+ return true;
+ }
+
+ return false;
+ }
+
+ /// This function handles the contact data calculation for the case where there is an intersection between the
+ /// capsule and the triangle, and the capsule axis is perpendicular to the triangle.
+ /// \return True, if the axis of the capsule is perpendicular to the triangle.
+ /// \note This function presupposes that isIntersecting() returned true and axisAwayFromTriangle() returned false.
+ bool axisPerpendicularToTriangle()
+ {
+ if (std::abs(m_cAxis.dot(m_tn) + 1.0) < m_epsilon)
+ {
+ m_penetrationPointCapsule = m_cvBottom - m_tn * m_cr;
+ m_penetrationPointCapsuleAxis = m_cvBottom;
+ m_contactNormal = -m_tn;
+ m_penetrationDepth = (m_tv0 - m_penetrationPointCapsule).dot(m_tn);
+ return true;
+ }
+
+ return false;
+ }
+
+ /// This function handles the contact data calculation for the case where there is an intersection between the
+ /// capsule and the triangle, and the capsule axis just touches the triangle.
+ /// \return True, if the axis of the capsule is just touching triangle.
+ /// \note This function presupposes that isIntersecting() returned true, axisAwayFromTriangle() returned false,
+ /// and axisPerpendicularToTriangle() returned false.
+ bool axisTouchingTriangle()
+ {
+ if (m_penetrationPointCapsuleAxis.isApprox(m_cvTop, m_epsilon) ||
+ m_penetrationPointCapsuleAxis.isApprox(m_cvBottom, m_epsilon) ||
+ isPointOnTriangleEdge(m_penetrationPointTriangle, m_tv0, m_tv1, m_tv2, m_tn))
+ {
+ m_contactNormal = -m_tn;
+
+ auto projectionCvBottom = (m_cvBottom + m_tn * (m_tv0 - m_cvBottom).dot(m_tn)).eval();
+ if (SurgSim::Math::isPointInsideTriangle(projectionCvBottom, m_tv0, m_tv1, m_tv2, m_tn))
+ {
+ m_contactNormal = -m_tn;
+ m_penetrationPointCapsule = m_cvBottom - m_tn * m_cr;
+ m_penetrationPointCapsuleAxis = m_cvBottom;
+ }
+ else
+ {
+ farthestIntersectionLineCapsule(m_penetrationPointTriangle, -m_tn,
+ &m_penetrationPointCapsule, &m_penetrationPointCapsuleAxis);
+ }
+ m_penetrationDepth = (m_tv0 - m_penetrationPointCapsule).dot(m_tn);
+ return true;
+ }
+
+ return false;
+ }
+
+ /// This function handles the contact data calculation for the case where there is an intersection between the
+ /// capsule and the triangle, and the capsule axis goes through the triangle.
+ /// \return True, if the axis of the capsule goes through the triangle. Also calculates the contact info.
+ /// \note This function presupposes that isIntersecting() returned true, axisAwayFromTriangle() returned false,
+ /// axisPerpendicularToTriangle() returned false, and axisTouchingTriangle() returned false.
+ bool axisThroughTriangle()
+ {
+ if (m_distance != 0.0)
+ {
+ return false;
+ }
+
+ // Extruding the triangle along the direction of -tn creates a volume. Clip off the capsule axis that is
+ // outside of this volume.
+ Vector3 v[3] = {m_tv0, m_tv1, m_tv2};
+ Vector3 planeN[4] = {m_tn, (-m_tn.cross(m_tv1 - m_tv0)).normalized(), (-m_tn.cross(m_tv2 - m_tv1)).normalized(),
+ (-m_tn.cross(m_tv0 - m_tv2)).normalized()};
+ double planeD[4] = {-m_tv0.dot(planeN[0]), -m_tv0.dot(planeN[1]), -m_tv1.dot(planeN[2]), -m_tv2.dot(planeN[3])};
+ auto segmentStart = m_cvTop;
+ auto segmentEnd = m_cvBottom;
+
+ size_t j = clipSegmentAgainstTriangle(&segmentStart, &segmentEnd, v, planeN, planeD);
+
+ if (j == 0)
+ {
+ m_contactNormal = -m_tn;
+ m_penetrationPointCapsule = m_cvBottom - m_tn * m_cr;
+ m_penetrationDepth = (m_tv0 - m_penetrationPointCapsule).dot(m_tn);
+ m_penetrationPointCapsuleAxis = m_cvBottom;
+ m_penetrationPointTriangle = m_penetrationPointCapsule + m_tn * m_penetrationDepth;
+ return true;
+ }
+
+ Vector3 deepestPoint = ((segmentStart - segmentEnd).dot(planeN[0]) < 0.0) ? segmentStart : segmentEnd;
+ Vector3 edgeVertices[2] = {v[(j - 1) % 3], v[j % 3]};
+ Vector3 triangleEdge = (edgeVertices[1] - edgeVertices[0]).normalized();
+
+ // The capsule axis intersects the plane (planeN[j], planeD[j]). First, the capsule is treated as a cylinder.
+ // Its intersection with this plane is an ellipse. The deepest point on this ellipse along -tn and bounded
+ // by vectors (edgeVertices[0] -> edgeVertices[0] - tn) and (edgeVertices[1] -> edgeVertices[1] - tn) is found.
+ Vector3 center = deepestPoint;
+ Vector3 majorAxis = (triangleEdge * (triangleEdge.dot(m_cAxis)) + m_tn * (m_tn.dot(m_cAxis))).normalized();
+ double majorRadius = farthestIntersectionLineCylinder(center, majorAxis, &deepestPoint);
+ SURGSIM_ASSERT(isValid(majorRadius)) << "The major radius of the ellipse should be a valid number.";
+
+ if (std::abs(majorAxis.dot(triangleEdge)) > m_epsilon)
+ {
+ // majorApex is not the deepest point because the ellipse is angled. The deepest point is between majorApex
+ // and minorApex on the circumference of the ellipse, and the tangent at that point is parallel to the
+ // triangleEdge.
+ auto minorAxis = planeN[j].cross(majorAxis);
+ double minorRadius = farthestIntersectionLineCylinder(center, minorAxis);
+ SURGSIM_ASSERT(isValid(minorRadius)) << "The minor radius of the ellipse should be a valid number.";
+
+ deepestPoint = pointWithTangentOnEllipse(center, majorAxis, minorAxis, majorRadius, minorRadius,
+ triangleEdge);
+ Vector3 result;
+ if (std::abs(distancePointSegment(deepestPoint, m_cvTop, m_cvBottom, &result) - m_cr) > m_epsilon)
+ {
+ // The deepest point is not on the capsule, which means that the capsule end (the sphere) is
+ // intersecting the triangle edge plane (planeN[j], planeD[j]). The intersection between them is a
+ // circle. Define a 2D co-ordinate system with the origin at edgeVertices[0], the x-axis as
+ // triangleEdge, and the y-axis as tn. Transforming the circle to this 2D co-ordinate system, creates a
+ // circle of radius, r, with its center at x, y. Now the deepest point on this circle is (x, y - r).
+ Vector3 origin = edgeVertices[0], xAxis = triangleEdge, yAxis = m_tn, zAxis = planeN[j];
+
+ double sphereCenterToXYPlane = (m_cvBottom - origin).dot(zAxis);
+ SURGSIM_ASSERT(m_cr + m_epsilon >= sphereCenterToXYPlane)
+ << "The sphere center is too far from the triangle edge plane.";
+
+ double circleRadius = std::sqrt(m_cr * m_cr - sphereCenterToXYPlane * sphereCenterToXYPlane);
+ SURGSIM_ASSERT(isValid(circleRadius))
+ << "The radius of the circle of intersection between the sphere and the plane is invalid.";
+ Vector3 circleCenter = m_cvBottom - zAxis * sphereCenterToXYPlane;
+ double x = (circleCenter - origin).dot(xAxis);
+ double y = (circleCenter - origin).dot(yAxis) - circleRadius;
+ deepestPoint = xAxis * x + yAxis * y + origin;
+ }
+ }
+
+ // Project deepestPoint on the triangle edge to make sure it is within the edge.
+ auto edgeLength = (edgeVertices[1] - edgeVertices[0]).norm();
+ double deepestPointDotEdge = triangleEdge.dot(deepestPoint - edgeVertices[0]);
+ if (deepestPointDotEdge <= -m_epsilon || deepestPointDotEdge >= edgeLength + m_epsilon)
+ {
+ // In this case, the intersection of the cylinder with the triangle edge plane gives an ellipse
+ // that is close to a triangle corner and the deepest penetration point on the ellipse is
+ // actually outside the triangle.
+ // Solution: find the deepest point on the ellipse which projection is still on the triangle.
+ // For that: Find the corner edgeVertex of the triangle closest to deepestPointDotEdge
+ // Find the deepest penetration point verifying P - tn*t and the cylinder equation.
+
+ // The triangle point to consider is edgeVertices[0] or edgeVertices[1].
+ Vector3 edgeVertex = (deepestPointDotEdge < 0.0) ? edgeVertices[0] : edgeVertices[1];
+ double d =
+ farthestIntersectionLineCapsule(edgeVertex, -m_tn, &deepestPoint, &m_penetrationPointCapsuleAxis);
+ SURGSIM_ASSERT(isValid(d)) << "There must be a part of the ellipse between the triangle edge at this point";
+ }
+
+ m_contactNormal = -m_tn;
+ m_penetrationPointCapsule = deepestPoint;
+ m_penetrationDepth = -m_tn.dot(deepestPoint - m_tv0);
+ m_penetrationPointTriangle = deepestPoint + m_tn * (m_penetrationDepth);
+
+ return true;
+ }
+
+ /// \param lineStart The origin of the line
+ /// \param lineDir Unit directional vector of the line
+ /// \param point [out] The point of intersection.
+ /// \return The distance of the point of intersection from the lineStart.
+ double farthestIntersectionLineCylinder(const Vector3& lineStart, const Vector3& lineDir, Vector3* point = nullptr)
+ {
+ if (!m_cInverseTransform.hasValue())
+ {
+ Vector3 j, k;
+ SurgSim::Math::buildOrthonormalBasis(&m_cAxis, &j, &k);
+
+ m_cTransform.translation() = m_cvTop;
+ m_cTransform.linear().col(0) = m_cAxis;
+ m_cTransform.linear().col(1) = j;
+ m_cTransform.linear().col(2) = k;
+ m_cInverseTransform = m_cTransform.inverse();
+ }
+
+ // Transform the problem in the cylinder space to solve the local cylinder equation y^2 + z^2 = r^2
+ // Point on ellipse should be on the line, P + t.(D)
+ // => Py^2 + t^2.Dy^2 + 2.Py.t.Dy + Pz^2 + t^2.Dz^2 + 2.Pz.t.Dz = r^2
+ // => t^2.(Dy^2 + Dz^2) + t.(2.Py.Dy + 2.Pz.Dz) + (Py^2 + Pz^2 - r^2) = 0
+ // Let a = (Dy^2 + Dz^2), b = (2.Py.Dy + 2.Pz.Dz), c = (Py^2 + Pz^2 - r^2):
+ // => t^2.a + t.b + c = 0, whose solution is:
+ // (-b +/- sqrt(b^2 - 4*a*c))/2*a
+ auto const P = (m_cInverseTransform.getValue() * lineStart).eval();
+ auto const D = (m_cInverseTransform.getValue().linear() * lineDir).eval();
+
+ T a = D[1] * D[1] + D[2] * D[2];
+ T b = static_cast<T>(2) * (P[1] * D[1] + P[2] * D[2]);
+ T c = (P[1] * P[1] + P[2] * P[2] - m_cr * m_cr);
+
+ T discriminant = b * b - static_cast<T>(4) * a * c;
+
+ if (discriminant < 0.0)
+ {
+ // Cannot use a sqrt on a negative number. Push it to zero if it is small.
+ if (discriminant >= -Geometry::ScalarEpsilon)
+ {
+ discriminant = 0.0;
+ }
+ else
+ {
+ return std::numeric_limits<T>::quiet_NaN();
+ }
+ }
+
+ // We have two solutions. We want the larger value.
+ double d = (-b / (static_cast<T>(2) * a)) + std::abs(std::sqrt(discriminant) / (static_cast<T>(2) * a));
+ SURGSIM_ASSERT(isValid(d));
+ if (point != nullptr)
+ {
+ *point = lineStart + lineDir * d;
+ }
+
+ return d;
+ }
+
+ /// \param lineStart The start of the line segment
+ /// \param lineDir The direction of the line segment
+ /// \param point [in,out] The point which is to be clipped.
+ /// \param pointOnCapsuleAxis [out] The recalculated point on the capsule axis.
+ /// \return The distance of the point of intersection from the lineStart.
+ /// \note Asserts when there is no intersection.
+ double farthestIntersectionLineCapsule(const Vector3& lineStart, const Vector& lineDir,
+ Vector3* point, Vector3* pointOnCapsuleAxis)
+ {
+ // Transform the problem into the capsule space to solve the local capsule equation. The capsule coordinate
+ // system has its origin on one the capsule ends (m_cvTop), the x axis is along the capsule axis (m_cAxis), and
+ // the y and z axes are any orthogonal vectors to the capsule axis. The equation of the capsule can be written
+ // as the following:
+ // x^2 + y^2 + z^2 = r^2 | x < 0 ------ [1]
+ // y^2 + z^2 = r^2 | 0 < x < length ------ [2]
+ // (x - length)^2 + y^2 + z^2 = r^2 | x > length ------ [3]
+ // Point should be on the line, P + t.(D)
+
+ // First find the intersection with an infinite cylinder centered around the capsule axis.
+ double d = farthestIntersectionLineCylinder(lineStart, lineDir, point);
+ SURGSIM_ASSERT(isValid(d));
+ *point = lineStart + lineDir * d;
+
+ // When x <0 or x > length, the equation of the capsule is that of the sphere (from equations [1] and [3]). So,
+ // the intersection between the line (P + t.D) and the sphere is calculated as below. Here l is the length of
+ // the capsule.
+ // => ((P + t.D).x - l)^2 + (P + t.D).y^2 + (P + t.D).z^2 = r^2
+ // => Px^2 + t^2.Dx^2 + l^2 + 2.Px.t.Dx - 2.t.Dx.l - 2.Px.l +
+ // Py^2 + t^2.Dy^2 + 2.Py.t.Dy + Pz^2 + t^2.Dz^2 + 2.Pz.t.Dz = r^2
+ // => t^2.(Dx^2 + Dy^2 + Dz^2) + t.(2.Px.Dx + 2.Py.Dy + 2.Pz.Dz - 2.Dx.l) +
+ // (Px^2 + Py^2 + Pz^2 + l^2 - 2.Px.l - r^2) = 0
+ // Let a = (Dx^2 + Dy^2 + Dz^2),
+ // b = (2.Px.Dx + 2.Py.Dy + 2.Pz.Dz - 2.Dx.l),
+ // c = (Px^2 + Py^2 + Pz^2 + l^2 - 2.Px.l - r^2):
+ double x = ((*point) - m_cvTop).dot(m_cAxis);
+ if (x <= 0.0 || x >= m_cLength)
+ {
+ x = (x <= 0.0) ? 0.0 : m_cLength;
+
+ auto const P = (m_cInverseTransform.getValue() * lineStart).eval();
+ auto const D = (m_cInverseTransform.getValue().linear() * lineDir).eval();
+
+ T a = D[0] * D[0] + D[1] * D[1] + D[2] * D[2];
+ T b = static_cast<T>(2) * (P[0] * D[0] + P[1] * D[1] + P[2] * D[2] - D[0] * x);
+ T c = (P[0] * P[0] + P[1] * P[1] + P[2] * P[2] + x * x - static_cast<T>(2) * P[0] * x - m_cr * m_cr);
+
+ // => t^2.a + t.b + c = 0, whose solution is:
+ // (-b +/- sqrt(b^2 - 4*a*c))/2*a
+ T discriminant = b * b - static_cast<T>(4) * a * c;
+
+ if (discriminant < 0.0 && discriminant >= -Geometry::ScalarEpsilon)
+ {
+ discriminant = 0.0;
+ }
+
+ // We have two solutions. We want the smaller value.
+ d = (-b - std::abs(std::sqrt(discriminant))) / (static_cast<T>(2) * a);
+ SURGSIM_ASSERT(isValid(d));
+ *point = lineStart + lineDir * d;
+ }
+
+ *pointOnCapsuleAxis = m_cTransform * Vector3(x, 0.0, 0.0);
+ return d;
+ }
+
+ /// \param segmentStart [in,out] The start of the line segment
+ /// \param segmentEnd [in,out] The end of the line segment
+ /// \param v The vertices of the triangle.
+ /// \param planeN Normals of the triangle and each of the edge planes.
+ /// \param planeD d from plane equation for the plane of the triangle and each of the edge planes.
+ /// \return The index of the last plane which clips the segment passed in.
+ size_t clipSegmentAgainstTriangle(Vector3* segmentStart, Vector3* segmentEnd, Vector3* v, Vector3* planeN,
+ double* planeD)
+ {
+ double ratio, dStart, dEnd;
+ size_t j = 4;
+ for (size_t i = 0; i < 4; ++i)
+ {
+ dStart = segmentStart->dot(planeN[i]) + planeD[i];
+ dEnd = segmentEnd->dot(planeN[i]) + planeD[i];
+
+ if (dStart < -m_epsilon && dEnd > m_epsilon)
+ {
+ ratio = std::abs(dStart) / (std::abs(dStart) + dEnd);
+ *segmentEnd = *segmentStart + (*segmentEnd - *segmentStart) * ratio;
+ j = i;
+ }
+ else if (dStart > m_epsilon && dEnd < -m_epsilon)
+ {
+ ratio = dStart / (dStart + std::abs(dEnd));
+ *segmentStart = *segmentStart + (*segmentEnd - *segmentStart) * ratio;
+ j = i;
+ }
+ else if (dStart < m_epsilon && dEnd > m_epsilon)
+ {
+ *segmentEnd = *segmentStart;
+ j = i;
+ }
+ else if (dStart > m_epsilon && dEnd < m_epsilon)
+ {
+ *segmentStart = *segmentEnd;
+ j = i;
+ }
+ }
+
+ SURGSIM_ASSERT(j < 4) << "The clipping should have happened at least with the triangle plane.";
+ return j;
+ }
+
+ ///@{
+ /// Triangle vertices and normal.
+ Vector3 m_tv0;
+ Vector3 m_tv1;
+ Vector3 m_tv2;
+ Vector3 m_tn;
+ ///@}
+ ///@{
+ /// Capsule ends, axis , radius and length.
+ Vector3 m_cvTop;
+ Vector3 m_cvBottom;
+ Vector3 m_cAxis;
+ double m_cr;
+ double m_cLength;
+ ///@}
+ /// Distance between triangle and capsule
+ double m_distance;
+ ///@{
+ /// Contact info
+ T m_penetrationDepth;
+ Vector3 m_penetrationPointTriangle;
+ Vector3 m_penetrationPointCapsule;
+ Vector3 m_contactNormal;
+ Vector3 m_penetrationPointCapsuleAxis;
+ ///@}
+ /// The transform of the capsule
+ RigidTransform3 m_cTransform;
+ /// The inverse transform of the capsule
+ SurgSim::DataStructures::OptionalValue<RigidTransform3> m_cInverseTransform;
+ /// epsilon
+ T m_epsilon;
+};
+}
+
+template <class T, int MOpt> inline
+bool calculateContactTriangleCapsule(
+ 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,
+ const Eigen::Matrix<T, 3, 1, MOpt>& cv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& cv1,
+ double cr,
+ T* penetrationDepth,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointTriangle,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsule,
+ Eigen::Matrix<T, 3, 1, MOpt>* contactNormal,
+ Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsuleAxis)
+{
+ TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation<T, MOpt>
+ calc(tv0, tv1, tv2, tn, cv0, cv1, cr);
+
+ if (calc.isIntersecting())
+ {
+ calc.calculateContact(penetrationDepth, penetrationPointTriangle, penetrationPointCapsule,
+ contactNormal, penetrationPointCapsuleAxis);
+ return true;
+ }
+
+ return false;
+}
+
+
+template <class T, int MOpt> inline
+ bool calculateContactTriangleCapsule(
+ 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>& cv0,
+ const Eigen::Matrix<T, 3, 1, MOpt>& cv1,
+ double cr,
+ 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>* penetrationPointCapsuleAxis)
+{
+ Eigen::Matrix<T, 3, 1, MOpt> tn = (tv1 - tv0).cross(tv2 - tv0);
+ Eigen::Matrix<T, 3, 1, MOpt> ca = cv1 - cv0;
+ if (tn.isZero() || ca.isZero())
+ {
+ // Degenerate triangle/capsule passed to calculateContactTriangleTriangle
+ return false;
+ }
+ tn.normalize();
+ return calculateContactTriangleCapsule(tv0, tv1, tv2, tn, cv0, cv1, cr, penetrationDepth,
+ penetrationPoint0, penetrationPoint1, contactNormal, penetrationPointCapsuleAxis);
+}
+
+} // namespace Math
+
+} // namespace SurgSim
+
+#endif // SURGSIM_MATH_TRIANGLECAPSULECONTACTCALCULATION_INL_H
diff --git a/SurgSim/Math/TriangleTriangleContactCalculation-inl.h b/SurgSim/Math/TriangleTriangleContactCalculation-inl.h
index cafb69b..1568eef 100644
--- a/SurgSim/Math/TriangleTriangleContactCalculation-inl.h
+++ b/SurgSim/Math/TriangleTriangleContactCalculation-inl.h
@@ -298,6 +298,206 @@ bool calculateContactTriangleTriangle(
penetrationPoint0, penetrationPoint1, contactNormal);
}
+template <class T, int MOpt> inline
+bool calculateContactTriangleTriangleSeparatingAxis(
+ 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;
+ using SurgSim::Math::Geometry::DistanceEpsilon;
+ using SurgSim::Math::Geometry::ScalarEpsilon;
+
+ SURGSIM_ASSERT(std::abs(t0n.norm() - 1.0) < ScalarEpsilon && std::abs(t1n.norm() - 1.0) < ScalarEpsilon)
+ << "The normals sent in are not normalized! t0n{" << t0n.transpose() << "}, t1n{" << t1n.transpose() << "}.";
+
+ // 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.
+ std::array<Vector3, 2> d = {Vector3(t1n.dot(t0v0 - t1v0), t1n.dot(t0v1 - t1v0), t1n.dot(t0v2 - t1v0)),
+ Vector3(t0n.dot(t1v0 - t0v0), t0n.dot(t1v1 - t0v0), t0n.dot(t1v2 - t0v0))};
+
+ if ((d[0].array() < DistanceEpsilon).all() || (d[0].array() > -DistanceEpsilon).all() ||
+ (d[1].array() < DistanceEpsilon).all() || (d[1].array() > -DistanceEpsilon).all())
+ {
+ return false;
+ }
+
+ const std::array<std::array<Vector3, 3>, 2> tv = {{{t0v0, t0v1, t0v2}, {t1v0, t1v1, t1v2}}};
+
+ // Find the intersection between a triangle and the plane of the other triangle. These intersections would
+ // lie on the separating axis (which is the cross product of the triangle normals).
+ // The name tsa is to indicate that this is storing the intersection of the Triangle with the Separating Axis.
+ std::array<std::array<Vector3, 2>, 2> tsa;
+ for (int i = 0; i < 2; ++i)
+ {
+ int index = 0;
+
+ for (int j = 0; j < 3; ++j)
+ {
+ int k = (j + 1) % 3;
+
+ // Intersect the edge tivj->tivk with the plane of t{(i+1)/2}
+ if ((d[i][j] < 0.0 && d[i][k] >= 0.0) || (d[i][j] > 0.0 && d[i][k] <= 0.0))
+ {
+ // The edge intersects the plane of t{(i+1)/2}.
+ auto ratio = std::abs(d[i][j]) / (std::abs(d[i][j]) + std::abs(d[i][k]));
+ tsa[i][index++] = tv[i][j] + ratio * (tv[i][k] - tv[i][j]);
+ }
+ }
+
+ SURGSIM_ASSERT(index == 2)
+ << "The intersection between the edges of triangle " << i << " and the plane of the other triangle"
+ << " must result in two points exactly.";
+ }
+
+ // The separating axis.
+ const Vector3 D = t0n.cross(t1n).normalized();
+ Vector3 result;
+
+ SURGSIM_ASSERT(distancePointLine(tsa[0][1], tsa[0][0], (tsa[0][0] + D).eval(), &result) < DistanceEpsilon &&
+ distancePointLine(tsa[1][0], tsa[0][0], (tsa[0][0] + D).eval(), &result) < DistanceEpsilon &&
+ distancePointLine(tsa[1][1], tsa[0][0], (tsa[0][0] + D).eval(), &result) < DistanceEpsilon)
+ << "The intersection points on the triangles do not lie on the separating axis";
+
+ static const int DISTANCE = 0;
+ static const int TRIANGLE = 1;
+ static const int VERTEX = 2;
+
+ // Find the signed distance of the four points on D (from tsa[0][0])
+ // Store it in a tuple containing this signed distance, the corresponding triangle ID and vertex ID.
+ std::array<std::tuple<T, int, int>, 4> intervals =
+ {std::tuple<T, int, int>(0.0, 0, 0),
+ std::tuple<T, int, int>((tsa[0][1] - tsa[0][0]).dot(D), 0, 1),
+ std::tuple<T, int, int>((tsa[1][0] - tsa[0][0]).dot(D), 1, 0),
+ std::tuple<T, int, int>((tsa[1][1] - tsa[0][0]).dot(D), 1, 1)};
+
+ // Sort the signed distance
+ std::sort(intervals.begin(), intervals.end(), [](const std::tuple<T, int, int>& i, std::tuple<T, int, int>& j)
+ {
+ return (std::get<DISTANCE>(i) < std::get<DISTANCE>(j));
+ });
+
+ // The intersections of the triangles on the separating axis (P, Q, R, S) are now sorted according to their distance
+ // along the separating axis.
+ //
+ // *--------*--------*--------*
+ // P Q R S
+
+ // If P and Q belong to the same triangle, there is no intersection between the triangles.
+ enum {P = 0, Q, R, S};
+ if (std::get<TRIANGLE>(intervals[P]) == std::get<TRIANGLE>(intervals[Q]))
+ {
+ return false;
+ }
+
+ // At this point, there is some overlap of the triangles along the separating axis.
+ size_t indexLeft, indexRight;
+ if (std::get<TRIANGLE>(intervals[Q]) == std::get<TRIANGLE>(intervals[R]))
+ {
+ // Q and R belong to the same triangle: Depending on whether PQ or RS is shorter, either PS is moved to the
+ // right of QR or QR is moved to the right of PS.
+ //
+ // *--------*--------*--------*
+ // P Q R S
+ // *--------*
+ // t1
+ // *--------------------------*
+ // t2
+
+ if ((std::get<DISTANCE>(intervals[Q]) - std::get<DISTANCE>(intervals[P])) <
+ (std::get<DISTANCE>(intervals[S]) - std::get<DISTANCE>(intervals[R])))
+ {
+ indexLeft = P;
+ indexRight = R;
+ }
+ else
+ {
+ indexLeft = Q;
+ indexRight = S;
+ }
+ }
+ else
+ {
+ // Q and R belong to different triangle: QS is moved to the right of PR.
+ //
+ // *--------*--------*--------*
+ // P Q R S
+ // *-----------------*
+ // t1
+ // *-----------------*
+ // t2
+
+ indexLeft = Q;
+ indexRight = R;
+ }
+
+ *penetrationDepth = std::get<DISTANCE>(intervals[indexRight]) - std::get<DISTANCE>(intervals[indexLeft]);
+ if (*penetrationDepth < DistanceEpsilon)
+ {
+ // Not enough overlap to be determined as an intersection.
+ return false;
+ }
+
+ if (std::get<TRIANGLE>(intervals[indexLeft]) == 0)
+ {
+ *penetrationPoint0 = tsa[0][std::get<VERTEX>(intervals[indexLeft])];
+ *penetrationPoint1 = tsa[1][std::get<VERTEX>(intervals[indexRight])];
+ }
+ else
+ {
+ *penetrationPoint0 = tsa[0][std::get<VERTEX>(intervals[indexRight])];
+ *penetrationPoint1 = tsa[1][std::get<VERTEX>(intervals[indexLeft])];
+ }
+
+ if ((*penetrationPoint0 - *penetrationPoint1).dot(D) > 0.0)
+ {
+ *contactNormal = -D;
+ }
+ else
+ {
+ *contactNormal = D;
+ }
+
+ return true;
+}
+
+
+template <class T, int MOpt> inline
+bool calculateContactTriangleTriangleSeparatingAxis(
+ 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 calculateContactTriangleTriangleSeparatingAxis(t0v0, t0v1, t0v2, t1v0, t1v1, t1v2, t0n, t1n,
+ penetrationDepth, penetrationPoint0, penetrationPoint1, contactNormal);
+}
+
} // namespace Math
diff --git a/SurgSim/Math/TriangleTriangleIntersection-inl.h b/SurgSim/Math/TriangleTriangleIntersection-inl.h
index da5e2e8..57a9acc 100644
--- a/SurgSim/Math/TriangleTriangleIntersection-inl.h
+++ b/SurgSim/Math/TriangleTriangleIntersection-inl.h
@@ -16,7 +16,6 @@
#ifndef SURGSIM_MATH_TRIANGLETRIANGLEINTERSECTION_INL_H
#define SURGSIM_MATH_TRIANGLETRIANGLEINTERSECTION_INL_H
-
namespace
{
static const double EPSILOND = 1e-12;
@@ -79,6 +78,7 @@ bool doesIntersectTriangleTriangle(
const Eigen::Matrix<T, 3, 1, MOpt>& t1n)
{
typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
+ using SurgSim::Math::Geometry::DistanceEpsilon;
if (t0n.isZero() || t1n.isZero())
{
@@ -110,7 +110,7 @@ bool doesIntersectTriangleTriangle(
// 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())
+ if ((d2.array() < DistanceEpsilon).all() || (d2.array() > -DistanceEpsilon).all())
{
return false;
}
@@ -118,13 +118,13 @@ bool doesIntersectTriangleTriangle(
// 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())
+ if ((d1.array() < DistanceEpsilon).all() || (d1.array() > -DistanceEpsilon).all())
{
return false;
}
// The separating axis.
- Vector3 D = t0n.cross(t1n);
+ Vector3 D = t0n.cross(t1n).normalized();
// Projection of the triangle vertices on the separating axis.
Vector3 pv1(D.dot(t0v0), D.dot(t0v1), D.dot(t0v2));
@@ -136,7 +136,7 @@ bool doesIntersectTriangleTriangle(
size_t s1Index = 0;
size_t s2Index = 0;
- // Loop through the edges of each triangle and find the intersectio of these edges onto
+ // Loop through the edges of each triangle and find the intersection of these edges onto
// the plane of the other triangle.
for (int i = 0; i < 3; ++i)
{
@@ -150,8 +150,16 @@ bool doesIntersectTriangleTriangle(
<< "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]);
+ // s1[0], s1[1] are the (unordered) extents of the projection of T1 on D.
+ // s2[0], s2[1] are the (unordered) extents of the projection of T2 on D.
+ // If both these are line segments (i.e. the distance between them is > epsilon),
+ // and if they overlap, then the two triangles intersect.
+
+ return !(std::abs(s1[0] - s1[1]) <= DistanceEpsilon || std::abs(s2[0] - s2[1]) <= DistanceEpsilon) &&
+ !(s1[0] <= (s2[0] + DistanceEpsilon) && s1[0] <= (s2[1] + DistanceEpsilon) &&
+ s1[1] <= (s2[0] + DistanceEpsilon) && s1[1] <= (s2[1] + DistanceEpsilon)) &&
+ !(s1[0] >= (s2[0] - DistanceEpsilon) && s1[0] >= (s2[1] - DistanceEpsilon) &&
+ s1[1] >= (s2[0] - DistanceEpsilon) && s1[1] >= (s2[1] - DistanceEpsilon));
}
diff --git a/SurgSim/Math/UnitTests/AabbTests.cpp b/SurgSim/Math/UnitTests/AabbTests.cpp
index cb8679f..12551e2 100644
--- a/SurgSim/Math/UnitTests/AabbTests.cpp
+++ b/SurgSim/Math/UnitTests/AabbTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -17,6 +17,7 @@
#include "SurgSim/Math/Aabb.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/RigidTransform.h"
namespace SurgSim
{
@@ -70,6 +71,23 @@ TEST(AabbdTests, makeAabb)
EXPECT_TRUE(expected.isApprox(aabb));
}
+TEST(AabbdTest, rotateAabb)
+{
+ Vector3d one(1.0, 2.0, 3.0);
+ Vector3d two(-6.0, -4.0, -3.0);
+
+ RigidTransform3d transform = makeRigidTransform(Quaterniond(0.0, 1.0, 0.0, 0.0), Vector3d(1.0, 2.0, 3.0));
+
+ Aabbd expected(transform * one);
+ expected.extend(transform * two);
+
+ Aabbd aabb(one);
+ aabb.extend(two);
+
+ aabb = transformAabb(transform, aabb);
+ EXPECT_TRUE(expected.isApprox(aabb));
+}
+
}
}
diff --git a/SurgSim/Math/UnitTests/AngleAxisTests.cpp b/SurgSim/Math/UnitTests/AngleAxisTests.cpp
new file mode 100644
index 0000000..d2e754c
--- /dev/null
+++ b/SurgSim/Math/UnitTests/AngleAxisTests.cpp
@@ -0,0 +1,66 @@
+// 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 AngleAxisTests : public testing::Test
+{
+public:
+ typedef T AngleAxis;
+ typedef typename T::Scalar Scalar;
+};
+typedef ::testing::Types<Eigen::AngleAxisd, Eigen::AngleAxisf> AngleAxisVariants;
+TYPED_TEST_CASE(AngleAxisTests, AngleAxisVariants);
+
+// Now we're ready to start testing...
+
+// Test conversion to and from yaml node
+TYPED_TEST(AngleAxisTests, YamlConvert)
+{
+ typedef typename TestFixture::AngleAxis AngleAxis;
+ typedef typename TestFixture::Scalar T;
+
+ AngleAxis angleAxis;
+ angleAxis.angle() = T(0.1);
+ angleAxis.axis()[0] = T(1.2);
+ angleAxis.axis()[1] = T(2.3);
+ angleAxis.axis()[2] = T(3.4);
+
+ YAML::Node node;
+
+ ASSERT_NO_THROW(node = angleAxis);
+
+ EXPECT_TRUE(node.IsMap());
+ YAML::Node angleNode = node["Angle"];
+ EXPECT_TRUE(angleNode.IsScalar());
+ YAML::Node axisNode = node["Axis"];
+ EXPECT_TRUE(axisNode.IsSequence());
+ EXPECT_EQ(3u, axisNode.size());
+
+ ASSERT_NO_THROW({ AngleAxis expected = node.as<AngleAxis>(); });
+ EXPECT_TRUE(angleAxis.isApprox(node.as<AngleAxis>()));
+}
+
diff --git a/SurgSim/Math/UnitTests/CMakeLists.txt b/SurgSim/Math/UnitTests/CMakeLists.txt
index 07abb38..494a258 100644
--- a/SurgSim/Math/UnitTests/CMakeLists.txt
+++ b/SurgSim/Math/UnitTests/CMakeLists.txt
@@ -20,9 +20,16 @@ include_directories(
set(UNIT_TEST_SOURCES
AabbTests.cpp
+ CardinalSplinesTests.cpp
+ CompoundShapeTests.cpp
+ CubicSolverTests.cpp
GeometryTests.cpp
+ IntervalArithmeticTests.cpp
+ KalmanFilterTests.cpp
+ LinearMotionArithmeticTests.cpp
MakeRigidTransformTests.cpp
MeshShapeTests.cpp
+ MinMaxTests.cpp
MlcpGaussSeidelSolverTests.cpp
OdeEquationTests.cpp
OdeSolverEulerExplicitModifiedTests.cpp
@@ -32,24 +39,36 @@ set(UNIT_TEST_SOURCES
OdeSolverStaticTests.cpp
OdeSolverTests.cpp
OdeStateTests.cpp
+ ParticlesShapeTests.cpp
+ PolynomialRootTests.cpp
+ PolynomialTests.cpp
+ PolynomialValuesTests.cpp
+ ScalarTests.cpp
+ SegmentMeshShapeTests.cpp
ShapeTests.cpp
SurfaceMeshShapeTests.cpp
+ TriangleCapsuleContactCalculationTests.cpp
TriangleTriangleContactCalculationTests.cpp
TriangleTriangleIntersectionTests.cpp
+ TriangleTriangleSeparatingAxisContactCalculationTests.cpp
ValidTests.cpp
)
set(UNIT_TEST_HEADERS
+ MockCapsule.h
MockObject.h
MockTriangle.h
TriangleTriangleTestParameters.h
)
set(UNIT_TEST_EIGEN_TYPE_SOURCES
+ AngleAxisTests.cpp
LinearSolveAndInverseTests.cpp
+ LinearSparseSolveAndInverseTests.cpp
MatrixTests.cpp
QuaternionTests.cpp
RigidTransformTests.cpp
+ SparseMatrixTests.cpp
VectorTests.cpp
)
@@ -62,7 +81,6 @@ option(BUILD_TESTING_EIGEN
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")
@@ -75,8 +93,7 @@ set(LIBS
)
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)
+file(COPY ${SURGSIM_SOURCE_DIR}/SurgSim/Math/UnitTests/Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
# Configure the path for the data files
configure_file(
diff --git a/SurgSim/Math/UnitTests/CardinalSplinesTests.cpp b/SurgSim/Math/UnitTests/CardinalSplinesTests.cpp
new file mode 100644
index 0000000..2c510b6
--- /dev/null
+++ b/SurgSim/Math/UnitTests/CardinalSplinesTests.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.
+
+/// \file
+/// Tests for Cardinal Splines interpolation utilities
+///
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <vector>
+
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Math/CardinalSplines.h"
+
+TEST(CardinalSplinesTests, extension)
+{
+ {
+ SurgSim::DataStructures::VerticesPlain emptyPoints;
+ std::vector<SurgSim::Math::Vector3d> result;
+ EXPECT_ANY_THROW(SurgSim::Math::CardinalSplines::extendControlPoints(emptyPoints, &result));
+ }
+
+ {
+ SurgSim::DataStructures::VerticesPlain onePoints;
+ onePoints.addVertex(SurgSim::DataStructures::VerticesPlain::VertexType(SurgSim::Math::Vector3d(0.0, 1.0, 2.0)));
+ std::vector<SurgSim::Math::Vector3d> result;
+ EXPECT_ANY_THROW(SurgSim::Math::CardinalSplines::extendControlPoints(onePoints, &result));
+ }
+
+ {
+ SurgSim::DataStructures::VerticesPlain twoPoints;
+ twoPoints.addVertex(SurgSim::DataStructures::VerticesPlain::VertexType(SurgSim::Math::Vector3d(0.0, 0.0, 0.0)));
+ twoPoints.addVertex(SurgSim::DataStructures::VerticesPlain::VertexType(SurgSim::Math::Vector3d(1.0, 2.0, 3.0)));
+ EXPECT_ANY_THROW(SurgSim::Math::CardinalSplines::extendControlPoints(twoPoints, nullptr));
+ }
+
+ {
+ SurgSim::DataStructures::VerticesPlain twoPoints;
+ twoPoints.addVertex(SurgSim::DataStructures::VerticesPlain::VertexType(SurgSim::Math::Vector3d(0.0, 0.0, 0.0)));
+ twoPoints.addVertex(SurgSim::DataStructures::VerticesPlain::VertexType(SurgSim::Math::Vector3d(1.0, 2.0, 3.0)));
+ std::vector<SurgSim::Math::Vector3d> result;
+ EXPECT_NO_THROW(SurgSim::Math::CardinalSplines::extendControlPoints(twoPoints, &result));
+ EXPECT_EQ(4u, result.size());
+ EXPECT_TRUE(result[0].isApprox(SurgSim::Math::Vector3d(-1.0, -2.0, -3.0)));
+ EXPECT_TRUE(result[3].isApprox(SurgSim::Math::Vector3d(2.0, 4.0, 6.0)));
+ }
+}
+
+TEST(CardinalSplinesTests, interpolate)
+{
+ std::vector<SurgSim::Math::Vector3d> controlPoints;
+ std::vector<SurgSim::Math::Vector3d> points;
+
+ controlPoints.push_back(SurgSim::Math::Vector3d(0.0, 1.0, 2.0));
+ controlPoints.push_back(SurgSim::Math::Vector3d(0.1, 1.1, 2.1));
+ controlPoints.push_back(SurgSim::Math::Vector3d(0.2, 1.2, 2.2));
+
+ // Less than 4 control points.
+ EXPECT_ANY_THROW(SurgSim::Math::CardinalSplines::interpolate(1, controlPoints, &points, 0.5));
+
+ controlPoints.push_back(SurgSim::Math::Vector3d(0.3, 1.3, 2.3));
+ EXPECT_NO_THROW(SurgSim::Math::CardinalSplines::interpolate(0, controlPoints, &points, 0.5));
+ EXPECT_EQ(4u, points.size());
+ auto control = controlPoints.begin();
+ for (auto point : points)
+ {
+ EXPECT_DOUBLE_EQ(point[0], (*control)[0]);
+ EXPECT_DOUBLE_EQ(point[1], (*control)[1]);
+ EXPECT_DOUBLE_EQ(point[2], (*control)[2]);
+ control++;
+ }
+ points.clear();
+
+ EXPECT_ANY_THROW(SurgSim::Math::CardinalSplines::interpolate(1, controlPoints, &points, -0.5));
+ EXPECT_ANY_THROW(SurgSim::Math::CardinalSplines::interpolate(1, controlPoints, &points, 1.5));
+ EXPECT_ANY_THROW(SurgSim::Math::CardinalSplines::interpolate(1, controlPoints, nullptr, 0.5));
+
+ EXPECT_NO_THROW(SurgSim::Math::CardinalSplines::interpolate(10, controlPoints, &points, 0.5));
+ EXPECT_EQ(10u, points.size());
+}
\ No newline at end of file
diff --git a/SurgSim/Math/UnitTests/CompoundShapeTests.cpp b/SurgSim/Math/UnitTests/CompoundShapeTests.cpp
new file mode 100644
index 0000000..652ab15
--- /dev/null
+++ b/SurgSim/Math/UnitTests/CompoundShapeTests.cpp
@@ -0,0 +1,238 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/CompoundShape.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/Shapes.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+#include <gtest/gtest.h>
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+struct CompoundShapeTest : public ::testing::Test
+{
+public:
+
+ virtual void SetUp()
+ {
+ compoundShape = std::make_shared<CompoundShape>();
+ shape1 = std::make_shared<BoxShape>(1.0, 1.0, 1.0);
+ shape2 = std::make_shared<BoxShape>(1.0, 2.0, 1.0);
+
+ transform1 = makeRigidTranslation(Vector3d(-1.0, 0.0, 0.0));
+ transform2 = makeRigidTranslation(Vector3d(1.0, 0.0, 0.0));
+ }
+
+ std::shared_ptr<CompoundShape> compoundShape;
+ std::shared_ptr<Shape> shape1;
+ std::shared_ptr<Shape> shape2;
+
+ RigidTransform3d transform1;
+ RigidTransform3d transform2;
+};
+
+TEST_F(CompoundShapeTest, SimpleShapes)
+{
+ EXPECT_EQ(0u, compoundShape->getNumShapes());
+
+ auto index = compoundShape->addShape(shape1);
+ EXPECT_EQ(0u, index);
+ EXPECT_EQ(1u, compoundShape->getNumShapes());
+
+ index = compoundShape->addShape(shape2, transform2);
+ EXPECT_EQ(1u, index);
+ EXPECT_EQ(2u, compoundShape->getNumShapes());
+
+ EXPECT_EQ(shape1, compoundShape->getShape(0));
+ EXPECT_EQ(shape2, compoundShape->getShape(1));
+ EXPECT_ANY_THROW(compoundShape->getShape(3));
+
+ compoundShape->clearShapes();
+ EXPECT_EQ(0u, compoundShape->getNumShapes());
+}
+
+TEST_F(CompoundShapeTest, Transforms)
+{
+ compoundShape->addShape(shape1);
+ EXPECT_TRUE(RigidTransform3d::Identity().isApprox(compoundShape->getPose(0)));
+
+ compoundShape->addShape(shape2, transform2);
+ EXPECT_TRUE(transform2.isApprox(compoundShape->getPose(1)));
+
+ compoundShape->setPose(0, transform2);
+ EXPECT_TRUE(transform2.isApprox(compoundShape->getPose(0)));
+
+ compoundShape->setPose(1, transform1);
+
+ std::vector<RigidTransform3d> poses;
+
+ EXPECT_ANY_THROW(compoundShape->setPoses(poses));
+ poses.push_back(transform1);
+ poses.push_back(transform2);
+ EXPECT_NO_THROW(compoundShape->setPoses(poses));
+
+ EXPECT_TRUE(transform1.isApprox(compoundShape->getPose(0)));
+ EXPECT_TRUE(transform2.isApprox(compoundShape->getPose(1)));
+}
+
+TEST_F(CompoundShapeTest, Volume)
+{
+ EXPECT_DOUBLE_EQ(0.0, compoundShape->getVolume());
+
+ compoundShape->addShape(shape1);
+ EXPECT_DOUBLE_EQ(1.0, compoundShape->getVolume());
+
+ compoundShape->addShape(shape2, transform2);
+ EXPECT_DOUBLE_EQ(3.0, compoundShape->getVolume());
+}
+
+TEST_F(CompoundShapeTest, Center)
+{
+ Vector3d center = Vector3d::Zero();
+ EXPECT_TRUE(center.isApprox(compoundShape->getCenter()))
+ << "Expected:" << center.transpose()
+ << " Actual: " << compoundShape->getCenter().transpose();
+
+
+ center = transform1.translation();
+ compoundShape->addShape(shape1, transform1);
+ EXPECT_TRUE(center.isApprox(compoundShape->getCenter()))
+ << "Expected:" << center.transpose()
+ << " Actual: " << compoundShape->getCenter().transpose();
+ EXPECT_DOUBLE_EQ(1.0, compoundShape->getVolume());
+
+ center = Vector3d(1.0 - 2.0 / 3.0, 0.0, 0.0);
+ compoundShape->addShape(shape2, transform2);
+ EXPECT_TRUE(center.isApprox(compoundShape->getCenter()))
+ << "Expected:" << center.transpose()
+ << " Actual: " << compoundShape->getCenter().transpose();
+ EXPECT_DOUBLE_EQ(3.0, compoundShape->getVolume());
+}
+
+TEST_F(CompoundShapeTest, SecondMomentOfVolumeBasic)
+{
+ auto zero = Math::Matrix33d::Zero();
+ EXPECT_TRUE(zero.isApprox(compoundShape->getSecondMomentOfVolume()));
+
+ auto box1 = std::make_shared<BoxShape>(1.0, 1.0, 1.0);
+ auto box2 = std::make_shared<BoxShape>(2.0, 1.0, 1.0);
+
+ auto left = makeRigidTranslation(Vector3d(-0.5, 0.0, 0.0));
+ auto right = makeRigidTranslation(Vector3d(0.5, 0.0, 0.0));
+
+ compoundShape->addShape(box1);
+
+ auto shapeInertia = box1->getSecondMomentOfVolume();
+ auto compoundInertia = compoundShape->getSecondMomentOfVolume();
+
+ EXPECT_TRUE(shapeInertia.isApprox(compoundInertia));
+
+ compoundShape->clearShapes();
+ compoundShape->addShape(box1, left);
+ compoundShape->addShape(box1, right);
+
+ shapeInertia = box2->getSecondMomentOfVolume();
+ compoundInertia = compoundShape->getSecondMomentOfVolume();
+
+ EXPECT_TRUE(shapeInertia.isApprox(compoundInertia));
+}
+
+TEST_F(CompoundShapeTest, SecondMomentOfVolumeComplex)
+{
+ // Organisation of shape
+ //
+ // tl Y-Axis
+ // tl ^
+ // bl -> 0, Z-Axis
+ // bl
+ // bl
+
+ auto base = std::make_shared<BoxShape>(1.0, 5.0, 2.0);
+
+ auto l = std::make_shared<BoxShape>(1.0, 5.0, 1.0);
+ compoundShape->addShape(l, makeRigidTranslation(Vector3d(0.0, 0.0, 0.5)));
+
+ auto t = std::make_shared<BoxShape>(2.0, 1.0, 1.0);
+ Quaterniond quat = makeRotationQuaternion(M_PI_2, Vector3d::UnitZ().eval());
+ auto transform = makeRigidTransform(quat, Vector3d(0.0, 1.5, -0.5));
+ compoundShape->addShape(t, transform);
+
+ auto b = std::make_shared<BoxShape>(3.0, 1.0, 1.0);
+ quat = makeRotationQuaternion(-M_PI_2, Vector3d::UnitZ().eval());
+ transform = makeRigidTransform(quat, Vector3d(0.0, -1.0, -0.5));
+ compoundShape->addShape(b, transform);
+
+ auto shapeInertia = base->getSecondMomentOfVolume();
+ auto compoundInertia = compoundShape->getSecondMomentOfVolume();
+ EXPECT_TRUE(shapeInertia.isApprox(compoundInertia));
+}
+
+TEST_F(CompoundShapeTest, Properties)
+{
+ auto box = std::make_shared<BoxShape>(1.0, 5.0, 2.0);
+ std::vector<CompoundShape::SubShape> shapes, result;
+ shapes.emplace_back(shape1, transform1);
+ shapes.emplace_back(box, transform2);
+
+
+ ASSERT_NO_THROW(compoundShape->setValue("Shapes", shapes));
+
+ ASSERT_NO_THROW(result = compoundShape->getValue<std::vector<CompoundShape::SubShape>>("Shapes"));
+
+ // Spot check content
+ EXPECT_EQ(2u, result.size());
+ EXPECT_TRUE(transform2.isApprox(result[1].second));
+
+}
+
+TEST_F(CompoundShapeTest, Serialization)
+{
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = Shape::getFactory().create("SurgSim::Math::CompoundShape"));
+ std::vector<CompoundShape::SubShape> shapes;
+ shapes.emplace_back(shape1, transform1);
+ shapes.emplace_back(shape2, transform2);
+ shape->setValue("Shapes", shapes);
+ 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::CompoundShape"].IsDefined());
+ auto data = node["SurgSim::Math::CompoundShape"];
+ EXPECT_EQ(1u, data.size());
+
+ std::shared_ptr<CompoundShape> compoundShape;
+ ASSERT_NO_THROW(compoundShape = std::dynamic_pointer_cast<CompoundShape>(node.as<std::shared_ptr<Shape>>()));
+ EXPECT_EQ("SurgSim::Math::CompoundShape", compoundShape->getClassName());
+ EXPECT_TRUE(compoundShape->isValid());
+
+ // Check de-serialized data
+ std::vector<CompoundShape::SubShape> result;
+ ASSERT_NO_THROW(result = compoundShape->getValue<std::vector<CompoundShape::SubShape>>("Shapes"));
+ EXPECT_EQ(2u, result.size());
+ EXPECT_TRUE(transform2.isApprox(result[1].second));
+}
+
+}
+}
diff --git a/SurgSim/Math/UnitTests/CubicSolverTests.cpp b/SurgSim/Math/UnitTests/CubicSolverTests.cpp
new file mode 100644
index 0000000..edaf9ac
--- /dev/null
+++ b/SurgSim/Math/UnitTests/CubicSolverTests.cpp
@@ -0,0 +1,185 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file CubicSolverTests.cpp
+/// Tests for the cubic solver function.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/CubicSolver.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+typedef Polynomial<double, 3> CubicPolynomial;
+
+TEST(CubicSolverTests, DegenerateCases)
+{
+ std::array<double, 3> roots;
+ int numberOfRoots;
+
+ {
+ SCOPED_TRACE("0.x^3 + x^2 + x + 1 = 0 (no solution on R: none on [0..1])");
+ EXPECT_NO_THROW(numberOfRoots = findRootsInRange01(CubicPolynomial(1.0, 1.0, 1.0, 0.0), &roots));
+ EXPECT_EQ(0, numberOfRoots);
+ }
+
+ {
+ SCOPED_TRACE("0.x^3 + 1x^2 + 4x - 12 = 0 (2 solutions on R: -6 and 2, none on [0..1])");
+ EXPECT_NO_THROW(numberOfRoots = findRootsInRange01(CubicPolynomial(-12.0, 4.0, 1.0, 0.0), &roots));
+ EXPECT_EQ(0, numberOfRoots);
+ }
+
+ {
+ SCOPED_TRACE("0.x^3 + x^2 + 0x + 0 = 0 (1 solution on R: 0, 1 on [0..1])");
+ EXPECT_NO_THROW(numberOfRoots = findRootsInRange01(CubicPolynomial(0.0, 0.0, 1.0, 0.0), &roots));
+ EXPECT_EQ(1, numberOfRoots);
+ EXPECT_DOUBLE_EQ(0.0, roots[0]);
+ }
+
+ {
+ SCOPED_TRACE("0.x^3 + x^2 + x - 2 = 0 (2 solutions on R: -2 and 1, 1 on [0..1])");
+ EXPECT_NO_THROW(numberOfRoots = findRootsInRange01(CubicPolynomial(-2.0, 1.0, 1.0, 0.0), &roots));
+ EXPECT_EQ(1, numberOfRoots);
+ EXPECT_DOUBLE_EQ(1.0, roots[0]);
+ }
+
+ {
+ SCOPED_TRACE("0.x^3 + 2x^2 + x - 1 = 0 (2 solutions on R: -1 and 0.5, 1 on [0..1])");
+ EXPECT_NO_THROW(numberOfRoots = findRootsInRange01(CubicPolynomial(-1.0, 1.0, 2.0, 0.0), &roots));
+ EXPECT_EQ(1, numberOfRoots);
+ EXPECT_DOUBLE_EQ(0.5, roots[0]);
+ }
+
+};
+
+TEST(CubicSolverTests, dNullCase)
+{
+ std::array<double, 3> roots;
+ int numberOfRoots;
+
+ {
+ SCOPED_TRACE("x^3 + x^2 + x + 0 = 0");
+ EXPECT_NO_THROW(numberOfRoots = findRootsInRange01(CubicPolynomial(0.0, 1.0, 1.0, 1.0), &roots));
+ EXPECT_EQ(1, numberOfRoots);
+ EXPECT_DOUBLE_EQ(0.0, roots[0]);
+ }
+}
+
+TEST(CubicSolverTests, DerivativeNullDeterminantCases)
+{
+ std::array<double, 3> roots;
+ int numberOfRoots;
+
+ {
+ SCOPED_TRACE("P(x) = -x^3 + 3x^2 - 3x + 1 = 0 => P'(x) = -3x^2 + 6x - 3 => discriminant = 36 - 4*(3)*(3) = 0");
+ EXPECT_NO_THROW(numberOfRoots = findRootsInRange01(CubicPolynomial(1.0, -3.0, 3.0, -1.0), &roots));
+ EXPECT_EQ(1, numberOfRoots);
+ EXPECT_DOUBLE_EQ(1.0, roots[0]);
+ }
+
+ {
+ SCOPED_TRACE("P(x) = -x^3 + 3x^2 - 3x - 2 = 0 => P'(x) = -3x^2 + 6x - 3 => discriminant = 36 - 4*(3)*(3) = 0");
+ EXPECT_NO_THROW(numberOfRoots = findRootsInRange01(CubicPolynomial(-2.0, -3.0, 3.0, -1.0), &roots));
+ EXPECT_EQ(0, numberOfRoots);
+ }
+};
+
+TEST(CubicSolverTests, DerivativeNegativeDeterminantCases)
+{
+ std::array<double, 3> roots;
+ int numberOfRoots;
+
+ {
+ SCOPED_TRACE("P(x) = x^3 + x^2 + x + 1 = 0 => P'(x) = 3x^2 + 2x + 1 => discriminant = 4 - 4*(3)*(1) = -8");
+ // P(0) = 1
+ // P(1) = 4
+ // P is monotonic and 0 is not in [P(0)..P(1)], so the unique solution is not within [0..1]
+ EXPECT_NO_THROW(numberOfRoots = findRootsInRange01(CubicPolynomial(1.0, 1.0, 1.0, 1.0), &roots));
+ EXPECT_EQ(0, numberOfRoots);
+ }
+
+ {
+ SCOPED_TRACE("P(x) = x^3 - x^2 + x - 0.5 = 0 => P'(x) = 3x^2 - 2x + 1 => discriminant = 4 - 4*(3)*(1) = -8");
+ // P(0) = -0.5
+ // P(1) = 0.5
+ // P is monotonic and 0 is in [P(0)..P(1)], so the unique solution is within [0..1]
+ CubicPolynomial p(-0.5, 1.0, -1.0, 1.0);
+ EXPECT_NO_THROW(numberOfRoots = findRootsInRange01(p, &roots));
+ EXPECT_EQ(1, numberOfRoots);
+ EXPECT_TRUE(roots[0] >= 0.0 && roots[0] <= 1.0);
+ double eval = p.evaluate(roots[0]);
+ EXPECT_TRUE(isNearZero(eval)) << "P(" << roots[0] << ") = " << eval;
+ }
+};
+
+TEST(CubicSolverTests, DerivativePositiveDeterminantCases)
+{
+ std::array<double, 3> roots;
+ int numberOfRoots;
+
+ {
+ SCOPED_TRACE("P(x) = -x^3 + x^2 + x + 1 = 0 => P'(x) = -3x^2 + 2x + 1 => discriminant = 4 - 4*(-3)*(1) = 16");
+ // P' has 2 roots: -1/3 and 1
+ // P'(-1/3) = 0 P'(1) = 0
+ // P (-1/3) = 1 > -1/27 + 1/9 - 1/3 + 1 > 0 P(0) = 1 P (1) = 2
+ // P is monotonic in 3 intervals [-Inf -1/3[, [-1/3 1] and ]1 +Inf[
+ // Therefore P is monotonic in [0..1] and above 0 on this interval, so there is no root in [0..1]
+ EXPECT_NO_THROW(numberOfRoots = findRootsInRange01(CubicPolynomial(1.0, 1.0, 1.0, -1.0), &roots));
+ EXPECT_EQ(0, numberOfRoots);
+ }
+
+ {
+ SCOPED_TRACE("P(x) = -x^3 + x^2 + x - 0.5 = 0 => P'(x) = -3x^2 + 2x + 1 => discriminant = 4 - 4*(-3)*(1) = 16");
+ // P' has 2 roots: -1/3 and 1
+ // P'(-1/3) = 0 P'(1) = 0
+ // P (-1/3) = -1/27 + 1/9 - 1/3 -0.5 < 0 P(0) = -0.5 P (1) = 0.5 > 0
+ // P is monotonic in 3 intervals [-Inf -1/3[, [-1/3 1] and ]1 +Inf[
+ // Therefore P is monotonic in [0..1] and cross 0 on this interval, so there is 1 root in [0..1]
+ CubicPolynomial p(-0.5, 1.0, 1.0, -1.0);
+ EXPECT_NO_THROW(numberOfRoots = findRootsInRange01(p, &roots));
+ EXPECT_EQ(1, numberOfRoots);
+ EXPECT_TRUE(roots[0] >= 0.0 && roots[0] <= 1.0);
+ double eval = p.evaluate(roots[0]);
+ EXPECT_TRUE(isNearZero(eval)) << "P(" << roots[0] << ") = " << eval;
+ }
+
+ {
+ // From a real use case of point/triangle continuous collision detection
+ SCOPED_TRACE("P(x) = 7.84x^3 - 37.08x^2 + 19.5x - 1.46 = 0 => P'(x) = 23.52x^2 - 74.16x + 19.5");
+ // P' has 2 roots: x1 = (74.16 - 60.540445984482142729500176673066) / 47.04 = 0.2895313353639000270089248156236
+ // and x2 = (74.16 + 60.540445984482142729500176673066) / 47.04 = 2.8635298891258958913584221231519
+ // P is monotonic in 3 intervals [-Inf x1[, [x1 x2] and ]x2 +Inf[
+ // P'(0) > 0 ; P'(x1) = 0 ; P'(1) = -31.14 ; P'(x2) = 0
+ // P(0) = -1.46 < 0 ; P (x1) = 1.26 > 0 ; P (1) = -11.2 < 0 ; P(x2) = -65.58
+ // Therefore P is monotonic and has a solution in [0..x1] and [x1 1]
+ CubicPolynomial p(-1.46, 19.5, -37.08, 7.84);
+ EXPECT_NO_THROW(numberOfRoots = findRootsInRange01(p, &roots));
+ EXPECT_EQ(2, numberOfRoots);
+ EXPECT_TRUE(roots[0] >= 0.0 && roots[0] <= 1.0);
+ EXPECT_TRUE(roots[1] >= 0.0 && roots[1] <= 1.0);
+ EXPECT_TRUE(roots[1] > roots[0]);
+ double eval = p.evaluate(roots[0]);
+ EXPECT_TRUE(isNearZero(eval)) << "P(" << roots[0] << ") = " << eval;
+ eval = p.evaluate(roots[1]);
+ EXPECT_TRUE(isNearZero(eval)) << "P(" << roots[1] << ") = " << eval;
+ }
+};
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/UnitTests/Data/segmentmesh.ply b/SurgSim/Math/UnitTests/Data/segmentmesh.ply
new file mode 100644
index 0000000..1fe0582
--- /dev/null
+++ b/SurgSim/Math/UnitTests/Data/segmentmesh.ply
@@ -0,0 +1,21 @@
+ply
+format ascii 1.0
+comment Created by hand
+element vertex 4
+property double x
+property double y
+property double z
+element 1d_element 3
+property list uint uint vertex_indices
+element radius 1
+property double value
+end_header
+1 2 3
+4 5 6
+7 8 9
+11 11 12
+2 0 1
+2 1 2
+2 2 3
+1.234
+
diff --git a/SurgSim/Math/UnitTests/GeometryTests.cpp b/SurgSim/Math/UnitTests/GeometryTests.cpp
index 1fd511d..151e02a 100644
--- a/SurgSim/Math/UnitTests/GeometryTests.cpp
+++ b/SurgSim/Math/UnitTests/GeometryTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -144,6 +144,38 @@ protected:
};
+TEST_F(GeometryTest, BaryCentricOfSegment)
+{
+ typedef Eigen::Matrix<SizeType, 2, 1> Vector2;
+
+ Vector2 outputPoint;
+ EXPECT_TRUE(barycentricCoordinates(plainSegment.a, plainSegment.a, plainSegment.b, &outputPoint));
+ EXPECT_TRUE(Vector2(1.0, 0.0).isApprox(outputPoint));
+
+ EXPECT_TRUE(barycentricCoordinates(plainSegment.b, plainSegment.a, plainSegment.b, &outputPoint));
+ EXPECT_TRUE(Vector2(0.0, 1.0).isApprox(outputPoint));
+
+ // Halfway points
+ EXPECT_TRUE(barycentricCoordinates(plainSegment.pointOnLine(0.5), plainSegment.a, plainSegment.b, &outputPoint));
+ EXPECT_TRUE(Vector2(0.5, 0.5).isApprox(outputPoint));
+
+ // Random point
+ EXPECT_TRUE(barycentricCoordinates(plainSegment.pointOnLine(0.327), plainSegment.a, plainSegment.b, &outputPoint));
+ EXPECT_TRUE(Vector2(1.0 - 0.327, 0.327).isApprox(outputPoint));
+
+ // Point not on line
+ VectorType orthogonal =
+ plainSegment.ab.cross(VectorType(plainSegment.ab[0] * 0.5, plainSegment.ab[1] * 0.6, plainSegment.ab[2] * 0.7));
+ VectorType point = plainSegment.pointOnLine(0.486) + orthogonal;
+ EXPECT_TRUE(barycentricCoordinates(point, plainSegment.a, plainSegment.b, &outputPoint));
+ EXPECT_TRUE(Vector2(1.0 - 0.486, 0.486).isApprox(outputPoint));
+
+ // Degenerate
+ EXPECT_FALSE(barycentricCoordinates(degenerateSegment.a, degenerateSegment.a, degenerateSegment.b, &outputPoint));
+ EXPECT_TRUE(boost::math::isnan(outputPoint[0]) && boost::math::isnan(outputPoint[1]));
+}
+
+
TEST_F(GeometryTest, BaryCentricWithNormal)
{
// Order of Points is v0,v1,v2
@@ -224,10 +256,10 @@ TEST_F(GeometryTest, BaryCentricWithoutNormal)
EXPECT_TRUE(eigenEqual(VectorType(0.2, 0.25, 0.55), outputPoint));
// Degenerate
- EXPECT_FALSE(barycentricCoordinates(inputPoint, tri.v1, tri.v1, tri.v2, &outputPoint));
+ EXPECT_FALSE(barycentricCoordinates(inputPoint, tri.v0, tri.v0, tri.v2, &outputPoint));
EXPECT_TRUE(eigenAllNan(outputPoint));
- EXPECT_FALSE(barycentricCoordinates(inputPoint, tri.v0, tri.v0, tri.v2, &outputPoint));
+ EXPECT_FALSE(barycentricCoordinates(inputPoint, tri.v0, tri.v1, tri.v1, &outputPoint));
EXPECT_TRUE(eigenAllNan(outputPoint));
EXPECT_FALSE(barycentricCoordinates(inputPoint, tri.v2, tri.v1, tri.v2, &outputPoint));
@@ -563,11 +595,11 @@ TEST_F(GeometryTest, DistanceSegmentSegment)
// <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 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));
+ shortSegment.pointOnLine(0.5), otherShortSegment.a));
for (size_t i = 0; i < segments.size(); ++i)
{
@@ -586,7 +618,7 @@ TEST_F(GeometryTest, DistanceSegmentSegment)
closestPoint = plainSegment.a;
const Vector3d segmentDirection = plainSegment.a - plainSegment.b;
otherSegment = Segment(closestPoint + plainNormal * 4 + 2 * segmentDirection,
- closestPoint + plainNormal * 4 + 4 * 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));
@@ -598,7 +630,7 @@ TEST_F(GeometryTest, DistanceSegmentSegment)
// <1> anti-parallel, non-overlapping
closestPoint = plainSegment.a;
otherSegment = Segment(closestPoint + plainNormal * 4 + 4 * segmentDirection,
- closestPoint + plainNormal * 4 + 2 * 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));
@@ -721,16 +753,16 @@ TEST_F(GeometryTest, DistancePointTriangle)
// Degenerate Edges
// Edge v0v1
distance = distancePointTriangle(inputPoint,
- tri.v0, (tri.v0 + tri.v0v1 * epsilon * 0.01).eval(), tri.v2,
- &result);
+ 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);
+ (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));
@@ -781,6 +813,49 @@ TEST_F(GeometryTest, PointInsideTriangleWithoutNormal)
EXPECT_FALSE(isPointInsideTriangle(inputPoint, tri.v0, tri.v1, tri.v2));
}
+TEST_F(GeometryTest, PointOnTriangleEdgeWithNormal)
+{
+ EXPECT_TRUE(isPointOnTriangleEdge(tri.v0, tri.v0, tri.v1, tri.v2, tri.n));
+ EXPECT_TRUE(isPointOnTriangleEdge(tri.v1, tri.v0, tri.v1, tri.v2, tri.n));
+ EXPECT_TRUE(isPointOnTriangleEdge(tri.v2, tri.v0, tri.v1, tri.v2, tri.n));
+
+ VectorType inputPoint = tri.v0 + tri.v0v1 * 0.2;
+ EXPECT_TRUE(isPointOnTriangleEdge(inputPoint, tri.v0, tri.v1, tri.v2, tri.n));
+ inputPoint = tri.v0 + tri.v0v2 * 0.2;
+ EXPECT_TRUE(isPointOnTriangleEdge(inputPoint, tri.v0, tri.v1, tri.v2, tri.n));
+ inputPoint = tri.v1 + tri.v1v2 * 0.2;
+ EXPECT_TRUE(isPointOnTriangleEdge(inputPoint, tri.v0, tri.v1, tri.v2, tri.n));
+
+ inputPoint = tri.v0 + tri.v0v1 * 1.5;
+ EXPECT_FALSE(isPointOnTriangleEdge(inputPoint, tri.v0, tri.v1, tri.v2, tri.n));
+ EXPECT_FALSE(isPointOnTriangleEdge(inputPoint, tri.v1, tri.v1, tri.v2, tri.n));
+
+ inputPoint = tri.v0 + tri.v0v2 * 2 + tri.v0v1 * 2;
+ EXPECT_FALSE(isPointOnTriangleEdge(inputPoint, tri.v0, tri.v1, tri.v2, tri.n));
+
+}
+
+TEST_F(GeometryTest, PointOnTriangleEdgeWithoutNormal)
+{
+ EXPECT_TRUE(isPointOnTriangleEdge(tri.v0, tri.v0, tri.v1, tri.v2));
+ EXPECT_TRUE(isPointOnTriangleEdge(tri.v1, tri.v0, tri.v1, tri.v2));
+ EXPECT_TRUE(isPointOnTriangleEdge(tri.v2, tri.v0, tri.v1, tri.v2));
+
+ VectorType inputPoint = tri.v0 + tri.v0v1 * 0.2;
+ EXPECT_TRUE(isPointOnTriangleEdge(inputPoint, tri.v0, tri.v1, tri.v2));
+ inputPoint = tri.v0 + tri.v0v2 * 0.2;
+ EXPECT_TRUE(isPointOnTriangleEdge(inputPoint, tri.v0, tri.v1, tri.v2));
+ inputPoint = tri.v1 + tri.v1v2 * 0.2;
+ EXPECT_TRUE(isPointOnTriangleEdge(inputPoint, tri.v0, tri.v1, tri.v2));
+
+ inputPoint = tri.v0 + tri.v0v1 * 1.5;
+ EXPECT_FALSE(isPointOnTriangleEdge(inputPoint, tri.v0, tri.v1, tri.v2));
+ EXPECT_FALSE(isPointOnTriangleEdge(inputPoint, tri.v1, tri.v1, tri.v2));
+
+ inputPoint = tri.v0 + tri.v0v2 * 2 + tri.v0v1 * 2;
+ EXPECT_FALSE(isPointOnTriangleEdge(inputPoint, tri.v0, tri.v1, tri.v2));
+}
+
TEST_F(GeometryTest, Coplanarity)
{
struct CoplanarityTestCandidate
@@ -805,22 +880,24 @@ TEST_F(GeometryTest, Coplanarity)
{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)}
+ {
+ 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() << "]";
+ 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() << "]";
}
}
@@ -1027,6 +1104,45 @@ TEST_F(GeometryTest, SegmentPlaneDistance)
}
}
+template <class T>
+class GeometryVectorTestBase : public testing::Test
+{
+public:
+ typedef T Scalar;
+};
+
+template <class T>
+class GeometryVector3Tests : public GeometryVectorTestBase<typename T::Scalar>
+{
+public:
+ typedef T Vector3;
+};
+
+typedef ::testing::Types<SurgSim::Math::Vector3d,
+ SurgSim::Math::Vector3f> GeometryVector3Variants;
+TYPED_TEST_CASE(GeometryVector3Tests, GeometryVector3Variants);
+
+TYPED_TEST(GeometryVector3Tests, nearestPointOnLine)
+{
+ typedef typename TestFixture::Vector3 Vector3;
+ typedef typename Vector3::Scalar T;
+ const int VOpt = Vector3::Options;
+
+ Vector3 point(static_cast<T>(2.0), static_cast<T>(-4.0), static_cast<T>(3.0));
+ Vector3 segmentEnd1(static_cast<T>(-2.0), static_cast<T>(2.0), static_cast<T>(4.0));
+ Vector3 segmentEnd2(static_cast<T>(-1.0), static_cast<T>(1.0), static_cast<T>(2.0));
+ T precision = Eigen::NumTraits<T>::dummy_precision();
+
+ // Assert if segment is degenerate
+ ASSERT_ANY_THROW((SurgSim::Math::nearestPointOnLine<T, VOpt>(point, segmentEnd1, segmentEnd1)));
+
+ // Otherwise, calculate the correct value
+ ASSERT_NO_THROW((SurgSim::Math::nearestPointOnLine<T, VOpt>(point, segmentEnd1, segmentEnd2)));
+
+ auto result = SurgSim::Math::nearestPointOnLine<T, VOpt>(point, segmentEnd1, segmentEnd2);
+ EXPECT_GT(precision, (std::abs((segmentEnd2 - segmentEnd1).dot(result - point))));
+}
+
typedef std::tuple<MockTriangle, VectorType, double, VectorType, VectorType, int> TriPlaneData;
void checkTriPlaneDistance(const TriPlaneData& data)
{
@@ -1041,7 +1157,7 @@ void checkTriPlaneDistance(const TriPlaneData& data)
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((sign == 0 && std::abs(distance) <= epsilon) || (distance * sign > 0));
EXPECT_TRUE(expectedTrianglePoint.isApprox(triangleResultPoint));
EXPECT_TRUE(expectedPlanePoint.isApprox(planeResultPoint));
}
@@ -1559,5 +1675,234 @@ TEST_F(GeometryTest, DoesIntersectBoxCapsule)
}
}
+TEST_F(GeometryTest, TimesOfCoplanarity)
+{
+ std::array<SizeType, 3> times;
+
+ {
+ SCOPED_TRACE("No coplanarity case in [0..1]");
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(0.0, 0.0, 0.0), VectorType(1.0, 0.0, 1.0));
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(1.0, 0.0, 2.0), VectorType(2.0, 0.0, 4.0));
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(0.0, 0.1, 10.0), VectorType(0.0, 1.1, 10.0));
+ std::pair<VectorType, VectorType> D = std::make_pair(VectorType(0.0, 1.1, 10.0), VectorType(0.0, 2.1, 10.0));
+ EXPECT_EQ(0, (timesOfCoplanarityInRange01<double, Vector3d::Options>(A, B, C, D, ×)));
+ }
+
+ {
+ SCOPED_TRACE("1 case of coplanarity at time 0");
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(0.0, 0.0, 0.0), VectorType(1.0, 0.0, 1.0));
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(1.0, 0.0, 2.0), VectorType(2.0, 0.0, 4.0));
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(0.0, 1.0, -2.0), VectorType(0.0, 2.0, -3.5));
+ std::pair<VectorType, VectorType> D = std::make_pair(VectorType(0.0, 0.0, 0.0), VectorType(0.0, 1.0, -10.0));
+ EXPECT_EQ(1, (timesOfCoplanarityInRange01<double, Vector3d::Options>(A, B, C, D, ×)));
+ EXPECT_DOUBLE_EQ(0.0, times[0]);
+ }
+
+ {
+ SCOPED_TRACE("1 case of coplanarity at time 0.5");
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(0.1, 0.0, 1.0), VectorType(-0.1, 0.0, -1.0));
+ // A(0.5) = 0.0 0.0 0.0
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(1.0, 0.0, 2.0), VectorType(2.0, 3.0, -2.0));
+ // B(0.5) = 1.5 1.5 0.0
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(-0.3, 2.0, -2.0), VectorType(0.3, 1.0, 2.0));
+ // C(0.5) = 0.0 1.5 0.0
+ std::pair<VectorType, VectorType> D = std::make_pair(VectorType(1.7, 1.0, -1.0), VectorType(1.3, -1.0, 1.0));
+ // D(0.5) = 1.5 0.0 0.0
+ EXPECT_EQ(1, (timesOfCoplanarityInRange01<double, Vector3d::Options>(A, B, C, D, ×)));
+ EXPECT_NEAR(0.5, times[0], Math::Geometry::ScalarEpsilon);
+ }
+
+ {
+ SCOPED_TRACE("2 cases of coplanarity at times 0.089971778657590915 and 0.5");
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(0.4, 0.8, 1.0), VectorType(0.8, 1.0, -1.0));
+ // A(0.5) = 0.6 0.9 0.0
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(1.0, 0.0, 2.0), VectorType(2.0, 3.0, -2.0));
+ // B(0.5) = 1.5 1.5 0.0
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(-0.3, 2.0, -2.0), VectorType(0.3, 1.0, 2.0));
+ // C(0.5) = 0.0 1.5 0.0
+ std::pair<VectorType, VectorType> D = std::make_pair(VectorType(1.7, 1.0, -1.0), VectorType(1.3, -1.0, 1.0));
+ // D(0.5) = 1.5 0.0 0.0
+ EXPECT_EQ(2, (timesOfCoplanarityInRange01<double, Vector3d::Options>(A, B, C, D, ×)));
+ EXPECT_NEAR(0.089971778657590915, times[0], Math::Geometry::ScalarEpsilon);
+ EXPECT_NEAR(0.5, times[1], Math::Geometry::ScalarEpsilon);
+ }
+
+ {
+ SCOPED_TRACE("1 case of coplanarity at time 1");
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(0.0, 0.0, 0.0), VectorType(1.0, 0.0, 1.0));
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(1.0, 0.0, 1.24), VectorType(2.0, 0.0, 3.0));
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(0.0, 1.0, -2.0), VectorType(2.0, 0.0, 3.0));
+ std::pair<VectorType, VectorType> D = std::make_pair(VectorType(0.1, 1.0, 1.1), VectorType(0.0, 1.0, -1.54));
+ EXPECT_EQ(1, (timesOfCoplanarityInRange01<double, Vector3d::Options>(A, B, C, D, ×)));
+ EXPECT_NEAR(1.0, times[0], Math::Geometry::ScalarEpsilon);
+ }
+}
+
+TEST_F(GeometryTest, CcdIntersectionsSegmentSegment)
+{
+ SizeType time, s0p1Factor, s1p1Factor;
+
+ {
+ SCOPED_TRACE("No intersection");
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(0.0, 0.0, 0.0), VectorType(1.0, 0.0, 1.0));
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(1.0, 0.0, 2.0), VectorType(2.0, 0.0, 4.0));
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(0.0, 0.1, 4.0), VectorType(0.0, 1.1, 8.0));
+ std::pair<VectorType, VectorType> D = std::make_pair(VectorType(0.0, 1.1, 6.0), VectorType(0.0, 2.1, 54.0));
+ EXPECT_FALSE(calculateCcdContactSegmentSegment(A, B, C, D, &time, &s0p1Factor, &s1p1Factor));
+ }
+
+ {
+ SCOPED_TRACE("Intersection at t=0");
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(0.0, 0.0, 0.0), VectorType(1.0, 0.0, 1.0));
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(1.0, 0.0, 2.0), VectorType(2.0, 0.0, 4.0));
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(0.0, 1.0, -2.0), VectorType(0.0, 2.0, -3.5));
+ std::pair<VectorType, VectorType> D = std::make_pair(VectorType(0.0, 0.0, 0.0), VectorType(0.0, 1.0, -10.0));
+ EXPECT_TRUE(calculateCcdContactSegmentSegment(A, B, C, D, &time, &s0p1Factor, &s1p1Factor));
+ EXPECT_DOUBLE_EQ(0.0, time);
+ EXPECT_DOUBLE_EQ(0.0, s0p1Factor);
+ EXPECT_DOUBLE_EQ(1.0, s1p1Factor);
+ }
+
+ {
+ SCOPED_TRACE("Intersection at t=0.5");
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(0.1, 0.0, 1.0), VectorType(-0.1, 0.0, -1.0));
+ // A(0.5) = 0.0 0.0 0.0
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(1.0, 0.0, 2.0), VectorType(2.0, 3.0, -2.0));
+ // B(0.5) = 1.5 1.5 0.0
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(-0.3, 2.0, -2.0), VectorType(0.3, 1.0, 2.0));
+ // C(0.5) = 0.0 1.5 0.0
+ std::pair<VectorType, VectorType> D = std::make_pair(VectorType(1.7, 1.0, -1.0), VectorType(1.3, -1.0, 1.0));
+ // D(0.5) = 1.5 0.0 0.0
+ // At time 0.5, the segments AB and CD are coplanar and intersect exactly in their middle
+ EXPECT_TRUE(calculateCcdContactSegmentSegment(A, B, C, D, &time, &s0p1Factor, &s1p1Factor));
+ EXPECT_NEAR(0.5, time, Math::Geometry::ScalarEpsilon);
+ EXPECT_NEAR(0.5, s0p1Factor, Math::Geometry::ScalarEpsilon);
+ EXPECT_NEAR(0.5, s1p1Factor, Math::Geometry::ScalarEpsilon);
+ }
+
+ {
+ SCOPED_TRACE("Intersection at t=0.5");
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(0.4, 0.0, 1.0), VectorType(0.6, 0.0, -1.0));
+ // A(0.5) = 0.5 0.0 0.0
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(1.0, 0.0, 2.0), VectorType(2.0, 3.0, -2.0));
+ // B(0.5) = 1.5 1.5 0.0
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(-0.3, 2.0, -2.0), VectorType(0.3, 1.0, 2.0));
+ // C(0.5) = 0.0 1.5 0.0
+ std::pair<VectorType, VectorType> D = std::make_pair(VectorType(1.7, 1.0, -1.0), VectorType(1.3, -1.0, 1.0));
+ // D(0.5) = 1.5 0.0 0.0
+ // At time 0.5, the segments AB and CD are coplanar and intersect at P:
+ // P = A + 0.4 AB = (0.5 0.0 0.0) + 0.4 (1.0 1.5 0.0) = (0.9 0.6 0.0)
+ // P = C + 0.6 CD = (0.0 1.5 0.0) + 0.6 (1.5 -1.5 0.0) = (0.9 0.6 0.0)
+ EXPECT_TRUE(calculateCcdContactSegmentSegment(A, B, C, D, &time, &s0p1Factor, &s1p1Factor));
+ EXPECT_NEAR(0.5, time, Math::Geometry::ScalarEpsilon);
+ EXPECT_NEAR(0.4, s0p1Factor, Math::Geometry::ScalarEpsilon);
+ EXPECT_NEAR(0.6, s1p1Factor, Math::Geometry::ScalarEpsilon);
+ }
+
+ {
+ SCOPED_TRACE("Intersection at t=0.5 (2 cubic roots in [0..1]: 0.0899 (no collision) and 0.5 (collision))");
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(0.4, 0.8, 1.0), VectorType(0.8, 1.0, -1.0));
+ // A(0.5) = 0.6 0.9 0.0
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(1.0, 0.0, 2.0), VectorType(2.0, 3.0, -2.0));
+ // B(0.5) = 1.5 1.5 0.0
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(-0.3, 2.0, -2.0), VectorType(0.3, 1.0, 2.0));
+ // C(0.5) = 0.0 1.5 0.0
+ std::pair<VectorType, VectorType> D = std::make_pair(VectorType(1.7, 1.0, -1.0), VectorType(1.3, -1.0, 1.0));
+ // D(0.5) = 1.5 0.0 0.0
+ // At time 0.5, all 4 points are coplanar and AB/CD intersect:
+ // P = A + 0*AB = (0.6 0.9 0.0)
+ // P = C + 0.4*CD = (0.0 1.5 0.0) + 0.4 (1.5 -1.5 0.0) = (0.6 0.9 0.0)
+ EXPECT_TRUE(calculateCcdContactSegmentSegment(A, B, C, D, &time, &s0p1Factor, &s1p1Factor));
+ EXPECT_NEAR(0.5, time, Math::Geometry::ScalarEpsilon);
+ EXPECT_NEAR(0.0, s0p1Factor, Math::Geometry::ScalarEpsilon);
+ EXPECT_NEAR(0.4, s1p1Factor, Math::Geometry::ScalarEpsilon);
+ }
+
+ {
+ SCOPED_TRACE("Intersection at t=1");
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(0.0, 0.0, 0.0), VectorType(1.0, 0.0, 1.0));
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(1.0, 0.0, 1.24), VectorType(2.0, 0.0, 3.0));
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(0.0, 1.0, -2.0), VectorType(2.0, 0.0, 3.0));
+ std::pair<VectorType, VectorType> D = std::make_pair(VectorType(0.1, 1.0, 1.1), VectorType(0.0, 1.0, -1.54));
+ EXPECT_TRUE(calculateCcdContactSegmentSegment(A, B, C, D, &time, &s0p1Factor, &s1p1Factor));
+ EXPECT_DOUBLE_EQ(1.0, time);
+ EXPECT_DOUBLE_EQ(1.0, s0p1Factor);
+ EXPECT_DOUBLE_EQ(0.0, s1p1Factor);
+ }
+}
+
+TEST_F(GeometryTest, CcdIntersectionsPointTriangle)
+{
+ SizeType time, tv01Factor, tv02Factor;
+
+ {
+ SCOPED_TRACE("No intersection");
+ std::pair<VectorType, VectorType> P = std::make_pair(VectorType(0.0, 0.0, 0.0), VectorType(1.0, 0.0, 1.0));
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(1.0, 0.0, 2.0), VectorType(2.0, 0.0, 4.0));
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(0.0, 0.1, 4.0), VectorType(0.0, 1.1, 8.0));
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(0.0, 1.1, 6.0), VectorType(0.0, 2.1, 54.0));
+ EXPECT_FALSE(calculateCcdContactPointTriangle(P, A, B, C, &time, &tv01Factor, &tv02Factor));
+ }
+
+ {
+ SCOPED_TRACE("Intersection at t=0");
+ std::pair<VectorType, VectorType> P = std::make_pair(VectorType(0.0, 0.0, 0.0), VectorType(1.0, 0.0, 1.0));
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(1.0, 0.0, 2.0), VectorType(2.0, 0.0, 4.0));
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(0.0, 1.0, -2.0), VectorType(0.0, 2.0, -3.5));
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(0.0, 0.0, 0.0), VectorType(0.0, 1.0, -10.0));
+ EXPECT_TRUE(calculateCcdContactPointTriangle(P, A, B, C, &time, &tv01Factor, &tv02Factor));
+ EXPECT_DOUBLE_EQ(0.0, time);
+ EXPECT_DOUBLE_EQ(0.0, tv01Factor);
+ EXPECT_DOUBLE_EQ(1.0, tv02Factor);
+ }
+
+ {
+ SCOPED_TRACE("Intersection at t=0.5");
+ std::pair<VectorType, VectorType> P = std::make_pair(VectorType(1.1, 0.0, 1.0), VectorType(0.9, 2.0, -1.0));
+ // P(0.5) = 1.0 1.0 0.0
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(1.0, 0.0, 2.0), VectorType(2.0, 3.0, -2.0));
+ // A(0.5) = 1.5 1.5 0.0
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(-0.3, 2.0, -2.0), VectorType(0.3, 1.0, 2.0));
+ // B(0.5) = 0.0 1.5 0.0
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(1.7, 1.0, -1.0), VectorType(1.3, -1.0, 1.0));
+ // C(0.5) = 1.5 0.0 0.0
+ // At time 0.5, all 4 points are coplanar and P is inside ABC with the barycentric coordinates (1/3 1/3 1/3)
+ EXPECT_TRUE(calculateCcdContactPointTriangle(P, A, B, C, &time, &tv01Factor, &tv02Factor));
+ EXPECT_NEAR(0.5, time, Math::Geometry::ScalarEpsilon);
+ EXPECT_NEAR(1.0 / 3.0, tv01Factor, Math::Geometry::ScalarEpsilon);
+ EXPECT_NEAR(1.0 / 3.0, tv01Factor, Math::Geometry::ScalarEpsilon);
+ }
+
+ {
+ SCOPED_TRACE("Intersection at t=0.5 (2 cubic roots in [0..1]: 0.0899 (no collision) and 0.5 (collision))");
+ std::pair<VectorType, VectorType> P = std::make_pair(VectorType(0.4, 0.8, 1.0), VectorType(0.8, 1.0, -1.0));
+ // P(0.5) = 0.6 0.9 0.0
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(1.0, 0.0, 2.0), VectorType(2.0, 3.0, -2.0));
+ // A(0.5) = 1.5 1.5 0.0
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(-0.3, 2.0, -2.0), VectorType(0.3, 1.0, 2.0));
+ // B(0.5) = 0.0 1.5 0.0
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(1.7, 1.0, -1.0), VectorType(1.3, -1.0, 1.0));
+ // C(0.5) = 1.5 0.0 0.0
+ // At time 0.5, all 4 points are coplanar and P is inside ABC with the barycentric coordinates (0 0.6 0.4)
+ // P = A + 0.6 AB + 0.4 AC = (1.5 1.5 0.0) + 0.6 (-1.5 0.0 0.0) + 0.4 (0.0 -1.5 0.0) = (0.6 0.9 0.0)
+ EXPECT_TRUE(calculateCcdContactPointTriangle(P, A, B, C, &time, &tv01Factor, &tv02Factor));
+ EXPECT_NEAR(0.5, time, Math::Geometry::ScalarEpsilon);
+ EXPECT_NEAR(0.6, tv01Factor, Math::Geometry::ScalarEpsilon);
+ EXPECT_NEAR(0.4, tv02Factor, Math::Geometry::ScalarEpsilon);
+ }
+
+ {
+ SCOPED_TRACE("Intersection at t=1");
+ std::pair<VectorType, VectorType> P = std::make_pair(VectorType(1.0, 0.0, 1.24), VectorType(2.0, 0.0, 3.0));
+ std::pair<VectorType, VectorType> A = std::make_pair(VectorType(0.0, 0.0, 0.0), VectorType(1.0, 0.0, 1.0));
+ std::pair<VectorType, VectorType> B = std::make_pair(VectorType(0.0, 1.0, -2.0), VectorType(2.0, 0.0, 3.0));
+ std::pair<VectorType, VectorType> C = std::make_pair(VectorType(0.1, 1.0, 1.1), VectorType(0.0, 1.0, -1.54));
+ EXPECT_TRUE(calculateCcdContactPointTriangle(P, A, B, C, &time, &tv01Factor, &tv02Factor));
+ EXPECT_DOUBLE_EQ(1.0, time);
+ EXPECT_DOUBLE_EQ(1.0, tv01Factor);
+ EXPECT_DOUBLE_EQ(0.0, tv02Factor);
+ }
+}
+
}; // namespace Math
}; // namespace SurgSim
diff --git a/SurgSim/Math/UnitTests/IntervalArithmeticTests.cpp b/SurgSim/Math/UnitTests/IntervalArithmeticTests.cpp
new file mode 100644
index 0000000..e834041
--- /dev/null
+++ b/SurgSim/Math/UnitTests/IntervalArithmeticTests.cpp
@@ -0,0 +1,734 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 IntervalArithmetic functions.
+
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/IntervalArithmetic.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+namespace
+{
+double epsilon = 1.0e-10;
+double delta = 1.0e-11;
+}
+
+class IntervalArithmeticTests : public ::testing::Test
+{
+public:
+ typedef IntervalND<double, 2> IntervalDouble2;
+ typedef IntervalND<double, 3> IntervalDouble3;
+
+ Interval<double> testIntervalMoveConstructor(Interval<double> dummy)
+ {
+ Interval<double> ret;
+ ret = dummy;
+ return ret;
+ }
+
+ IntervalND<double, 2> testIntervalNDMoveConstructor(IntervalND<double, 2> dummy)
+ {
+ IntervalND<double, 2> ret;
+ ret = dummy;
+ return ret;
+ }
+
+ IntervalND<double, 3> testInterval3DMoveConstructor(IntervalND<double, 3> dummy)
+ {
+ IntervalND<double, 3> ret;
+ ret = dummy;
+ return ret;
+ }
+};
+
+TEST_F(IntervalArithmeticTests, IntervalInitializationTests)
+{
+ // Constructor tests
+ EXPECT_NO_THROW(Interval<double> a);
+ EXPECT_THROW(Interval<double> a(3.8, 3.7), SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(Interval<double> a(3.7, 3.8));
+ Interval<double> b(9.8, 100);
+ EXPECT_NO_THROW(Interval<double> a(b));
+ Interval<double> a(b);
+ EXPECT_TRUE(a == b);
+ EXPECT_NO_THROW(testIntervalMoveConstructor(b));
+ Interval<double> f = testIntervalMoveConstructor(b);
+ EXPECT_TRUE(f == b);
+
+ // Assignment and equal/not equal tests
+ Interval<double> c;
+ EXPECT_TRUE(c != b);
+ EXPECT_FALSE(c == b);
+ c = a;
+ EXPECT_FALSE(c != b);
+ EXPECT_TRUE(c == b);
+ Interval<double> e;
+ e = (b * 1);
+ EXPECT_TRUE(e == b);
+ Interval<double> ePlusEpsilon = e + delta;
+ Interval<double> ePlus2Epsilon = e + (2 * epsilon);
+ EXPECT_TRUE(e.isApprox(ePlusEpsilon, epsilon));
+ EXPECT_TRUE(ePlusEpsilon.isApprox(e, epsilon));
+ EXPECT_FALSE(e.isApprox(ePlus2Epsilon, epsilon));
+ EXPECT_FALSE(ePlus2Epsilon.isApprox(e, epsilon));
+
+ // Reordering initializers
+ EXPECT_TRUE(b == SurgSim::Math::Interval<double>::minToMax(100, 9.8));
+ EXPECT_TRUE(b == SurgSim::Math::Interval<double>::minToMax(12, 100, 9.8));
+ EXPECT_TRUE(b == SurgSim::Math::Interval<double>::minToMax(12, 100, 9.8, 99));
+
+ // Get upper and lower
+ EXPECT_EQ(-5.0, Interval<double> (-5.0, 37).getMin());
+ EXPECT_EQ(37.0, Interval<double> (-5.0, 37).getMax());
+};
+
+TEST_F(IntervalArithmeticTests, RangeTests)
+{
+ Interval<double> middle(10.0, 20.0);
+ Interval<double> low(5.0, 10 - epsilon);
+ Interval<double> high(20 + epsilon, 40.0);
+ Interval<double> lowOverlap(5.0, 10.0);
+ Interval<double> highOverlap(20.0, 40.0);
+ Interval<double> contained(12.0, 15.0);
+ Interval<double> contains(9.0, 21.0);
+
+ // Overlap
+ EXPECT_FALSE(middle.overlapsWith(low));
+ EXPECT_FALSE(middle.overlapsWith(high));
+ EXPECT_TRUE(middle.overlapsWith(lowOverlap));
+ EXPECT_TRUE(middle.overlapsWith(highOverlap));
+ EXPECT_TRUE(middle.overlapsWith(contained));
+ EXPECT_TRUE(middle.overlapsWith(contains));
+
+ // Contains value ...
+ EXPECT_FALSE(middle.contains(10 - epsilon));
+ EXPECT_FALSE(middle.contains(20 + epsilon));
+ EXPECT_TRUE(middle.contains(10.0));
+ EXPECT_TRUE(middle.contains(20.0));
+ EXPECT_TRUE(middle.contains(15.0));
+
+ Interval<double> aroundZero(-5.0, 13.0);
+ Interval<double> zeroLow(-5.0, 0.0);
+ Interval<double> zeroHigh(0.0, 13.0);
+ Interval<double> almostZeroLow(-5.0, -epsilon);
+ Interval<double> almostZeroHigh(epsilon, 13.0);
+
+ // Contains zero ...
+ EXPECT_FALSE(middle.containsZero());
+ EXPECT_FALSE(almostZeroLow.containsZero());
+ EXPECT_FALSE(almostZeroHigh.containsZero());
+ EXPECT_TRUE(aroundZero.containsZero());
+ EXPECT_TRUE(zeroLow.containsZero());
+ EXPECT_TRUE(zeroHigh.containsZero());
+
+ // Subrange tests
+ EXPECT_TRUE(middle.lowerHalf().isApprox(Interval<double> (10.0, 15.0), epsilon));
+ EXPECT_TRUE(middle.upperHalf().isApprox(Interval<double> (15.0, 20.0), epsilon));
+};
+
+TEST_F(IntervalArithmeticTests, ExtendRangeTests)
+{
+ Interval<double> middle(10.0, 20.0);
+ Interval<double> original(middle);
+ EXPECT_TRUE(original == middle);
+
+ // Add onto both ends
+ EXPECT_FALSE(middle.contains(8.0));
+ EXPECT_FALSE(middle.contains(22.0));
+ Interval<double> add(middle.addThickness(2.0));
+ EXPECT_TRUE(add == middle);
+ EXPECT_FALSE(original == middle);
+ EXPECT_TRUE(middle.contains(8.0));
+ EXPECT_TRUE(middle.contains(22.0));
+ EXPECT_FALSE(middle.contains(7.999999999));
+ EXPECT_FALSE(middle.contains(22.00000001));
+
+ // Extend the interval ...
+ original = middle;
+ Interval<double> extend(middle.extendToInclude(12.0));
+ EXPECT_TRUE(original == middle);
+ EXPECT_TRUE(extend == middle);
+ EXPECT_TRUE(middle.contains(8.0));
+ EXPECT_TRUE(middle.contains(22.0));
+ EXPECT_FALSE(middle.contains(7.999999999));
+ EXPECT_FALSE(middle.contains(22.00000001));
+
+ extend = middle.extendToInclude(5.0);
+ EXPECT_FALSE(original == middle);
+ EXPECT_TRUE(extend == middle);
+ EXPECT_TRUE(middle == Interval<double>(5.0, 22));
+
+ extend = middle.extendToInclude(24.0);
+ EXPECT_TRUE(extend == middle);
+ EXPECT_TRUE(middle == Interval<double>(5.0, 24.0));
+
+ original = middle;
+ extend = middle.extendToInclude(Interval<double> (5.0, 24.0));
+ EXPECT_TRUE(extend == middle);
+ EXPECT_TRUE(middle == original);
+
+ extend = middle.extendToInclude(Interval<double> (4.0, 25.0));
+ EXPECT_TRUE(extend == middle);
+ EXPECT_FALSE(middle == original);
+ EXPECT_TRUE(middle == Interval<double>(4.0, 25.0));
+
+ extend = middle.extendToInclude(Interval<double> (3.0, 24.0));
+ EXPECT_TRUE(extend == middle);
+ EXPECT_TRUE(middle == Interval<double>(3.0, 25.0));
+
+ extend = middle.extendToInclude(Interval<double> (7.0, 32.0));
+ EXPECT_TRUE(extend == middle);
+ EXPECT_TRUE(middle == Interval<double>(3.0, 32.0));
+};
+
+TEST_F(IntervalArithmeticTests, OperatorTests)
+{
+ Interval<double> initial(10.0, 20.0);
+
+ // +
+ EXPECT_TRUE(Interval<double>(12, 30).isApprox((initial + Interval<double>(2.0, 10.0)), epsilon));
+ EXPECT_TRUE(Interval<double>(20, 30).isApprox((initial + 10), epsilon));
+
+ // +=
+ Interval<double> dummy(initial);
+ dummy += Interval<double>(2.0, 10.0);
+ EXPECT_TRUE(dummy.isApprox(Interval<double>(12, 30), epsilon));
+ dummy = initial;
+ dummy += 10.0;
+ EXPECT_TRUE(dummy.isApprox(Interval<double>(20, 30), epsilon));
+
+ // -
+ EXPECT_TRUE(Interval<double>(0.0, 18.0).isApprox((initial - Interval<double>(2.0, 10.0)), epsilon));
+ EXPECT_TRUE(Interval<double>(0, 10).isApprox((initial - 10), epsilon));
+
+ // -=
+ dummy = initial;
+ dummy -= Interval<double>(2.0, 10.0);
+ EXPECT_TRUE(Interval<double>(0.0, 18.0).isApprox(dummy, epsilon));
+ dummy = initial;
+ dummy -= 10.0;
+ EXPECT_TRUE(Interval<double>(0.0, 10.0).isApprox(dummy, epsilon));
+
+ // Negation
+ EXPECT_TRUE(Interval<double>(-20.0, -10.0).isApprox(-initial, epsilon));
+
+ // *
+ EXPECT_TRUE(Interval<double>(20.0, 200.0).isApprox((initial * Interval<double>(2.0, 10.0)), epsilon));
+ EXPECT_TRUE(Interval<double>(-200.0, 40.0).isApprox((-initial * Interval<double>(-2.0, 10.0)), epsilon));
+ EXPECT_TRUE(Interval<double>(100.0, 200.0).isApprox((initial * 10), epsilon));
+ EXPECT_TRUE(Interval<double>(100.0, 200.0).isApprox((-initial * -10), epsilon));
+
+ // *=
+ dummy = initial;
+ dummy *= Interval<double>(2.0, 10.0);
+ EXPECT_TRUE(Interval<double>(20.0, 200.0).isApprox(dummy, epsilon));
+ dummy = -initial;
+ dummy *= Interval<double>(-2.0, 10.0);
+ EXPECT_TRUE(Interval<double>(-200.0, 40.0).isApprox(dummy, epsilon));
+ dummy = initial;
+ dummy *= 10.0;
+ EXPECT_TRUE(Interval<double>(100.0, 200.0).isApprox((initial * 10), epsilon));
+ dummy = -initial;
+ dummy *= -10.0;
+ EXPECT_TRUE(Interval<double>(100.0, 200.0).isApprox((-initial * -10), epsilon));
+
+ // /
+ Interval<double> zeroInterval(-5.0, 5.0);
+ EXPECT_THROW(initial / zeroInterval, SurgSim::Framework::AssertionFailure);
+ EXPECT_TRUE(Interval<double>(20.0, 200.0).isApprox((initial / Interval<double>(0.1, 0.5)), epsilon));
+
+ // /=
+ dummy = zeroInterval;
+ EXPECT_THROW(dummy /= zeroInterval, SurgSim::Framework::AssertionFailure);
+ dummy = initial;
+ dummy /= Interval<double>(0.1, 0.5);
+ EXPECT_TRUE(Interval<double>(20.0, 200.0).isApprox(dummy, epsilon));
+
+ // Inverse
+ EXPECT_THROW(zeroInterval.inverse(), SurgSim::Framework::AssertionFailure);
+ EXPECT_TRUE(Interval<double> (2, 10).isApprox(Interval<double>(0.1, 0.5).inverse(), epsilon));
+
+ // Square
+ EXPECT_TRUE(Interval<double> (0, 25).isApprox(zeroInterval.square(), epsilon));
+ EXPECT_TRUE(Interval<double> (0, 25).isApprox((-zeroInterval).square(), epsilon));
+ EXPECT_TRUE(Interval<double> (100, 400).isApprox(initial.square(), epsilon));
+ EXPECT_TRUE(Interval<double> (100, 400).isApprox((-initial).square(), epsilon));
+};
+
+// Interval ND tests
+TEST_F(IntervalArithmeticTests, IntervalNDInitializationTests)
+{
+ // Constructor tests
+ Interval<double> testInterval(3.7, 3.8);
+ std::array<double, 2> minimums = {1.1, 4.4};
+ std::array<double, 2> maximums = {4.1, 3.2};
+ std::array<double, 2> minimumsPlusEpsilon = {1.1 + delta, 4.4 + delta};
+ std::array<double, 2> maximumsPlusEpsilon = {4.1 + delta, 3.2 + delta};
+ std::array<double, 2> maximumsPlus2Epsilon = {4.1 + (2 * epsilon), 3.2 + delta};
+
+ std::array<Interval<double>, 2> testIntervalArray;
+ testIntervalArray[0] = Interval<double> (1.0, 2.0);
+ testIntervalArray[1] = Interval<double> (2.0, 3.0);
+
+ EXPECT_NO_THROW(IntervalDouble2 a);
+ EXPECT_NO_THROW(IntervalDouble2 a(testIntervalArray));
+ EXPECT_NO_THROW(IntervalDouble2 a(minimums, maximums));
+ EXPECT_NO_THROW(IntervalDouble2 a(IntervalDouble2(minimums, maximums)));
+
+ IntervalDouble2 b(minimums, maximums);
+ IntervalDouble2 a(b);
+ EXPECT_TRUE(a == b);
+ EXPECT_NO_THROW(testIntervalNDMoveConstructor(b));
+ IntervalDouble2 f = testIntervalNDMoveConstructor(b);
+ EXPECT_TRUE(f == b);
+
+ // Assignment and equal/not equal tests
+ IntervalDouble2 c;
+ EXPECT_TRUE(c != b);
+ EXPECT_FALSE(c == b);
+ c = a;
+ EXPECT_FALSE(c != b);
+ EXPECT_TRUE(c == b);
+ IntervalDouble2 e;
+ e = (b * c);
+ EXPECT_TRUE(e == (b * c));
+
+ IntervalDouble2 bPlusEpsilon(minimumsPlusEpsilon, maximumsPlusEpsilon);
+ IntervalDouble2 bPlus2Epsilon(minimumsPlusEpsilon, maximumsPlus2Epsilon);
+ EXPECT_TRUE(b.isApprox(bPlusEpsilon, epsilon));
+ EXPECT_TRUE(bPlusEpsilon.isApprox(b, epsilon));
+ EXPECT_FALSE(b.isApprox(bPlus2Epsilon, epsilon));
+ EXPECT_FALSE(bPlus2Epsilon.isApprox(b, epsilon));
+
+ // Getting components and the reordering initializer
+ Interval<double> axis0(b.getAxis(0));
+ Interval<double> axis1(b.getAxis(1));
+
+ EXPECT_TRUE(axis0 == Interval<double>(1.1, 4.1));
+ EXPECT_TRUE(axis1 == Interval<double>(3.2, 4.4));
+};
+
+TEST_F(IntervalArithmeticTests, IntervalNDRangeTests)
+{
+ std::array<double, 2> minimumsShape = {1.1, 3.2};
+ std::array<double, 2> maximumsShape = {4.1, 4.4};
+
+ std::array<double, 2> noOverlapMin = {1.1, 4.5};
+ std::array<double, 2> noOverlapMax = {4.5, 4.8};
+
+ std::array<double, 2> overlapMin = {1.7, 2.0};
+ std::array<double, 2> overlapMax = {4.3, 4.7};
+
+ IntervalDouble2 shape(minimumsShape, maximumsShape);
+
+ EXPECT_FALSE(shape.overlapsWith(IntervalDouble2(noOverlapMin, noOverlapMax)));
+ EXPECT_TRUE(shape.overlapsWith(IntervalDouble2(overlapMin, overlapMax)));
+};
+
+TEST_F(IntervalArithmeticTests, IntervalNDExtendRangeTests)
+{
+ std::array<double, 2> minimumsShape = {1.1, 3.2};
+ std::array<double, 2> maximumsShape = {4.1, 4.4};
+
+ std::array<double, 2> finalMin = { -5.9, -3.8};
+ std::array<double, 2> finalMax = {11.1, 11.4};
+
+ IntervalDouble2 shape(minimumsShape, maximumsShape);
+ shape.addThickness(7.0);
+ EXPECT_EQ(IntervalDouble2(finalMin, finalMax), shape);
+};
+
+TEST_F(IntervalArithmeticTests, IntervalNDOperatorTests)
+{
+ std::array<double, 2> minimumsShape = {1.1, 3.2};
+ std::array<double, 2> maximumsShape = {4.1, 4.4};
+
+ std::array<double, 2> deltaMin = { -1.0, -2.0};
+ std::array<double, 2> deltaMax = {3.0, 4.0};
+
+ std::array<double, 2> nonZeroDeltaMin = { -3.0, 2.0};
+ std::array<double, 2> nonZeroDeltaMax = { -1.0, 4.0};
+
+ IntervalDouble2 initial(minimumsShape, maximumsShape);
+ IntervalDouble2 deltaInterval(deltaMin, deltaMax);
+ IntervalDouble2 nonZeroDelta(nonZeroDeltaMin, nonZeroDeltaMax);
+ IntervalDouble2 dummy;
+
+ // +
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) + Interval<double>(-1.0, 3.0)).isApprox(
+ (initial + deltaInterval).getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) + Interval<double>(-2.0, 4.0)).isApprox(
+ (initial + deltaInterval).getAxis(1), epsilon));
+
+ // +=
+ dummy = initial;
+ dummy += deltaInterval;
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) + Interval<double>(-1.0, 3.0)).isApprox(dummy.getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) + Interval<double>(-2.0, 4.0)).isApprox(dummy.getAxis(1), epsilon));
+
+ // -
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) - Interval<double>(-1.0, 3.0)).isApprox(
+ (initial - deltaInterval).getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) - Interval<double>(-2.0, 4.0)).isApprox(
+ (initial - deltaInterval).getAxis(1), epsilon));
+
+ // -=
+ dummy = initial;
+ dummy -= deltaInterval;
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) - Interval<double>(-1.0, 3.0)).isApprox(dummy.getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) - Interval<double>(-2.0, 4.0)).isApprox(dummy.getAxis(1), epsilon));
+
+ // *
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) * Interval<double>(-1.0, 3.0)).isApprox(
+ (initial * deltaInterval).getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) * Interval<double>(-2.0, 4.0)).isApprox(
+ (initial * deltaInterval).getAxis(1), epsilon));
+
+ // *=
+ dummy = initial;
+ dummy *= deltaInterval;
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) * Interval<double>(-1.0, 3.0)).isApprox(dummy.getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) * Interval<double>(-2.0, 4.0)).isApprox(dummy.getAxis(1), epsilon));
+
+ // /
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) / Interval<double>(-3.0, -1.0)).isApprox(
+ (initial / nonZeroDelta).getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) / Interval<double>(2.0, 4.0)).isApprox(
+ (initial / nonZeroDelta).getAxis(1), epsilon));
+
+ // /=
+ dummy = initial;
+ dummy /= nonZeroDelta;
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) / Interval<double>(-3.0, -1.0)).isApprox(dummy.getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) / Interval<double>(2.0, 4.0)).isApprox(dummy.getAxis(1), epsilon));
+
+ // Inverse
+ EXPECT_TRUE(Interval<double>(1.1, 4.1).inverse().isApprox(initial.inverse().getAxis(0), epsilon));
+ EXPECT_TRUE(Interval<double>(3.2, 4.4).inverse().isApprox(initial.inverse().getAxis(1), epsilon));
+
+ // Dot product
+ Interval<double> dotValue(initial.dotProduct(deltaInterval));
+ EXPECT_TRUE(dotValue.isApprox((initial.getAxis(0) * deltaInterval.getAxis(0)) +
+ (initial.getAxis(1) * deltaInterval.getAxis(1)), epsilon));
+
+ // Magnitude and magnitude squared
+ Interval<double> magnitudeSquaredValue(initial.magnitudeSquared());
+ EXPECT_TRUE(magnitudeSquaredValue.isApprox((initial.getAxis(0).square() + initial.getAxis(1).square()), epsilon));
+
+ Interval<double> magnitudeValue(initial.magnitude());
+ EXPECT_NEAR(std::sqrt(magnitudeSquaredValue.getMin()), magnitudeValue.getMin(), epsilon);
+ EXPECT_NEAR(std::sqrt(magnitudeSquaredValue.getMax()), magnitudeValue.getMax(), epsilon);
+};
+
+// Interval ND tests
+TEST_F(IntervalArithmeticTests, Interval3DInitializationTests)
+{
+ // Constructor tests
+ Interval<double> testInterval(3.7, 3.8);
+ std::array<double, 3> minimums = {1.1, 4.4, -7.2};
+ std::array<double, 3> maximums = {4.1, 3.2, -1.0};
+ std::array<double, 3> minimumsPlusEpsilon = {1.1 + delta, 4.4 + delta, -7.2 + delta};
+ std::array<double, 3> maximumsPlusEpsilon = {4.1 + delta, 3.2 + delta, -1.0 + delta};
+ std::array<double, 3> maximumsPlus2Epsilon = {4.1 + (2 * epsilon), 3.2 + delta, -1.0 + delta};
+
+ std::array<Interval<double>, 3> testIntervalArray;
+ testIntervalArray[0] = Interval<double> (1.0, 2.0);
+ testIntervalArray[1] = Interval<double> (2.0, 3.0);
+ testIntervalArray[2] = Interval<double> (3.0, 4.0);
+
+ EXPECT_NO_THROW(IntervalDouble3 a);
+ EXPECT_NO_THROW(IntervalDouble3 a(testIntervalArray));
+ EXPECT_NO_THROW(IntervalDouble3 a(Interval<double> (1.0, 2.0),
+ Interval<double> (2.0, 3.0),
+ Interval<double> (3.0, 4.0)));
+ EXPECT_NO_THROW(IntervalDouble3 a(minimums, maximums));
+ EXPECT_NO_THROW(IntervalDouble3 a(IntervalDouble3(minimums, maximums)));
+
+ IntervalDouble3 b(minimums, maximums);
+ IntervalDouble3 a(b);
+ EXPECT_TRUE(a == b);
+ EXPECT_NO_THROW(testInterval3DMoveConstructor(b));
+ IntervalDouble3 f = testInterval3DMoveConstructor(b);
+ EXPECT_TRUE(f == b);
+
+ // Assignment and equal/not equal tests
+ IntervalDouble3 c;
+ EXPECT_TRUE(c != b);
+ EXPECT_FALSE(c == b);
+ c = a;
+ EXPECT_FALSE(c != b);
+ EXPECT_TRUE(c == b);
+ IntervalDouble3 e;
+ e = (b * c);
+ EXPECT_TRUE(e == (b * c));
+
+ IntervalDouble3 bPlusEpsilon(minimumsPlusEpsilon, maximumsPlusEpsilon);
+ IntervalDouble3 bPlus2Epsilon(minimumsPlusEpsilon, maximumsPlus2Epsilon);
+ EXPECT_TRUE(b.isApprox(bPlusEpsilon, epsilon));
+ EXPECT_TRUE(bPlusEpsilon.isApprox(b, epsilon));
+ EXPECT_FALSE(b.isApprox(bPlus2Epsilon, epsilon));
+ EXPECT_FALSE(bPlus2Epsilon.isApprox(b, epsilon));
+ // Getting components and the reordering initializer
+ Interval<double> axis0 = b.getAxis(0);
+ Interval<double> axis1 = b.getAxis(1);
+ Interval<double> axis2 = b.getAxis(2);
+
+ EXPECT_TRUE(axis0 == Interval<double>(1.1, 4.1));
+ EXPECT_TRUE(axis1 == Interval<double>(3.2, 4.4));
+ EXPECT_TRUE(axis2 == Interval<double>(-7.2, -1.0));
+};
+
+TEST_F(IntervalArithmeticTests, Interval3DRangeTests)
+{
+ std::array<double, 3> minimumsShape = {1.1, 3.2, -7.2};
+ std::array<double, 3> maximumsShape = {4.1, 4.4, -1.0};
+
+ std::array<double, 3> noOverlapMin = {1.1, 4.5, -8.0};
+ std::array<double, 3> noOverlapMax = {4.5, 4.8, -3.0};
+
+ std::array<double, 3> overlapMin = {1.7, 2.0, -8.0};
+ std::array<double, 3> overlapMax = {4.3, 4.7, -3.0};
+
+ IntervalDouble3 shape(minimumsShape, maximumsShape);
+
+ EXPECT_FALSE(shape.overlapsWith(IntervalDouble3(noOverlapMin, noOverlapMax)));
+ EXPECT_TRUE(shape.overlapsWith(IntervalDouble3(overlapMin, overlapMax)));
+};
+
+TEST_F(IntervalArithmeticTests, Interval3DExtendRangeTests)
+{
+ std::array<double, 3> minimumsShape = {1.1, 3.2, -7.2};
+ std::array<double, 3> maximumsShape = {4.1, 4.4, -1.0};
+
+ std::array<double, 3> finalMin = { -5.9, -3.8, -14.2};
+ std::array<double, 3> finalMax = {11.1, 11.4, 6.0};
+
+ IntervalDouble3 shape(minimumsShape, maximumsShape);
+ shape.addThickness(7.0);
+ EXPECT_EQ(IntervalDouble3(finalMin, finalMax), shape);
+};
+
+TEST_F(IntervalArithmeticTests, Interval3DOperatorTests)
+{
+ std::array<double, 3> minimumsShape = {1.1, 3.2, -7.2};
+ std::array<double, 3> maximumsShape = {4.1, 4.4, -1.0};
+
+ std::array<double, 3> deltaMin = { -1.0, -2.0, -3.0};
+ std::array<double, 3> deltaMax = {3.0, 4.0, 5.0};
+
+ std::array<double, 3> nonZeroDeltaMin = { -3.0, 2.0, 3.0};
+ std::array<double, 3> nonZeroDeltaMax = { -1.0, 4.0, 5.0};
+
+ IntervalDouble3 initial(minimumsShape, maximumsShape);
+ IntervalDouble3 delta(deltaMin, deltaMax);
+ IntervalDouble3 nonZeroDelta(nonZeroDeltaMin, nonZeroDeltaMax);
+ IntervalDouble3 dummy;
+
+ // +
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) + Interval<double>(-1.0, 3.0)).isApprox(
+ (initial + delta).getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) + Interval<double>(-2.0, 4.0)).isApprox(
+ (initial + delta).getAxis(1), epsilon));
+ EXPECT_TRUE((Interval<double>(-7.2, -1.0) + Interval<double>(-3.0, 5.0)).isApprox(
+ (initial + delta).getAxis(2), epsilon));
+
+ // +=
+ dummy = initial;
+ dummy += delta;
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) + Interval<double>(-1.0, 3.0)).isApprox(dummy.getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) + Interval<double>(-2.0, 4.0)).isApprox(dummy.getAxis(1), epsilon));
+ EXPECT_TRUE((Interval<double>(-7.2, -1.0) + Interval<double>(-3.0, 5.0)).isApprox(dummy.getAxis(2), epsilon));
+
+ // -
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) - Interval<double>(-1.0, 3.0)).isApprox(
+ (initial - delta).getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) - Interval<double>(-2.0, 4.0)).isApprox(
+ (initial - delta).getAxis(1), epsilon));
+ EXPECT_TRUE((Interval<double>(-7.2, -1.0) - Interval<double>(-3.0, 5.0)).isApprox(
+ (initial - delta).getAxis(2), epsilon));
+
+ // -=
+ dummy = initial;
+ dummy -= delta;
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) - Interval<double>(-1.0, 3.0)).isApprox(dummy.getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) - Interval<double>(-2.0, 4.0)).isApprox(dummy.getAxis(1), epsilon));
+ EXPECT_TRUE((Interval<double>(-7.2, -1.0) - Interval<double>(-3.0, 5.0)).isApprox(dummy.getAxis(2), epsilon));
+
+ // *
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) * Interval<double>(-1.0, 3.0)).isApprox(
+ (initial * delta).getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) * Interval<double>(-2.0, 4.0)).isApprox(
+ (initial * delta).getAxis(1), epsilon));
+ EXPECT_TRUE((Interval<double>(-7.2, -1.0) * Interval<double>(-3.0, 5.0)).isApprox(
+ (initial * delta).getAxis(2), epsilon));
+
+ // *=
+ dummy = initial;
+ dummy *= delta;
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) * Interval<double>(-1.0, 3.0)).isApprox(dummy.getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) * Interval<double>(-2.0, 4.0)).isApprox(dummy.getAxis(1), epsilon));
+ EXPECT_TRUE((Interval<double>(-7.2, -1.0) * Interval<double>(-3.0, 5.0)).isApprox(dummy.getAxis(2), epsilon));
+
+ // /
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) / Interval<double>(-3.0, -1.0)).isApprox(
+ (initial / nonZeroDelta).getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) / Interval<double>(2.0, 4.0)).isApprox(
+ (initial / nonZeroDelta).getAxis(1), epsilon));
+ EXPECT_TRUE((Interval<double>(-7.2, -1.0) / Interval<double>(3.0, 5.0)).isApprox(
+ (initial / nonZeroDelta).getAxis(2), epsilon));
+
+ // /=
+ dummy = initial;
+ dummy /= nonZeroDelta;
+ EXPECT_TRUE((Interval<double>(1.1, 4.1) / Interval<double>(-3.0, -1.0)).isApprox(dummy.getAxis(0), epsilon));
+ EXPECT_TRUE((Interval<double>(3.2, 4.4) / Interval<double>(2.0, 4.0)).isApprox(dummy.getAxis(1), epsilon));
+ EXPECT_TRUE((Interval<double>(-7.2, -1.0) / Interval<double>(3.0, 5.0)).isApprox(dummy.getAxis(2), epsilon));
+
+ // Inverse
+ EXPECT_TRUE(Interval<double>(1.1, 4.1).inverse().isApprox(initial.inverse().getAxis(0), epsilon));
+ EXPECT_TRUE(Interval<double>(3.2, 4.4).inverse().isApprox(initial.inverse().getAxis(1), epsilon));
+ EXPECT_TRUE(Interval<double>(-7.2, -1.0).inverse().isApprox(initial.inverse().getAxis(2), epsilon));
+
+ // Dot product
+ Interval<double> dotValue = initial.dotProduct(delta);
+ EXPECT_TRUE(dotValue.isApprox((initial.getAxis(0) * delta.getAxis(0)) +
+ (initial.getAxis(1) * delta.getAxis(1)) +
+ (initial.getAxis(2) * delta.getAxis(2)), epsilon));
+
+ // Cross product
+ IntervalDouble3 crossValue = initial.crossProduct(delta);
+ EXPECT_TRUE(crossValue.getAxis(0).isApprox((initial.getAxis(1) * delta.getAxis(2) -
+ initial.getAxis(2) * delta.getAxis(1)), epsilon));
+ EXPECT_TRUE(crossValue.getAxis(1).isApprox((initial.getAxis(2) * delta.getAxis(0) -
+ initial.getAxis(0) * delta.getAxis(2)), epsilon));
+ EXPECT_TRUE(crossValue.getAxis(2).isApprox((initial.getAxis(0) * delta.getAxis(1) -
+ initial.getAxis(1) * delta.getAxis(0)), epsilon));
+
+ // Magnitude and magnitude squared
+ Interval<double> magnitudeSquaredValue = initial.magnitudeSquared();
+ EXPECT_TRUE(magnitudeSquaredValue.isApprox((initial.getAxis(0).square() +
+ initial.getAxis(1).square() + initial.getAxis(2).square()), epsilon));
+
+ Interval<double> magnitudeValue = initial.magnitude();
+ EXPECT_NEAR(std::sqrt(magnitudeSquaredValue.getMin()), magnitudeValue.getMin(), epsilon);
+ EXPECT_NEAR(std::sqrt(magnitudeSquaredValue.getMax()), magnitudeValue.getMax(), epsilon);
+};
+
+TEST_F(IntervalArithmeticTests, IntervalUtilityTests)
+{
+ Interval<double> initial(10.0, 20.0);
+ Interval<double> deltaInterval(3.0, 4.0);
+ Interval<double> resultLoad(-2.0, -1.0);
+ Interval<double> result;
+
+ // Constant + interval
+ EXPECT_TRUE((initial + 11.5).isApprox(11.5 + initial, epsilon));
+
+ // Constant * interval
+ EXPECT_TRUE((initial * 11.5).isApprox(11.5 * initial, epsilon));
+
+ // Output
+ std::ostringstream intervalOutput;
+ intervalOutput << initial;
+ EXPECT_EQ("[10,20]", intervalOutput.str());
+
+ // +
+ IntervalArithmetic_add(initial, deltaInterval, &result);
+ EXPECT_TRUE((initial + deltaInterval).isApprox(result, epsilon));
+
+ // +=( + )
+ result = resultLoad;
+ IntervalArithmetic_addadd(initial, deltaInterval, &result);
+ EXPECT_TRUE((resultLoad + initial + deltaInterval).isApprox(result, epsilon));
+
+ // -
+ IntervalArithmetic_sub(initial, deltaInterval, &result);
+ EXPECT_TRUE((initial - deltaInterval).isApprox(result, epsilon));
+
+ // +=( - )
+ result = resultLoad;
+ IntervalArithmetic_addsub(initial, deltaInterval, &result);
+ EXPECT_TRUE((resultLoad + (initial - deltaInterval)).isApprox(result, epsilon));
+
+ // *
+ IntervalArithmetic_mul(initial, deltaInterval, &result);
+ EXPECT_TRUE((initial * deltaInterval).isApprox(result, epsilon));
+
+ // += ( * )
+ result = resultLoad;
+ IntervalArithmetic_addmul(initial, deltaInterval, &result);
+ EXPECT_TRUE((resultLoad + (initial * deltaInterval)).isApprox(result, epsilon));
+
+ // -= ( * )
+ result = resultLoad;
+ IntervalArithmetic_submul(initial, deltaInterval, &result);
+ EXPECT_TRUE((resultLoad - (initial * deltaInterval)).isApprox(result, epsilon));
+};
+
+TEST_F(IntervalArithmeticTests, IntervalNDUtilityTests)
+{
+ std::array<double, 2> minimumsShape = {1.1, 3.2};
+ std::array<double, 2> maximumsShape = {4.1, 4.4};
+
+ IntervalDouble2 shape(minimumsShape, maximumsShape);
+
+ // Output
+ std::ostringstream intervalOutput;
+ intervalOutput << shape;
+ EXPECT_EQ("([1.1,4.1];[3.2,4.4])", intervalOutput.str());
+};
+
+TEST_F(IntervalArithmeticTests, Interval3DUtilityTests)
+{
+ std::array<double, 3> minimumsShape = {1.1, 3.2, -7.2};
+ std::array<double, 3> maximumsShape = {4.1, 4.4, -1.0};
+
+ std::array<double, 3> deltaMin = { -1.0, -2.0, -3.0};
+ std::array<double, 3> deltaMax = {3.0, 4.0, 5.0};
+
+ IntervalDouble3 initial(minimumsShape, maximumsShape);
+ IntervalDouble3 deltaInterval(deltaMin, deltaMax);
+ IntervalDouble3 result;
+ Interval<double> intervalResult;
+
+ // +
+ IntervalArithmetic_add(initial, deltaInterval, &result);
+ EXPECT_TRUE((initial + deltaInterval).isApprox(result, epsilon));
+
+ // -
+ IntervalArithmetic_sub(initial, deltaInterval, &result);
+ EXPECT_TRUE((initial - deltaInterval).isApprox(result, epsilon));
+
+ // dot product
+ IntervalArithmetic_dotProduct(initial, deltaInterval, &intervalResult);
+ EXPECT_TRUE(initial.dotProduct(deltaInterval).isApprox(intervalResult, epsilon));
+
+ // Cross product
+ IntervalArithmetic_crossProduct(initial, deltaInterval, &result);
+ EXPECT_TRUE(initial.crossProduct(deltaInterval).isApprox(result, epsilon));
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/UnitTests/KalmanFilterTests.cpp b/SurgSim/Math/UnitTests/KalmanFilterTests.cpp
new file mode 100644
index 0000000..3f1989c
--- /dev/null
+++ b/SurgSim/Math/UnitTests/KalmanFilterTests.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 KalmanFilter.cpp functions.
+
+#include <boost/math/special_functions/fpclassify.hpp>
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/KalmanFilter.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Vector.h"
+
+typedef Eigen::Matrix<double, 1, 1> Vector1d;
+typedef Eigen::Matrix<double, 1, 1, Eigen::RowMajor> Matrix11d;
+
+namespace SurgSim
+{
+
+namespace Math
+{
+TEST(KalmanFilterTests, 1DConstant)
+{
+ auto kalman = std::make_shared<KalmanFilter<1, 1>>();
+ EXPECT_TRUE(boost::math::isnan(kalman->getState()[0]));
+
+ const Vector1d initialState = Vector1d::Zero();
+ kalman->setInitialState(initialState); // the state is a scalar, and we have an initial guess for it
+ EXPECT_DOUBLE_EQ(initialState[0], kalman->getState()[0]);
+
+ kalman->setInitialStateCovariance(Matrix11d::Constant(1000.0)); // the uncertainty about initial guess is high
+ kalman->setStateTransition(Matrix11d::Constant(1.0)); // we predict the true state will stay constant
+ kalman->setObservationMatrix(Matrix11d::Constant(1.0)); // we observe the actual state plus measurement error
+ kalman->setProcessNoiseCovariance(Matrix11d::Constant(0.0001)); // the noise in the prediction action is low
+ kalman->setMeasurementNoiseCovariance(Matrix11d::Constant(0.1)); // the assumed measurement noise
+
+ const double measurements[] = {0.9, 0.8, 1.1, 1, 0.95, 1.05, 1.2, 0.9, 0.85, 1.15};
+ for (double measurement : measurements)
+ {
+ kalman->update(Vector1d::Constant(measurement));
+ }
+
+ const double result = 0.99046032523740679;
+ EXPECT_NEAR(result , kalman->getState()[0], 1e-9);
+ // The state doesn't change unless there's an update.
+ EXPECT_NEAR(result, kalman->getState()[0], 1e-9);
+ // Updating a constant model without a measurement shouldn't change the state.
+ EXPECT_NEAR(result, kalman->update()[0], 1e-9);
+}
+
+TEST(KalmanFilterTests, 1DVelocity)
+{
+ auto kalman = std::make_shared<KalmanFilter<2, 1>>();
+ // The state is the position and velocity, and here's our initial guess.
+ const Vector2d initialState = Vector2d::Zero();
+ kalman->setInitialState(initialState);
+ const Matrix22d initialStateCovariance = 1000.0 * Matrix22d::Identity();
+ kalman->setInitialStateCovariance(initialStateCovariance);
+ // The new position is the old position plus velocity times dt. The velocity is constant.
+ const double dt = 0.1;
+ Matrix22d stateTransition;
+ stateTransition << 1.0, dt,
+ 0.0, 1.0;
+ kalman->setStateTransition(stateTransition);
+ // The measurements are of the position, plus measurement error.
+ Eigen::Matrix<double, 1, 2, Eigen::RowMajor> observationMatrix;
+ observationMatrix.setIdentity();
+ kalman->setObservationMatrix(observationMatrix);
+ const double velocityNoise = 0.001;
+ // Assume the noise is only in the velocity, so a continuous noise model of [0, 0; 0, velocityNoise], then
+ // approximate that by a time-discrete process to get...
+ Matrix22d processNoise;
+ processNoise << dt * dt * dt * velocityNoise / 3.0, dt * dt * velocityNoise / 2.0,
+ dt * dt * velocityNoise / 3.0, dt * velocityNoise;
+ kalman->setProcessNoiseCovariance(processNoise);
+ kalman->setMeasurementNoiseCovariance(Matrix11d::Constant(0.1));
+
+ const double measurements[] = {0.11, 0.29, 0.32, 0.5, 0.58, 0.54};
+ for (double measurement : measurements)
+ {
+ kalman->update(Vector1d::Constant(measurement));
+ }
+
+ // check the state
+ const double position = 0.61844193701221828;
+ EXPECT_NEAR(position, kalman->getState()[0], 1e-9);
+ const double velocity = 0.91376229444025137;
+ EXPECT_NEAR(velocity, kalman->getState()[1], 1e-9);
+
+ // The state doesn't change unless there's an update.
+ EXPECT_NEAR(position, kalman->getState()[0], 1e-9);
+ // Updating without a measurement will predict ahead.
+ EXPECT_NEAR(position + velocity * dt, kalman->update()[0], 1e-9);
+ EXPECT_NEAR(velocity, kalman->getState()[1], 1e-9);
+}
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/UnitTests/LinearMotionArithmeticTests.cpp b/SurgSim/Math/UnitTests/LinearMotionArithmeticTests.cpp
new file mode 100644
index 0000000..688a3be
--- /dev/null
+++ b/SurgSim/Math/UnitTests/LinearMotionArithmeticTests.cpp
@@ -0,0 +1,435 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 LinearMotionArithmetic functions.
+
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/LinearMotionArithmetic.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+namespace
+{
+double epsilon = 1.0e-10;
+}
+
+template <typename T>
+class LinearMotionArithmeticTests : public ::testing::Test
+{
+public:
+ typedef LinearMotionND<double, 2> LinearMotionDouble2;
+ typedef LinearMotionND<double, 3> LinearMotionDouble3;
+
+ LinearMotion<double> testLinearMotionMoveConstructor(LinearMotion<double> dummy)
+ {
+ LinearMotion<double> ret;
+ ret = dummy;
+ return ret;
+ }
+
+ template <int dimension>
+ LinearMotionND<double, dimension> testLinearMotionNDMoveConstructor(LinearMotionND<double, dimension> dummy)
+ {
+ LinearMotionND<double, dimension> ret;
+ ret = dummy;
+ return ret;
+ }
+
+ void initializeConstructor(LinearMotion<double>* motion)
+ {
+ // Constructor tests
+ EXPECT_NO_THROW(LinearMotion<double> a);
+ EXPECT_NO_THROW(LinearMotion<double> a(1.0, 2.0));
+ EXPECT_NO_THROW(LinearMotion<double> a(2.0, 1.0));
+ LinearMotion<double> a(2.0, 1.0);
+ EXPECT_NO_THROW(LinearMotion<double> b(a));
+ LinearMotion<double> b(a);
+ EXPECT_TRUE(b == a);
+ EXPECT_NO_THROW(testLinearMotionMoveConstructor(b));
+ LinearMotion<double> f = testLinearMotionMoveConstructor(b);
+ EXPECT_TRUE(f == b);
+ *motion = a;
+ }
+
+ template <int dimension>
+ void initializeConstructor(LinearMotionND<double, dimension>* motion)
+ {
+ typedef LinearMotionND<double, dimension> LinearMotionDoubleType;
+
+ // Constructor tests
+ LinearMotion<double> testLinearMotion(3.8, 3.7);
+ std::array<double, dimension> starts;
+ std::array<double, dimension> ends;
+ std::array<LinearMotion<double>, dimension> testLinearMotionArray;
+ for (int counter = 0; counter < dimension; ++counter)
+ {
+ starts[counter] = counter + 1.0;
+ ends[counter] = counter + 2.0;
+ testLinearMotionArray[counter] = LinearMotion<double> (counter + 2.0, counter + 1.0);
+ }
+
+ EXPECT_NO_THROW(LinearMotionDoubleType a);
+ EXPECT_NO_THROW(LinearMotionDoubleType a(testLinearMotionArray));
+ EXPECT_NO_THROW(LinearMotionDoubleType a(starts, ends));
+ EXPECT_NO_THROW(LinearMotionDoubleType a(LinearMotionDoubleType(starts, ends)));
+
+ LinearMotionDoubleType b(testLinearMotionArray);
+ LinearMotionDoubleType a(b);
+ EXPECT_TRUE(a == b);
+ EXPECT_NO_THROW(testLinearMotionNDMoveConstructor(b));
+ LinearMotionDoubleType f = testLinearMotionNDMoveConstructor(b);
+ EXPECT_TRUE(f == b);
+ *motion = a;
+ }
+
+ void checkLinearMotionConstructor(LinearMotion<double>* motion)
+ {
+ EXPECT_DOUBLE_EQ(2.0, motion->getStart());
+ EXPECT_DOUBLE_EQ(1.0, motion->getEnd());
+ }
+
+ template <int dimension>
+ void checkLinearMotionConstructor(LinearMotionND<double, dimension>* motion)
+ {
+ // Check get ... up to numb
+ for (int counter = 0; counter < dimension; ++counter)
+ {
+ EXPECT_DOUBLE_EQ(counter + 2.0, motion->getAxis(counter).getStart());
+ EXPECT_DOUBLE_EQ(counter + 1.0, motion->getAxis(counter).getEnd());
+ }
+ EXPECT_THROW(motion->getAxis(-1), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(motion->getAxis(dimension), SurgSim::Framework::AssertionFailure);
+ }
+
+ void setLinearMotionFromOffset(double offsetStart, double offsetEnd, LinearMotion<double>* motion)
+ {
+ (*motion) = LinearMotion<double>(offsetStart, offsetEnd);
+ }
+
+ template <int dimension>
+ void setLinearMotionFromOffset(double offsetStart, double offsetEnd, LinearMotionND<double, dimension>* motion)
+ {
+ std::array<LinearMotion<double>, dimension> values;
+ for (size_t counter = 0; counter < dimension; ++counter)
+ {
+ values[counter] = LinearMotion<double>(counter + offsetStart, counter + offsetEnd);
+ }
+ (*motion) = LinearMotionND<double, dimension>(values);
+ }
+
+ void checkLinearMotionOperators(LinearMotion<double>* motion1, LinearMotion<double>* motion2)
+ {
+ // Assignment and equal/not equal tests
+ LinearMotion<double> c;
+ c = *motion2;
+ EXPECT_TRUE(c != *motion1);
+ EXPECT_FALSE(c == *motion1);
+ c = *motion1;
+ EXPECT_FALSE(c != *motion1);
+ EXPECT_TRUE(c == *motion1);
+
+ // Move assignment
+ LinearMotion<double> e;
+ LinearMotion<double> zeros(0.0, 0.0);
+ e = (*motion1 + zeros);
+ EXPECT_TRUE(e == *motion1);
+
+ // to interval and polynomial
+ EXPECT_TRUE(motion1->toInterval().isApprox(Interval<double>(1.0, 2.0), epsilon));
+ EXPECT_TRUE(motion1->toPolynomial().isApprox(Polynomial<double, 1>(2.0, -1.0), epsilon));
+
+ // Contains 0 and isApprox
+ LinearMotion<double> containsZero(-1.0, 1.0);
+ EXPECT_TRUE(containsZero.containsZero());
+ EXPECT_FALSE(motion1->containsZero());
+ EXPECT_FALSE(motion2->containsZero());
+ LinearMotion<double> closeStart(motion1->getStart() + epsilon, motion1->getEnd());
+ LinearMotion<double> closeEnd(motion1->getStart(), motion1->getEnd() + epsilon);
+ EXPECT_TRUE(motion1->isApprox(closeStart, 2 * epsilon));
+ EXPECT_TRUE(motion1->isApprox(closeEnd, 2 * epsilon));
+ EXPECT_FALSE(motion1->isApprox(closeStart, 0.5 * epsilon));
+ EXPECT_FALSE(motion1->isApprox(closeEnd, 0.5 * epsilon));
+
+ // +, +=, -, -=, *, /
+ LinearMotion<double> scratch;
+ scratch = *motion1 + *motion2;
+ EXPECT_TRUE(scratch.isApprox(LinearMotion<double>(3.0, 3.0), epsilon));
+ scratch = *motion1;
+ scratch += *motion2;
+ EXPECT_TRUE(scratch.isApprox(LinearMotion<double>(3.0, 3.0), epsilon));
+ scratch = *motion1 - *motion2;
+ EXPECT_TRUE(scratch.isApprox(LinearMotion<double>(1.0, -1.0), epsilon));
+ scratch = *motion1;
+ scratch -= *motion2;
+ EXPECT_TRUE(scratch.isApprox(LinearMotion<double>(1.0, -1.0), epsilon));
+ EXPECT_TRUE((*motion1 * *motion2).isApprox(Interval<double>(1.0, 4.0), epsilon));
+ EXPECT_TRUE((*motion1 / *motion2).isApprox(Interval<double>(0.5, 2.0), epsilon));
+
+ // Range operators
+ EXPECT_DOUBLE_EQ(1.75, motion1->atTime(0.25));
+ EXPECT_TRUE(motion1->firstHalf().isApprox(LinearMotion<double>(2.0, 1.5), epsilon));
+ EXPECT_TRUE(motion1->secondHalf().isApprox(LinearMotion<double>(1.5, 1.0), epsilon));
+ }
+
+ template <int dimension>
+ void checkLinearMotionOperators(LinearMotionND<double, dimension>* motion1,
+ LinearMotionND<double, dimension>* motion2)
+ {
+ typedef LinearMotionND<double, dimension> LinearMotionDoubleType;
+ typedef IntervalND<double, dimension> IntervalDoubleType;
+
+ // Assignment and equal/not equal tests
+ LinearMotionDoubleType c;
+ c = *motion2;
+ EXPECT_TRUE(c != *motion1);
+ EXPECT_FALSE(c == *motion1);
+ c = *motion1;
+ EXPECT_FALSE(c != *motion1);
+ EXPECT_TRUE(c == *motion1);
+
+ // Move assignment
+ LinearMotionDoubleType e;
+ std::array<double, dimension> zeroArray;
+ for (int counter = 0; counter < dimension; ++counter)
+ {
+ zeroArray[counter] = 0.0;
+ }
+ LinearMotionDoubleType zeros(zeroArray, zeroArray);
+ e = (*motion1 + zeros);
+ EXPECT_TRUE(e == *motion1);
+
+ // to interval
+ std::array<Interval<double>, dimension> motionArray;
+ for (int counter = 0; counter < dimension; ++counter)
+ {
+ motionArray[counter] = motion1->getAxis(counter).toInterval();
+ }
+ EXPECT_TRUE(motion1->toInterval().isApprox(IntervalDoubleType(motionArray), epsilon));
+
+ // is approx
+ std::array<Interval<double>, dimension> approxArray;
+ for (int counter = 0; counter < dimension; ++counter)
+ {
+ approxArray[counter] = motion1->getAxis(counter).toInterval();
+ }
+ for (int counter = 0; counter < dimension; ++counter)
+ {
+ approxArray[counter] = (motion1->getAxis(counter) + LinearMotion<double>(epsilon, 0.0)).toInterval();
+ EXPECT_TRUE(motion1->toInterval().isApprox(IntervalDoubleType(approxArray), 2.0 * epsilon));
+ EXPECT_FALSE(motion1->toInterval().isApprox(IntervalDoubleType(approxArray), epsilon / 2.0));
+ approxArray[counter] = motion1->getAxis(counter).toInterval();
+ }
+
+ // +, +=, -, -=, *, /
+ LinearMotionDoubleType scratch;
+ scratch = *motion1 + *motion2;
+ for (int counter = 0; counter < dimension; ++counter)
+ {
+ EXPECT_TRUE(scratch.getAxis(counter).isApprox(LinearMotion<double>((2.0 * counter) + 3.0,
+ (2.0 * counter) + 3.0), epsilon));
+ }
+ scratch = *motion1;
+ scratch += *motion2;
+ for (int counter = 0; counter < dimension; ++counter)
+ {
+ EXPECT_TRUE(scratch.getAxis(counter).isApprox(LinearMotion<double>((2.0 * counter) + 3.0,
+ (2.0 * counter) + 3.0), epsilon));
+ }
+
+ scratch = *motion1 - *motion2;
+ for (int counter = 0; counter < dimension; ++counter)
+ {
+ EXPECT_TRUE(scratch.getAxis(counter).isApprox(LinearMotion<double>(1.0, -1.0), epsilon));
+ }
+ scratch = *motion1;
+ scratch -= *motion2;
+ for (int counter = 0; counter < dimension; ++counter)
+ {
+ EXPECT_TRUE(scratch.getAxis(counter).isApprox(LinearMotion<double>(1.0, -1.0), epsilon));
+ }
+
+ IntervalDoubleType intervalScratch;
+ intervalScratch = *motion1 * *motion2;
+ for (int counter = 0; counter < dimension; ++counter)
+ {
+ EXPECT_TRUE(intervalScratch.getAxis(counter).isApprox(Interval<double>((counter + 1) * (counter + 1),
+ (counter + 2) * (counter + 2)), epsilon));
+ }
+
+ intervalScratch = *motion1 / *motion2;
+ for (int counter = 0; counter < dimension; ++counter)
+ {
+ EXPECT_TRUE(intervalScratch.getAxis(counter).isApprox(
+ Interval<double>((counter + 1.0) / (counter + 2.0),
+ (counter + 2.0) / (counter + 1.0)), epsilon));
+ }
+
+ // Range operators
+ for (int counter = 0; counter < dimension; ++counter)
+ {
+ EXPECT_TRUE(motion1->firstHalf().getAxis(counter).isApprox(
+ LinearMotion<double>(counter + 2.0, counter + 1.5), epsilon));
+ EXPECT_TRUE(motion1->secondHalf().getAxis(counter).isApprox(
+ LinearMotion<double>(counter + 1.5, counter + 1.0), epsilon));
+ }
+ }
+};
+
+typedef ::testing::Types <LinearMotion<double>,
+ LinearMotionND<double, 2>,
+ LinearMotionND<double, 3>> LinearMotionArithmeticTypes;
+
+TYPED_TEST_CASE(LinearMotionArithmeticTests, LinearMotionArithmeticTypes);
+
+TYPED_TEST(LinearMotionArithmeticTests, LinearMotionInitializationTests)
+{
+ // Constructor tests
+ EXPECT_NO_THROW(TypeParam motion);
+ TypeParam motion;
+ this->initializeConstructor(&motion);
+ this->checkLinearMotionConstructor(&motion);
+};
+
+TYPED_TEST(LinearMotionArithmeticTests, LinearMotionOperatorTests)
+{
+ TypeParam motion1;
+ TypeParam motion2;
+ this->setLinearMotionFromOffset(2, 1, &motion1);
+ this->setLinearMotionFromOffset(1, 2, &motion2);
+ this->checkLinearMotionOperators(&motion1, &motion2);
+};
+
+TEST(LinearMotionSpecializations, LinearMotionNDExtras)
+{
+ // Dot product
+ std::array<double, 2> high;
+ high[0] = 2.0;
+ high[1] = 3.0;
+ std::array<double, 2> low;
+ low[0] = 1.0;
+ low[1] = 2.0;
+ LinearMotionND<double, 2> motion1(high, low);
+ LinearMotionND<double, 2> motion2(low, high);
+
+ double lower = 0;
+ double upper = 0;
+ for (int counter = 0; counter < 2; ++counter)
+ {
+ lower += (counter + 1) * (counter + 1);
+ upper += (counter + 2) * (counter + 2);
+ }
+ EXPECT_TRUE(motion1.dotProduct(motion2).isApprox(Interval<double>(lower, upper), epsilon));
+}
+
+TEST(LinearMotionSpecializations, LinearMotion3DExtras)
+{
+ // Dot product
+ std::array<double, 3> high;
+ high[0] = 2.0;
+ high[1] = 3.0;
+ high[2] = 4.0;
+ std::array<double, 3> low;
+ low[0] = 1.0;
+ low[1] = 2.0;
+ low[2] = 3.0;
+ LinearMotionND<double, 3> motion1(high, low);
+ LinearMotionND<double, 3> motion2(low, high);
+ Interval<double> range(0.0, 1.0);
+
+ // Dot product
+ // Dot product has an optimization using polynomials. Each linear motion is first converted to a linear
+ // polynomial and then the 6 linear polynomials are multiplied against each other to give three quadratic
+ // polynomials. The extrema of the sum of the quadratic polynomials limited to the interval under
+ // evaluation form the minimum and maximum of the new interval. For this test, we end up with a parabola
+ // opening downward. At 0 and 1 it has value 20. The maximum value occurs at 0.5 and has the value 20.75.
+ EXPECT_TRUE(motion1.dotProduct(motion2, range).isApprox(Interval<double>(20.0, 20.75), epsilon));
+
+ // Cross product
+ IntervalND<double, 3> cross = motion1.crossProduct(motion2, range);
+ EXPECT_TRUE(cross.getAxis(0).isApprox(Interval<double>(-1.0, 1.0), epsilon));
+ EXPECT_TRUE(cross.getAxis(1).isApprox(Interval<double>(-2.0, 2.0), epsilon));
+ EXPECT_TRUE(cross.getAxis(2).isApprox(Interval<double>(-1.0, 1.0), epsilon));
+
+ // Analytic cross product (polynomials)
+ Polynomial<double, 2> xPoly;
+ Polynomial<double, 2> yPoly;
+ Polynomial<double, 2> zPoly;
+ analyticCrossProduct(motion1, motion2, &xPoly, &yPoly, &zPoly);
+ EXPECT_TRUE(valuesOverInterval(xPoly, range).isApprox(Interval<double>(-1.0, 1.0), epsilon));
+ EXPECT_TRUE(valuesOverInterval(yPoly, range).isApprox(Interval<double>(-2.0, 2.0), epsilon));
+ EXPECT_TRUE(valuesOverInterval(zPoly, range).isApprox(Interval<double>(-1.0, 1.0), epsilon));
+
+ /// Magnitudes
+ EXPECT_TRUE(motion1.magnitudeSquared(range).isApprox(motion1.dotProduct(motion1, range), epsilon));
+ Interval<double> magnitude = motion1.magnitude(range);
+ Interval<double> dot = motion1.dotProduct(motion1, range);
+ EXPECT_DOUBLE_EQ(std::sqrt(dot.getMin()), magnitude.getMin());
+ EXPECT_DOUBLE_EQ(std::sqrt(dot.getMax()), magnitude.getMax());
+
+ // At Time
+ for (int counter = 0; counter < 3; ++counter)
+ {
+ EXPECT_DOUBLE_EQ(counter + 1.75, motion1.atTime(0.25)[counter]);
+ }
+}
+
+TEST(LinearMotionUtilities, LinearMotionUtilities)
+{
+ LinearMotion<double> motion(-12.1, 8.7);
+
+ // Output
+ std::ostringstream linearMotionOutput;
+ linearMotionOutput << motion;
+ EXPECT_EQ("(-12.1 -> 8.7)", linearMotionOutput.str());
+}
+
+TEST(LinearMotionUtilities, LinearMotion3DUtilities)
+{
+ Math::Vector3d high;
+ high[0] = 3.0;
+ high[1] = 4.0;
+ high[2] = 5.0;
+ Math::Vector3d low;
+ low[0] = 1.0;
+ low[1] = 2.0;
+ low[2] = 3.0;
+ Math::Vector3d medium;
+ medium[0] = 2.0;
+ medium[1] = 3.0;
+ medium[2] = 4.0;
+ LinearMotionND<double, 3> motion1(medium, low);
+ LinearMotionND<double, 3> motion2(low, medium);
+ LinearMotionND<double, 3> motion3(high, low);
+ Interval<double> range(0.0, 1.0);
+
+ // Output
+ std::ostringstream linearMotionOutput;
+ linearMotionOutput << motion1;
+ EXPECT_EQ("([2,3,4] -> [1,2,3])", linearMotionOutput.str());
+
+ // Triple Product a * (b X c)
+ EXPECT_TRUE(tripleProduct(motion1, motion2, motion3, range).isApprox(Interval<double>(0.0, 0.0), epsilon));
+}
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/UnitTests/LinearSolveAndInverseTests.cpp b/SurgSim/Math/UnitTests/LinearSolveAndInverseTests.cpp
index 6ac29e3..9e69f7e 100644
--- a/SurgSim/Math/UnitTests/LinearSolveAndInverseTests.cpp
+++ b/SurgSim/Math/UnitTests/LinearSolveAndInverseTests.cpp
@@ -36,7 +36,7 @@ private:
v->resize(size);
for (size_t row = 0; row < size; row++)
{
- (*v)(row) = fmod(-4.1 * row * row + 3.46, 5.0);
+ (*v)(row) = std::fmod(-4.1 * row * row + 3.46, 5.0);
}
}
@@ -47,9 +47,9 @@ private:
{
for (size_t col = 0; col < size; col++)
{
- (*m)(row, col) = fmod((10.3 * cos(static_cast<double>(row * col)) + 3.24), 10.0);
- }
+ (*m)(row, col) = std::fmod((10.3 * std::cos(static_cast<double>(row * col)) + 3.24), 10.0);
}
+ }
}
void initializeDiagonalMatrix(Matrix* m)
@@ -58,7 +58,7 @@ private:
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);
+ (*m)(row, row) = std::fmod((10.3 * std::cos(static_cast<double>(row * row)) + 3.24), 10.0);
}
}
@@ -73,8 +73,8 @@ private:
for (size_t rowBlockId = 0; rowBlockId < numBlocks; rowBlockId++)
{
for (int colBlockId = static_cast<int>(rowBlockId) - 1;
- colBlockId <= static_cast<int>(rowBlockId) + 1;
- colBlockId++)
+ colBlockId <= static_cast<int>(rowBlockId) + 1;
+ colBlockId++)
{
if (colBlockId < 0 || colBlockId >= static_cast<int>(numBlocks))
{
@@ -87,7 +87,7 @@ private:
{
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);
+ (*m)(row, col) = fmod((10.3 * std::cos(static_cast<double>(row * col)) + 3.24), 10.0);
}
}
}
@@ -148,7 +148,9 @@ TEST_F(LinearSolveAndInverseTests, DenseMatrixTests)
setupDenseMatrixTest();
LinearSolveAndInverseDenseMatrix solveAndInverse;
- solveAndInverse(matrix, b, &x, &inverseMatrix);
+ solveAndInverse.setMatrix(matrix);
+ x = solveAndInverse.solve(b);
+ inverseMatrix = solveAndInverse.getInverse();
EXPECT_TRUE(x.isApprox(expectedX));
EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
@@ -159,7 +161,9 @@ TEST_F(LinearSolveAndInverseTests, DiagonalMatrixTests)
setupDiagonalMatrixTest();
LinearSolveAndInverseDiagonalMatrix solveAndInverse;
- solveAndInverse(matrix, b, &x, &inverseMatrix);
+ solveAndInverse.setMatrix(matrix);
+ x = solveAndInverse.solve(b);
+ inverseMatrix = solveAndInverse.getInverse();
EXPECT_TRUE(x.isApprox(expectedX));
EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
@@ -170,7 +174,9 @@ TEST_F(LinearSolveAndInverseTests, TriDiagonalBlockMatrixBlockSize2Tests)
setupTriDiagonalBlockMatrixTest<2>();
LinearSolveAndInverseTriDiagonalBlockMatrix<2> solveAndInverse;
- solveAndInverse(matrix, b, &x, &inverseMatrix);
+ solveAndInverse.setMatrix(matrix);
+ x = solveAndInverse.solve(b);
+ inverseMatrix = solveAndInverse.getInverse();
EXPECT_TRUE(x.isApprox(expectedX));
EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
@@ -181,7 +187,9 @@ TEST_F(LinearSolveAndInverseTests, TriDiagonalBlockMatrixBlockSize3Tests)
setupTriDiagonalBlockMatrixTest<3>();
LinearSolveAndInverseTriDiagonalBlockMatrix<3> solveAndInverse;
- solveAndInverse(matrix, b, &x, &inverseMatrix);
+ solveAndInverse.setMatrix(matrix);
+ x = solveAndInverse.solve(b);
+ inverseMatrix = solveAndInverse.getInverse();
EXPECT_TRUE(x.isApprox(expectedX));
EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
@@ -192,7 +200,9 @@ TEST_F(LinearSolveAndInverseTests, TriDiagonalBlockMatrixBlockSize4Tests)
setupTriDiagonalBlockMatrixTest<4>();
LinearSolveAndInverseTriDiagonalBlockMatrix<4> solveAndInverse;
- solveAndInverse(matrix, b, &x, &inverseMatrix);
+ solveAndInverse.setMatrix(matrix);
+ x = solveAndInverse.solve(b);
+ inverseMatrix = solveAndInverse.getInverse();
EXPECT_TRUE(x.isApprox(expectedX));
EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
@@ -203,7 +213,9 @@ TEST_F(LinearSolveAndInverseTests, TriDiagonalBlockMatrixBlockSize5Tests)
setupTriDiagonalBlockMatrixTest<5>();
LinearSolveAndInverseTriDiagonalBlockMatrix<5> solveAndInverse;
- solveAndInverse(matrix, b, &x, &inverseMatrix);
+ solveAndInverse.setMatrix(matrix);
+ x = solveAndInverse.solve(b);
+ inverseMatrix = solveAndInverse.getInverse();
EXPECT_TRUE(x.isApprox(expectedX));
EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
@@ -214,7 +226,9 @@ TEST_F(LinearSolveAndInverseTests, TriDiagonalBlockMatrixBlockSize6Tests)
setupTriDiagonalBlockMatrixTest<6>();
LinearSolveAndInverseTriDiagonalBlockMatrix<6> solveAndInverse;
- solveAndInverse(matrix, b, &x, &inverseMatrix);
+ solveAndInverse.setMatrix(matrix);
+ x = solveAndInverse.solve(b);
+ inverseMatrix = solveAndInverse.getInverse();
EXPECT_TRUE(x.isApprox(expectedX));
EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
@@ -225,7 +239,9 @@ TEST_F(LinearSolveAndInverseTests, SymmetricTriDiagonalBlockMatrixBlockSize2Test
setupTriDiagonalBlockMatrixTest<2>(true);
LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<2> solveAndInverse;
- solveAndInverse(matrix, b, &x, &inverseMatrix);
+ solveAndInverse.setMatrix(matrix);
+ x = solveAndInverse.solve(b);
+ inverseMatrix = solveAndInverse.getInverse();
EXPECT_TRUE(x.isApprox(expectedX));
EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
@@ -236,7 +252,9 @@ TEST_F(LinearSolveAndInverseTests, SymmetricTriDiagonalBlockMatrixBlockSize3Test
setupTriDiagonalBlockMatrixTest<3>(true);
LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<3> solveAndInverse;
- solveAndInverse(matrix, b, &x, &inverseMatrix);
+ solveAndInverse.setMatrix(matrix);
+ x = solveAndInverse.solve(b);
+ inverseMatrix = solveAndInverse.getInverse();
EXPECT_TRUE(x.isApprox(expectedX));
EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
@@ -247,7 +265,9 @@ TEST_F(LinearSolveAndInverseTests, SymmetricTriDiagonalBlockMatrixBlockSize4Test
setupTriDiagonalBlockMatrixTest<4>(true);
LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<4> solveAndInverse;
- solveAndInverse(matrix, b, &x, &inverseMatrix);
+ solveAndInverse.setMatrix(matrix);
+ x = solveAndInverse.solve(b);
+ inverseMatrix = solveAndInverse.getInverse();
EXPECT_TRUE(x.isApprox(expectedX));
EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
@@ -258,7 +278,9 @@ TEST_F(LinearSolveAndInverseTests, SymmetricTriDiagonalBlockMatrixBlockSize5Test
setupTriDiagonalBlockMatrixTest<5>(true);
LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<5> solveAndInverse;
- solveAndInverse(matrix, b, &x, &inverseMatrix);
+ solveAndInverse.setMatrix(matrix);
+ x = solveAndInverse.solve(b);
+ inverseMatrix = solveAndInverse.getInverse();
EXPECT_TRUE(x.isApprox(expectedX));
EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
@@ -269,7 +291,9 @@ TEST_F(LinearSolveAndInverseTests, SymmetricTriDiagonalBlockMatrixBlockSize6Test
setupTriDiagonalBlockMatrixTest<6>(true);
LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix<6> solveAndInverse;
- solveAndInverse(matrix, b, &x, &inverseMatrix);
+ solveAndInverse.setMatrix(matrix);
+ x = solveAndInverse.solve(b);
+ inverseMatrix = solveAndInverse.getInverse();
EXPECT_TRUE(x.isApprox(expectedX));
EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
diff --git a/SurgSim/Math/UnitTests/LinearSparseSolveAndInverseTests.cpp b/SurgSim/Math/UnitTests/LinearSparseSolveAndInverseTests.cpp
new file mode 100644
index 0000000..5c9a78b
--- /dev/null
+++ b/SurgSim/Math/UnitTests/LinearSparseSolveAndInverseTests.cpp
@@ -0,0 +1,197 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 LinearSparseSolveAndInverse.cpp functions.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/LinearSparseSolveAndInverse.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+class LinearSparseSolveAndInverseTests : public ::testing::Test
+{
+public:
+
+ void setupSparseMatrixTest()
+ {
+ inverseMatrix.resize(size, size);
+ initializeSparseMatrix(&matrix);
+ }
+
+ SparseMatrix matrix;
+ Matrix denseMatrix, inverseMatrix, expectedInverse;
+ Vector b;
+ Vector x, expectedX;
+
+protected:
+ void SetUp() override
+ {
+ size = 18;
+ initializeVector(&b);
+ initializeDenseMatrix(&denseMatrix);
+ expectedInverse = denseMatrix.inverse();
+ expectedX = expectedInverse * b;
+ }
+
+private:
+ size_t size;
+
+ void initializeVector(Vector* v)
+ {
+ v->resize(size);
+ for (size_t row = 0; row < size; row++)
+ {
+ (*v)(row) = std::fmod(-4.1 * row * row + 3.46, 5.0);
+ }
+ }
+
+ void initializeSparseMatrix(SparseMatrix* m)
+ {
+ m->resize(static_cast<SparseMatrix::Index>(size), static_cast<SparseMatrix::Index>(size));
+ for (SparseMatrix::Index row = 0; row < static_cast<SparseMatrix::Index>(size); row++)
+ {
+ for (SparseMatrix::Index col = 0; col < static_cast<SparseMatrix::Index>(size); col++)
+ {
+ (*m).insert(row, col) =
+ std::fmod((10.3 * std::cos(static_cast<double>(row * col)) + 3.24), 10.0);
+ }
+ }
+ m->makeCompressed();
+ }
+
+ 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) = std::fmod((10.3 * std::cos(static_cast<double>(row * col)) + 3.24), 10.0);
+ }
+ }
+ }
+};
+
+TEST_F(LinearSparseSolveAndInverseTests, SparseLUInitializationTests)
+{
+ SparseMatrix nonSquare(9, 18);
+ SparseMatrix square(18, 18);
+ nonSquare.setZero();
+
+ for (SparseMatrix::Index counter = 0; counter < 18; ++counter)
+ {
+ square.insert(counter, counter) = 1.0;
+ }
+ square.makeCompressed();
+
+ LinearSparseSolveAndInverseLU solveAndInverse;
+ EXPECT_THROW(solveAndInverse.setMatrix(nonSquare), SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(solveAndInverse.setMatrix(square));
+
+ clearMatrix(&square);
+ EXPECT_THROW(solveAndInverse.setMatrix(square), SurgSim::Framework::AssertionFailure);
+};
+
+TEST_F(LinearSparseSolveAndInverseTests, SparseLUMatrixComponentsTest)
+{
+ setupSparseMatrixTest();
+
+ LinearSparseSolveAndInverseLU solveAndInverse;
+ solveAndInverse.setMatrix(matrix);
+ x = solveAndInverse.solve(b);
+ inverseMatrix = solveAndInverse.getInverse();
+
+ EXPECT_TRUE(x.isApprox(expectedX));
+ EXPECT_TRUE(inverseMatrix.isApprox(expectedInverse));
+
+ inverseMatrix = solveAndInverse.solve(denseMatrix);
+ EXPECT_TRUE(inverseMatrix.isApprox(Matrix::Identity(18, 18)));
+};
+
+TEST_F(LinearSparseSolveAndInverseTests, SparseCGSetGetTests)
+{
+ LinearSparseSolveAndInverseCG solveAndInverse;
+
+ EXPECT_NE(100, solveAndInverse.getMaxIterations());
+ solveAndInverse.setMaxIterations(100);
+ EXPECT_EQ(100, solveAndInverse.getMaxIterations());
+
+ EXPECT_NE(1.0e-03, solveAndInverse.getTolerance());
+ solveAndInverse.setTolerance(1.0e-03);
+ EXPECT_EQ(1.0e-03, solveAndInverse.getTolerance());
+};
+
+TEST_F(LinearSparseSolveAndInverseTests, SparseCGInitializationTests)
+{
+ SparseMatrix nonSquare(9, 18);
+ SparseMatrix square(18, 18);
+ nonSquare.setZero();
+
+ for (SparseMatrix::Index counter = 0; counter < 18; ++counter)
+ {
+ square.insert(counter, counter) = 1.0;
+ }
+ square.makeCompressed();
+
+ LinearSparseSolveAndInverseCG solveAndInverse;
+ EXPECT_THROW(solveAndInverse.setMatrix(nonSquare), SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(solveAndInverse.setMatrix(square));
+
+ clearMatrix(&square);
+ EXPECT_NO_THROW(solveAndInverse.setMatrix(square));
+};
+
+TEST_F(LinearSparseSolveAndInverseTests, SparseCGMatrixComponentsTest)
+{
+ setupSparseMatrixTest();
+ double lowPrecision = 1.0e-05;
+ double highPrecision = 1.0e-10;
+
+ LinearSparseSolveAndInverseCG solveAndInverse;
+ solveAndInverse.setMatrix(matrix);
+ x = solveAndInverse.solve(b);
+ inverseMatrix = solveAndInverse.getInverse();
+
+ EXPECT_TRUE(x.isApprox(expectedX, lowPrecision)) << std::endl << "x: " << x.transpose() << std::endl <<
+ "Expected: " << expectedX.transpose() << std::endl;
+
+ inverseMatrix = solveAndInverse.solve(denseMatrix);
+ EXPECT_TRUE(inverseMatrix.isApprox(Matrix::Identity(18, 18), lowPrecision)) << std::endl << "Identity: " <<
+ inverseMatrix << std::endl;
+
+ solveAndInverse.setTolerance(highPrecision);
+ solveAndInverse.setMaxIterations(18);
+ x = solveAndInverse.solve(b);
+ EXPECT_TRUE(x.isApprox(expectedX, lowPrecision)) << std::endl << "x: " << x.transpose() << std::endl <<
+ "Expected: " << expectedX.transpose() << std::endl;
+ EXPECT_FALSE(x.isApprox(expectedX, highPrecision)) << std::endl << "x: " << x.transpose() << std::endl <<
+ "Expected: " << expectedX.transpose() << std::endl;
+
+ solveAndInverse.setMaxIterations(50);
+ x = solveAndInverse.solve(b);
+ EXPECT_TRUE(x.isApprox(expectedX, highPrecision)) << std::endl << "x: " << x.transpose() << std::endl <<
+ "Expected: " << expectedX.transpose() << std::endl;
+
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/UnitTests/MeshShapeTests.cpp b/SurgSim/Math/UnitTests/MeshShapeTests.cpp
index ba3d390..4bd0bb0 100644
--- a/SurgSim/Math/UnitTests/MeshShapeTests.cpp
+++ b/SurgSim/Math/UnitTests/MeshShapeTests.cpp
@@ -1,17 +1,17 @@
-//// This file is a part of the OpenSurgSim project.
-//// Copyright 2013, SimQuest Solutions Inc.
-////
-//// Licensed under the Apache License, Version 2.0 (the "License");
-//// you may not use this file except in compliance with the License.
-//// You may obtain a copy of the License at
-////
-//// http://www.apache.org/licenses/LICENSE-2.0
-////
-//// Unless required by applicable law or agreed to in writing, software
-//// distributed under the License is distributed on an "AS IS" BASIS,
-//// WITHOUT 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 <time.h>
@@ -30,6 +30,16 @@
using SurgSim::DataStructures::EmptyData;
using SurgSim::DataStructures::TriangleMeshPlain;
+namespace
+{
+const double epsilon = 1e-8;
+}
+
+namespace SurgSim
+{
+namespace Math
+{
+
// CUBE
// 3*----------*2
// / /|
@@ -40,17 +50,17 @@ using SurgSim::DataStructures::TriangleMeshPlain;
// 4*----------*5
static const int cubeNumPoints = 8;
-static const SurgSim::Math::Vector3d cubePoints[8] =
+static const 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)
+ 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;
@@ -93,7 +103,7 @@ TEST_F(MeshShapeTest, InvalidMeshCubeTest)
std::shared_ptr<TriangleMeshPlain> invalidTriMesh = std::make_shared<TriangleMeshPlain>();
for (int i = 0; i < cubeNumPoints; i++)
{
- SurgSim::Math::Vector3d p;
+ Vector3d p;
p[0] = cubePoints[i][0];
p[1] = cubePoints[i][1];
p[2] = cubePoints[i][2];
@@ -122,23 +132,24 @@ TEST_F(MeshShapeTest, InvalidMeshCubeTest)
invalidTriMesh->addTriangle(t);
}
- EXPECT_THROW(SurgSim::Math::MeshShape invalidMeshShape(*invalidTriMesh), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(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));
+ EXPECT_NO_THROW(MeshShape meshShape(*emptyMesh));
- SurgSim::Math::MeshShape meshShape(*emptyMesh);
- EXPECT_NEAR(0.0, meshShape.getVolume(), 1e-8);
+ MeshShape meshShape(*emptyMesh);
+ EXPECT_NEAR(0.0, meshShape.getVolume(), epsilon);
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());
+ MeshShape emptyMeshShape;
+ EXPECT_TRUE(emptyMeshShape.isValid());
+ EXPECT_TRUE(emptyMeshShape.getBoundingBox().isEmpty());
}
TEST_F(MeshShapeTest, ValidMeshTest)
@@ -146,8 +157,7 @@ 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();
+ auto meshShape = std::make_shared<MeshShape>(*emptyMesh);
for (int i = 0; i < cubeNumEdges; ++i)
{
@@ -157,7 +167,7 @@ TEST_F(MeshShapeTest, ValidMeshTest)
edgePoints[j] = cubeEdges[i][j];
}
TriangleMeshPlain::EdgeType e(edgePoints);
- mesh->addEdge(e);
+ meshShape->addEdge(e);
}
EXPECT_FALSE(meshShape->isValid());
@@ -166,17 +176,17 @@ TEST_F(MeshShapeTest, ValidMeshTest)
{
SCOPED_TRACE("Valid Mesh");
auto emptyMesh = std::make_shared<TriangleMeshPlain>();
- auto meshShape = std::make_shared<SurgSim::Math::MeshShape>(*emptyMesh);
- auto mesh = meshShape->getMesh();
+ auto meshShape = std::make_shared<MeshShape>(*emptyMesh);
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]);
+ Vector3d point(cubePoints[i][0], cubePoints[i][1], cubePoints[i][2]);
TriangleMeshPlain::VertexType vertex(point);
- mesh->addVertex(vertex);
+ meshShape->addVertex(vertex);
}
+
EXPECT_TRUE(meshShape->isValid());
}
}
@@ -193,7 +203,7 @@ TEST_F(MeshShapeTest, MeshCubeVSBoxTest)
std::shared_ptr<TriangleMeshPlain> mesh = std::make_shared<TriangleMeshPlain>();
for (int i = 0; i < cubeNumPoints; i++)
{
- SurgSim::Math::Vector3d p;
+ Vector3d p;
p[0] = cubePoints[i][0] * lx;
p[1] = cubePoints[i][1] * ly;
p[2] = cubePoints[i][2] * lz;
@@ -221,56 +231,54 @@ TEST_F(MeshShapeTest, MeshCubeVSBoxTest)
mesh->addTriangle(t);
}
- SurgSim::Math::MeshShape boxMesh(*mesh);
+ MeshShape boxMesh(*mesh);
- SurgSim::Math::BoxShape boxShape(lx, ly, lz);
+ BoxShape boxShape(lx, ly, lz);
- EXPECT_NEAR(boxShape.getVolume(), boxMesh.getVolume(), 1e-8);
+ EXPECT_NEAR(boxShape.getVolume(), boxMesh.getVolume(), epsilon);
EXPECT_TRUE((boxShape.getCenter() - boxMesh.getCenter()).isZero());
- EXPECT_TRUE(boxShape.getSecondMomentOfVolume().isApprox(boxMesh.getSecondMomentOfVolume(), 1e-8));
+ EXPECT_TRUE(boxShape.getSecondMomentOfVolume().isApprox(boxMesh.getSecondMomentOfVolume(), epsilon));
}
}
TEST_F(MeshShapeTest, SerializationTest)
{
- const std::string fileName = "MeshShapeData/staple_collision.ply";
- auto meshShape = std::make_shared<SurgSim::Math::MeshShape>();
+ const std::string fileName = "Geometry/staple_collision.ply";
+ auto meshShape = std::make_shared<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;
+ std::shared_ptr<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>>()));
+ std::shared_ptr<MeshShape> newMeshShape;
+ ASSERT_NO_THROW(newMeshShape = std::dynamic_pointer_cast<MeshShape>(node.as<std::shared_ptr<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_EQ(meshShape->getNumVertices(), newMeshShape->getNumVertices());
+ EXPECT_EQ(meshShape->getNumEdges(), newMeshShape->getNumEdges());
+ EXPECT_EQ(meshShape->getNumTriangles(), newMeshShape->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>();
+ const std::string fileName = "Geometry/staple_collision.ply";
+ auto meshShape = std::make_shared<MeshShape>();
EXPECT_NO_THROW(meshShape->load(fileName));
auto tree = meshShape->getAabbTree();
- auto triangles = meshShape->getMesh()->getTriangles();
- auto vertices = meshShape->getMesh()->getVertices();
+ auto triangles = meshShape->getTriangles();
+ auto vertices = meshShape->getVertices();
EXPECT_EQ(224u, triangles.size());
EXPECT_EQ(504u, vertices.size());
@@ -278,38 +286,136 @@ TEST_F(MeshShapeTest, CreateAabbTreeTest)
for (auto it = triangles.begin(); it != triangles.end(); ++it)
{
auto ids = it->verticesId;
- EXPECT_TRUE(tree->getAabb().contains(SurgSim::Math::makeAabb(
+ EXPECT_TRUE(tree->getAabb().contains(makeAabb(
vertices[ids[0]].position, vertices[ids[1]].position, vertices[ids[2]].position)));
}
+
+ EXPECT_TRUE(meshShape->getBoundingBox().isApprox(tree->getAabb()));
}
-TEST_F(MeshShapeTest, DoLoadTest)
+TEST_F(MeshShapeTest, TransformTest)
{
- SurgSim::Framework::ApplicationData data("config.txt");
+ const std::string fileName = "Geometry/staple_collision.ply";
+ auto mesh = std::make_shared<SurgSim::DataStructures::TriangleMeshPlain>();
+ ASSERT_NO_THROW(mesh->load(fileName));
+
+ auto originalMesh = std::make_shared<MeshShape>(*mesh);
+ auto actualMesh = std::make_shared<MeshShape>(*mesh);
+
+ EXPECT_TRUE(originalMesh->isTransformable());
+
+ RigidTransform3d transform = makeRigidTransform(Vector3d(4.3, 2.1, 6.5), Vector3d(-1.5, 7.5, -2.5),
+ Vector3d(8.7, -4.7, -3.1));
+ actualMesh->transform(transform);
+ EXPECT_TRUE(actualMesh->update());
+
+ Vector3d expectedPosition;
+ ASSERT_EQ(originalMesh->getNumVertices(), actualMesh->getNumVertices());
+ for (size_t i = 0; i < actualMesh->getNumVertices(); i++)
+ {
+ Vector3d expectedPosition = transform * originalMesh->getVertexPosition(i);
+ ASSERT_TRUE(actualMesh->getVertexPosition(i).isApprox(expectedPosition, epsilon)) <<
+ "Vertex #" << i << " is not as expected, remaining vertices may also be incorrect.";
+ }
+
+ Vector3d expectedNormal;
+ ASSERT_EQ(originalMesh->getNumTriangles(), actualMesh->getNumTriangles());
+ for (size_t i = 0; i < actualMesh->getNumTriangles(); i++)
{
- auto fileName = std::string("MeshShapeData/staple_collision.ply");
- auto meshShape = std::make_shared<SurgSim::Math::MeshShape>();
+ expectedNormal = transform.linear() * originalMesh->getTriangle(i).data.normal;
+ ASSERT_TRUE(expectedNormal.isApprox(actualMesh->getTriangle(i).data.normal, epsilon)) <<
+ "Normal #" << i << " is not as expected, remaining normals may also be incorrect.";
+ }
+
+ // Expect indeterminate normals due to numerical precision.
+ actualMesh->transform(makeRigidTranslation(Vector3d(1e100, 1e100, 1e100)));
+ EXPECT_FALSE(actualMesh->update());
+}
+
+TEST_F(MeshShapeTest, 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 = {0, 1};
+ std::array<size_t, 2> edgePoints12 = {1, 2};
+ std::array<size_t, 2> edgePoints20 = {2, 0};
- EXPECT_NO_THROW(meshShape->load(fileName, data));
+ TriangleMeshPlain::EdgeType e01(edgePoints01);
+ TriangleMeshPlain::EdgeType e12(edgePoints12);
+ TriangleMeshPlain::EdgeType e20(edgePoints20);
+
+ mesh->addEdge(e01);
+ mesh->addEdge(e12);
+ mesh->addEdge(e20);
+
+ // Add triangle
+ std::array<size_t, 3> trianglePoints = {0, 1, 2};
+
+ TriangleMeshPlain::TriangleType t(trianglePoints);
+ mesh->addTriangle(t);
+
+ std::shared_ptr<MeshShape> meshWithNormal = std::make_shared<MeshShape>(*mesh);
+
+ Vector3d expectedZNormal(0.0, 0.0, 1.0);
+ EXPECT_EQ(expectedZNormal, meshWithNormal->getNormal(0));
+
+ // Update new vertex location of v2 to v3
+ MeshShape::VertexType v3(Vector3d(-1.0, -1.0, 1.0));
+ SurgSim::DataStructures::Vertex<SurgSim::DataStructures::EmptyData>& v2p = meshWithNormal->getVertex(2);
+ v2p = v3;
+
+ // Recompute normals for meshWithNormal
+ EXPECT_TRUE(meshWithNormal->update());
+ Vector3d expectedXNormal(0.0, -1.0, 0.0);
+ EXPECT_EQ(expectedXNormal, meshWithNormal->getNormal(0));
+
+ v2p = v1;
+ EXPECT_FALSE(meshWithNormal->update());
+}
+
+
+TEST_F(MeshShapeTest, DoLoadTest)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ {
+ SCOPED_TRACE("Normal load should succeed");
+ auto meshShape = std::make_shared<MeshShape>();
+ EXPECT_NO_THROW(meshShape->load("Geometry/staple_collision.ply"));
EXPECT_TRUE(meshShape->isValid());
- EXPECT_EQ(fileName, meshShape->getFileName());
}
{
- auto fileName = std::string("MeshShapeData/InvalidMesh.ply");
- auto meshShape = std::make_shared<SurgSim::Math::MeshShape>();
+ SCOPED_TRACE("Load through parameter should succeed");
+ auto fileName = std::string("Geometry/staple_collision.ply");
+ auto meshShape = std::make_shared<MeshShape>();
- EXPECT_ANY_THROW(meshShape->load(fileName, data));
- EXPECT_FALSE(meshShape->isValid());
- EXPECT_EQ(fileName, meshShape->getFileName());
+ EXPECT_NO_THROW(meshShape->setValue("FileName", fileName));
+ EXPECT_TRUE(meshShape->isValid());
}
{
- auto fileName = std::string("Nonexistent file");
- auto meshShape = std::make_shared<SurgSim::Math::MeshShape>();
+ SCOPED_TRACE("Load of invalid mesh should throw");
+ auto meshShape = std::make_shared<MeshShape>();
+ EXPECT_THROW(meshShape->load("Geometry/InvalidMesh.ply"), SurgSim::Framework::AssertionFailure);
+ }
- EXPECT_ANY_THROW(meshShape->load(fileName, data));
- EXPECT_FALSE(meshShape->isValid());
- EXPECT_EQ(fileName, meshShape->getFileName());
+ {
+ SCOPED_TRACE("Load of non existant file should throw");
+ auto fileName = std::string("Nonexistent file");
+ auto meshShape = std::make_shared<MeshShape>();
+ EXPECT_THROW(meshShape->load("Nonexistent file"), SurgSim::Framework::AssertionFailure);
}
-}
\ No newline at end of file
+}
+
+};
+};
diff --git a/SurgSim/Math/UnitTests/MinMaxTests.cpp b/SurgSim/Math/UnitTests/MinMaxTests.cpp
new file mode 100644
index 0000000..0b523a1
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MinMaxTests.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 MinMax functions.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/MinMax.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+class MinMaxTests : public ::testing::Test {};
+
+
+TEST_F(MinMaxTests, TwoEntriesTests)
+{
+ const int numValues = 2;
+
+ float floatValues[numValues] = { 7, 52};
+ float minFloatVal = 7;
+ float maxFloatVal = 52;
+ float floatMin;
+ float floatMax;
+
+ double doubleValues[numValues] = { -47, -2};
+ double minDoubleVal = -47;
+ double maxDoubleVal = -2;
+ double doubleMin;
+ double doubleMax;
+
+ std::sort(floatValues, floatValues + numValues);
+ do
+ {
+ minMax(floatValues[0], floatValues[1], &floatMin, &floatMax);
+ EXPECT_EQ(minFloatVal, floatMin);
+ EXPECT_EQ(maxFloatVal, floatMax);
+ }
+ while (std::next_permutation(floatValues, floatValues + numValues));
+
+ std::sort(doubleValues, doubleValues + numValues);
+ do
+ {
+ minMax(doubleValues[0], doubleValues[1], &doubleMin, &doubleMax);
+ EXPECT_EQ(minDoubleVal, doubleMin);
+ EXPECT_EQ(maxDoubleVal, doubleMax);
+ }
+ while (std::next_permutation(doubleValues, doubleValues + numValues));
+};
+
+TEST_F(MinMaxTests, ThreeEntriesTests)
+{
+ const int numValues = 3;
+
+ float floatValues[numValues] = { 22, 7, 52};
+ float minFloatVal = 7;
+ float maxFloatVal = 52;
+ float floatMin;
+ float floatMax;
+
+ double doubleValues[numValues] = { -47, -2, -7};
+ double minDoubleVal = -47;
+ double maxDoubleVal = -2;
+ double doubleMin;
+ double doubleMax;
+
+ std::sort(floatValues, floatValues + numValues);
+ do
+ {
+ minMax(floatValues[0], floatValues[1], floatValues[2], &floatMin, &floatMax);
+ EXPECT_EQ(minFloatVal, floatMin);
+ EXPECT_EQ(maxFloatVal, floatMax);
+ }
+ while (std::next_permutation(floatValues, floatValues + numValues));
+
+ std::sort(doubleValues, doubleValues + numValues);
+ do
+ {
+ minMax(doubleValues[0], doubleValues[1], doubleValues[2], &doubleMin, &doubleMax);
+ EXPECT_EQ(minDoubleVal, doubleMin);
+ EXPECT_EQ(maxDoubleVal, doubleMax);
+ }
+ while (std::next_permutation(doubleValues, doubleValues + numValues));
+};
+
+TEST_F(MinMaxTests, FourEntriesTests)
+{
+ const int numValues = 4;
+
+ float floatValues[numValues] = { 18, 7, 52, 12};
+ float minFloatVal = 7;
+ float maxFloatVal = 52;
+ float floatMin;
+ float floatMax;
+
+ double doubleValues[numValues] = { -23, -47, -2, -17};
+ double minDoubleVal = -47;
+ double maxDoubleVal = -2;
+ double doubleMin;
+ double doubleMax;
+
+ std::sort(floatValues, floatValues + numValues);
+ do
+ {
+ minMax(floatValues[0], floatValues[1], floatValues[2], floatValues[3], &floatMin, &floatMax);
+ EXPECT_EQ(minFloatVal, floatMin);
+ EXPECT_EQ(maxFloatVal, floatMax);
+ }
+ while (std::next_permutation(floatValues, floatValues + numValues));
+
+ std::sort(doubleValues, doubleValues + numValues);
+ do
+ {
+ minMax(doubleValues[0], doubleValues[1], doubleValues[2], doubleValues[3], &doubleMin, &doubleMax);
+ EXPECT_EQ(minDoubleVal, doubleMin);
+ EXPECT_EQ(maxDoubleVal, doubleMax);
+ }
+ while (std::next_permutation(doubleValues, doubleValues + numValues));
+};
+
+TEST_F(MinMaxTests, FiveEntriesTests)
+{
+ const int numValues = 5;
+
+ float floatValues[numValues] = { 18, 7, 52, 12, 12};
+ float minFloatVal = 7;
+ float maxFloatVal = 52;
+ float floatMin;
+ float floatMax;
+
+ double doubleValues[numValues] = { -23, -47, -2, -17, -17};
+ double minDoubleVal = -47;
+ double maxDoubleVal = -2;
+ double doubleMin;
+ double doubleMax;
+
+ std::sort(floatValues, floatValues + numValues);
+ do
+ {
+ minMax(floatValues[0], floatValues[1], floatValues[2],
+ floatValues[3], floatValues[4], &floatMin, &floatMax);
+ EXPECT_EQ(minFloatVal, floatMin);
+ EXPECT_EQ(maxFloatVal, floatMax);
+ }
+ while (std::next_permutation(floatValues, floatValues + numValues));
+
+ std::sort(doubleValues, doubleValues + numValues);
+ do
+ {
+ minMax(doubleValues[0], doubleValues[1], doubleValues[2],
+ doubleValues[3], doubleValues[4], &doubleMin, &doubleMax);
+ EXPECT_EQ(minDoubleVal, doubleMin);
+ EXPECT_EQ(maxDoubleVal, doubleMax);
+ }
+ while (std::next_permutation(doubleValues, doubleValues + numValues));
+};
+
+TEST_F(MinMaxTests, AribtraryEntriesTests)
+{
+ const int numValues = 5;
+
+ float floatValues[numValues] = { 18, 7, 52, 12, 12};
+ float minFloatVal = 7;
+ float maxFloatVal = 52;
+ float floatMin;
+ float floatMax;
+
+ double doubleValues[numValues] = { -23, -47, -2, -17, -17};
+ double minDoubleVal = -47;
+ double maxDoubleVal = -2;
+ double doubleMin;
+ double doubleMax;
+
+ ASSERT_THROW(minMax(floatValues, 0, &floatMin, &floatMax), SurgSim::Framework::AssertionFailure);
+ ASSERT_THROW(minMax(floatValues, -1, &floatMin, &floatMax), SurgSim::Framework::AssertionFailure);
+
+ std::sort(floatValues, floatValues + numValues);
+ do
+ {
+ minMax(floatValues, numValues, &floatMin, &floatMax);
+ EXPECT_EQ(minFloatVal, floatMin);
+ EXPECT_EQ(maxFloatVal, floatMax);
+ }
+ while (std::next_permutation(floatValues, floatValues + numValues));
+
+ ASSERT_THROW(minMax(doubleValues, 0, &doubleMin, &doubleMax), SurgSim::Framework::AssertionFailure);
+ ASSERT_THROW(minMax(doubleValues, -1, &doubleMin, &doubleMax), SurgSim::Framework::AssertionFailure);
+
+ std::sort(doubleValues, doubleValues + numValues);
+ do
+ {
+ minMax(doubleValues, numValues, &doubleMin, &doubleMax);
+ EXPECT_EQ(minDoubleVal, doubleMin);
+ EXPECT_EQ(maxDoubleVal, doubleMax);
+ }
+ while (std::next_permutation(doubleValues, doubleValues + numValues));
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/UnitTests/MlcpTestData/mlcpOriginalTest.txt b/SurgSim/Math/UnitTests/MlcpTestData/mlcpOriginalTest.txt
index 9f83f8d..a4b93e7 100644
--- a/SurgSim/Math/UnitTests/MlcpTestData/mlcpOriginalTest.txt
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpOriginalTest.txt
@@ -23,6 +23,6 @@ HCHt = (
( 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)
+friction = ( 2.8797662206765437e-216 0 0 0 0 0 0 0 0 0 0.40000000000000002 0 0 0.40000000000000002 0 0)
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/mlcpTest001.txt b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest001.txt
index 0bbcea7..720b82c 100644
--- a/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest001.txt
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest001.txt
@@ -37,6 +37,6 @@ HCHt = (
( 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)
+friction = ( 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)
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
index f771626..59d1711 100644
--- a/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest002.txt
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest002.txt
@@ -25,6 +25,6 @@ HCHt = (
( 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)
+friction = ( 0 0 0 0 0 0 0 0 0 0 0 0 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
index 1573e8c..3395f19 100644
--- a/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest003.txt
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest003.txt
@@ -25,6 +25,6 @@ HCHt = (
( 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)
+friction = ( 0 0 0 0 0 0 0 0 0 0 0 0 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/mlcpTest009.txt b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest009.txt
index 96151c0..b05464d 100644
--- a/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest009.txt
+++ b/SurgSim/Math/UnitTests/MlcpTestData/mlcpTest009.txt
@@ -16,6 +16,6 @@ HCHt = (
( 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)
+friction = ( 0 0 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/MockCapsule.h b/SurgSim/Math/UnitTests/MockCapsule.h
new file mode 100644
index 0000000..323ce0e
--- /dev/null
+++ b/SurgSim/Math/UnitTests/MockCapsule.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_MATH_UNITTESTS_MOCKCAPSULE_H
+#define SURGSIM_MATH_UNITTESTS_MOCKCAPSULE_H
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+/// MockCapsule class used in the unit tests.
+class MockCapsule
+{
+public:
+ // Default constructor.
+ MockCapsule() {}
+
+ // Constructor.
+ MockCapsule(const Vector3d& vertex0, const Vector3d& vertex1, double radius) :
+ v0(vertex0), v1(vertex1), v0v1(vertex1 - vertex0), r(radius)
+ {}
+
+ // Move this capsule by the given vector.
+ void translate(const Vector3d& v)
+ {
+ v0 += v;
+ v1 += v;
+ }
+
+ // Rotate this capsule 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;
+ }
+
+ // Rotate this capsule 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;
+ }
+
+ // Rotate this capsule 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;
+ }
+
+ // Transform this capsule by the given matrix.
+ void transform(SurgSim::Math::RigidTransform3d transform)
+ {
+ v0 = transform * v0;
+ v1 = transform * v1;
+ }
+
+ // Vertices of this capsule.
+ Vector3d v0;
+ Vector3d v1;
+
+ // Axis of this capsule.
+ Vector3d v0v1;
+
+ // Radius
+ double r;
+};
+
+
+} // namespace Math
+
+} // namespace SurgSim
+
+#endif // SURGSIM_MATH_UNITTESTS_MOCKCAPSULE_H
\ No newline at end of file
diff --git a/SurgSim/Math/UnitTests/MockObject.h b/SurgSim/Math/UnitTests/MockObject.h
index a979b7e..08acd5b 100644
--- a/SurgSim/Math/UnitTests/MockObject.h
+++ b/SurgSim/Math/UnitTests/MockObject.h
@@ -16,7 +16,9 @@
#ifndef SURGSIM_MATH_UNITTESTS_MOCKOBJECT_H
#define SURGSIM_MATH_UNITTESTS_MOCKOBJECT_H
+#include <Eigen/Core>
#include "SurgSim/Math/OdeEquation.h"
+#include "SurgSim/Math/OdeSolver.h"
#include "SurgSim/Math/OdeState.h"
namespace SurgSim
@@ -44,12 +46,12 @@ public:
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)
+ m_gravity(0.0, -9.81, 0.0)
{
+ m_f.resize(3);
+ m_M.resize(3, 3);
+ m_D.resize(3, 3);
+ m_K.resize(3, 3);
this->m_initialState = std::make_shared<MassPointState>();
}
@@ -58,58 +60,57 @@ public:
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
+ Matrix applyCompliance(const OdeState& state, const Matrix& b) override
+ {
+ SURGSIM_ASSERT(m_odeSolver) << "Ode solver not initialized, it should have been initialized on wake-up";
+
+ Math::Matrix bTemp = b;
+ for (auto condition : state.getBoundaryConditions())
+ {
+ Math::zeroRow(condition, &bTemp);
+ }
+ auto solution = m_odeSolver->getLinearSolver()->solve(bTemp);
+ for (auto condition : state.getBoundaryConditions())
+ {
+ Math::zeroRow(condition, &solution);
+ }
+ return solution;
+ }
+
+ void setOdeSolver(std::shared_ptr<SurgSim::Math::OdeSolver> solver)
+ {
+ m_odeSolver = solver;
+ }
+
+ /// Ode solver (its type depends on the numerical integration scheme)
+ std::shared_ptr<SurgSim::Math::OdeSolver> m_odeSolver;
+ double m_mass, m_viscosity;
+ Vector3d m_gravity;
+
+protected:
+ void 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
+ void 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
+ void 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)
+ void computeK(const OdeState& state) override
{
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
+ void computeFMDK(const OdeState& state) override
{
m_M.setIdentity();
m_M *= m_mass;
@@ -117,17 +118,7 @@ public:
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;
};
@@ -151,10 +142,13 @@ class MassPointsForStatic : public SurgSim::Math::OdeEquation
public:
/// Constructor
MassPointsForStatic() :
- m_f(9),
- m_gravityForce(9),
- m_K(9, 9)
+ m_gravityForce(9)
{
+ m_f.resize(9);
+ m_M.resize(9, 9);
+ m_D.resize(9, 9);
+ m_K.resize(9, 9);
+
m_f.setZero();
m_K.setZero();
m_gravityForce.setZero();
@@ -169,55 +163,178 @@ public:
return m_gravityForce;
}
- virtual Vector& computeF(const OdeState& state) override
+ Matrix applyCompliance(const OdeState& state, const Matrix& b) override
{
- // Internale deformation forces
- m_f = -computeK(state) * (state.getPositions() - m_initialState->getPositions());
+ SURGSIM_ASSERT(m_odeSolver) << "Ode solver not initialized, it should have been initialized on wake-up";
+
+ Math::Matrix bTemp = b;
+ for (auto condition : state.getBoundaryConditions())
+ {
+ Math::zeroRow(condition, &bTemp);
+ }
+ auto solution = m_odeSolver->getLinearSolver()->solve(bTemp);
+ for (auto condition : state.getBoundaryConditions())
+ {
+ Math::zeroRow(condition, &solution);
+ }
+ return solution;
+ }
+
+ void setOdeSolver(std::shared_ptr<SurgSim::Math::OdeSolver> solver)
+ {
+ m_odeSolver = solver;
+ }
+
+protected:
+ void computeF(const OdeState& state) override
+ {
+ computeK(state);
+
+ // Internal deformation forces
+ m_f = -m_K * (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
+ void computeM(const OdeState& state) override
{
m_M.setZero();
- return m_M;
}
- virtual const Matrix& computeD(const OdeState& state) override
+ void computeD(const OdeState& state) override
{
m_D.setZero();
- return m_D;
}
- virtual const Matrix& computeK(const OdeState& state)
+ void computeK(const OdeState& state) override
{
// 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;
+ // Desired matrix is:
+ //
+ // 1 0 0 0 0 0 0 0 0
+ // 0 1 0 0 0 0 0 0 0
+ // 0 0 1 0 0 0 0 0 0
+ // 0 0 0 10 2 2 2 2 2
+ // 0 0 0 2 10 2 2 2 2
+ // 0 0 0 2 2 10 2 2 2
+ // 0 0 0 2 2 2 10 2 2
+ // 0 0 0 2 2 2 2 10 2
+ // 0 0 0 2 2 2 2 2 10
+ //
+ // Generate it using sparse matrix notation.
+
+ m_K.resize(9, 9);
+ typedef Eigen::Triplet<double> T;
+ std::vector<T> tripletList;
+ tripletList.reserve(39);
+
+ // Upper 3x3 identity block
+ for (int counter = 0; counter < 3; ++counter)
+ {
+ tripletList.push_back(T(counter, counter, 1.0));
+ }
+
+ for (int row = 3; row < 9; ++row)
+ {
+ // Diagonal is 8 more than the rest of the 6x6 block
+ tripletList.push_back(T(row, row, 8.0));
+
+ // Now add in the 2's over the entire block
+ for (int col = 3; col < 9; ++col)
+ {
+ tripletList.push_back(T(row, col, 2.0));
+ }
+ }
+ m_K.setFromTriplets(tripletList.begin(), tripletList.end());
}
- virtual void computeFMDK(const OdeState& state, Vector** f, Matrix** M, Matrix** D, Matrix** K)
- override
+ void computeFMDK(const OdeState& state) override
{
- m_f = computeF(state);
- m_M = computeM(state);
- m_D = computeD(state);
- m_K = computeK(state);
+ computeF(state);
+ computeM(state);
+ computeD(state);
+ computeK(state);
+ }
- *f = &m_f;
- *K = &m_K;
- *D = &m_D;
- *M = &m_M;
+private:
+ /// Ode solver (its type depends on the numerical integration scheme)
+ std::shared_ptr<SurgSim::Math::OdeSolver> m_odeSolver;
+ Vector m_gravityForce;
+};
+
+/// Class for the complex non-linear ODE a = x.v^2
+class OdeComplexNonLinear : public OdeEquation
+{
+public:
+ /// Constructor
+ OdeComplexNonLinear()
+ {
+ m_f.resize(3);
+ m_M.resize(3, 3);
+ m_D.resize(3, 3);
+ m_K.resize(3, 3);
+ this->m_initialState = std::make_shared<MassPointState>();
+ }
+
+ Matrix applyCompliance(const OdeState& state, const Matrix& b) override
+ {
+ SURGSIM_ASSERT(m_odeSolver) << "Ode solver not initialized, it should have been initialized on wake-up";
+
+ Math::Matrix bTemp = b;
+ for (auto condition : state.getBoundaryConditions())
+ {
+ Math::zeroRow(condition, &bTemp);
+ }
+ auto solution = m_odeSolver->getLinearSolver()->solve(bTemp);
+ for (auto condition : state.getBoundaryConditions())
+ {
+ Math::zeroRow(condition, &solution);
+ }
+ return solution;
+ }
+
+ void setOdeSolver(std::shared_ptr<SurgSim::Math::OdeSolver> solver)
+ {
+ m_odeSolver = solver;
+ }
+
+protected:
+ void computeF(const OdeState& state) override
+ {
+ double v2 = state.getVelocities().dot(state.getVelocities());
+ m_f = v2 * state.getPositions();
+ }
+
+ void computeM(const OdeState& state) override
+ {
+ m_M.setIdentity();
+ }
+
+ void computeD(const OdeState& state) override
+ {
+ m_D = 2.0 * state.getPositions() * state.getVelocities().transpose().sparseView();
+ }
+
+ void computeK(const OdeState& state) override
+ {
+ m_K.resize(static_cast<SparseMatrix::Index>(state.getNumDof()),
+ static_cast<SparseMatrix::Index>(state.getNumDof()));
+ m_K.setIdentity();
+ m_K *= state.getVelocities().squaredNorm();
+ }
+
+ void computeFMDK(const OdeState& state) override
+ {
+ computeF(state);
+ computeM(state);
+ computeD(state);
+ computeK(state);
}
private:
- Vector m_f, m_gravityForce;
- Matrix m_M, m_D, m_K;
+ /// Ode solver (its type depends on the numerical integration scheme)
+ std::shared_ptr<SurgSim::Math::OdeSolver> m_odeSolver;
};
}; // Math
diff --git a/SurgSim/Math/UnitTests/MockTriangle.h b/SurgSim/Math/UnitTests/MockTriangle.h
index b49eaa3..0c18bc3 100644
--- a/SurgSim/Math/UnitTests/MockTriangle.h
+++ b/SurgSim/Math/UnitTests/MockTriangle.h
@@ -84,6 +84,15 @@ public:
n = r * n;
}
+ // Transform this triangle by the given matrix.
+ void transform(SurgSim::Math::RigidTransform3d transform)
+ {
+ v0 = transform * v0;
+ v1 = transform * v1;
+ v2 = transform * v2;
+ n = transform.linear() * n;
+ }
+
// Vertices of this triangle.
Vector3d v0;
Vector3d v1;
diff --git a/SurgSim/Math/UnitTests/OdeEquationTests.cpp b/SurgSim/Math/UnitTests/OdeEquationTests.cpp
index 40e190e..8793eff 100644
--- a/SurgSim/Math/UnitTests/OdeEquationTests.cpp
+++ b/SurgSim/Math/UnitTests/OdeEquationTests.cpp
@@ -60,25 +60,20 @@ TEST(OdeEquationTests, ComputesTest)
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));
+ m.updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_F);
+ EXPECT_TRUE(m.getF().isApprox(expectedF));
+ m.updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_M);
+ EXPECT_TRUE(m.getM().isApprox(expectedM));
+ m.updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_D);
+ EXPECT_TRUE(m.getD().isApprox(expectedD));
+ m.updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_K);
+ EXPECT_TRUE(m.getK().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));
+ m.updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_FMDK);
+ EXPECT_TRUE(m.getF().isApprox(expectedF));
+ EXPECT_TRUE(m.getM().isApprox(expectedM));
+ EXPECT_TRUE(m.getD().isApprox(expectedD));
+ EXPECT_TRUE(m.getK().isApprox(expectedK));
}
}
@@ -93,25 +88,20 @@ TEST(OdeEquationTests, ComputesTest)
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));
+ m.updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_F);
+ EXPECT_TRUE(m.getF().isApprox(expectedF));
+ m.updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_M);
+ EXPECT_TRUE(m.getM().isApprox(expectedM));
+ m.updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_D);
+ EXPECT_TRUE(m.getD().isApprox(expectedD));
+ m.updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_K);
+ EXPECT_TRUE(m.getK().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));
+ m.updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_FMDK);
+ EXPECT_TRUE(m.getF().isApprox(expectedF));
+ EXPECT_TRUE(m.getM().isApprox(expectedM));
+ EXPECT_TRUE(m.getD().isApprox(expectedD));
+ EXPECT_TRUE(m.getK().isApprox(expectedK));
}
}
}
diff --git a/SurgSim/Math/UnitTests/OdeSolverEulerExplicitModifiedTests.cpp b/SurgSim/Math/UnitTests/OdeSolverEulerExplicitModifiedTests.cpp
index e57ea46..d484ef6 100644
--- a/SurgSim/Math/UnitTests/OdeSolverEulerExplicitModifiedTests.cpp
+++ b/SurgSim/Math/UnitTests/OdeSolverEulerExplicitModifiedTests.cpp
@@ -31,12 +31,15 @@ namespace SurgSim
namespace Math
{
+namespace
+{
template<class T>
void doConstructorTest()
{
MassPoint m;
ASSERT_NO_THROW({T solver(&m);});
}
+};
TEST(OdeSolverEulerExplicitModified, ConstructorTest)
{
@@ -50,20 +53,23 @@ TEST(OdeSolverEulerExplicitModified, ConstructorTest)
}
}
+namespace
+{
template<class T>
-void doSolveTest()
+void doSolveTest(bool computeCompliance)
{
// 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);
+ auto solver = std::make_shared<T>(&m);
+ m.setOdeSolver(solver);
// 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);});
+ ASSERT_NO_THROW({solver->solve(1e-3, state0, &state1, computeCompliance);});
EXPECT_EQ(defaultState, state0);
EXPECT_NE(defaultState, state1);
EXPECT_TRUE(state1.getVelocities().isApprox(m.m_gravity * 1e-3 + state0.getVelocities()));
@@ -71,7 +77,7 @@ void doSolveTest()
// v(2) = g.dt + v(1)
// x(2) = v(2).dt + x(1)
- ASSERT_NO_THROW({solver.solve(1e-3, state1, &state2);});
+ ASSERT_NO_THROW({solver->solve(1e-3, state1, &state2, computeCompliance);});
EXPECT_NE(defaultState, state1);
EXPECT_NE(defaultState, state2);
EXPECT_NE(state2, state1);
@@ -82,12 +88,13 @@ void doSolveTest()
{
MassPoint m(0.1);
MassPointState defaultState, state0, state1, state2;
- T solver(&m);
+ auto solver = std::make_shared<T>(&m);
+ m.setOdeSolver(solver);
// 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);});
+ ASSERT_NO_THROW({solver->solve(1e-3, state0, &state1, computeCompliance);});
EXPECT_EQ(defaultState, state0);
EXPECT_NE(defaultState, state1);
Vector3d acceleration0 = m.m_gravity - 0.1 * state0.getVelocities() / m.m_mass;
@@ -96,7 +103,7 @@ void doSolveTest()
// 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);});
+ ASSERT_NO_THROW({solver->solve(1e-3, state1, &state2, computeCompliance);});
EXPECT_NE(defaultState, state1);
EXPECT_NE(defaultState, state2);
EXPECT_NE(state1, state2);
@@ -105,16 +112,57 @@ void doSolveTest()
EXPECT_TRUE(state2.getPositions().isApprox(state2.getVelocities() * 1e-3 + state1.getPositions()));
}
}
+};
TEST(OdeSolverEulerExplicitModified, SolveTest)
{
{
+ SCOPED_TRACE("EulerExplicitModified computing the compliance matrix");
+ doSolveTest<OdeSolverEulerExplicitModified>(true);
+ }
+ {
+ SCOPED_TRACE("EulerExplicitModified not computing the compliance matrix");
+ doSolveTest<OdeSolverEulerExplicitModified>(false);
+ }
+
+ {
+ SCOPED_TRACE("LinearEulerExplicitModified computing the compliance matrix");
+ doSolveTest<OdeSolverLinearEulerExplicitModified>(true);
+ }
+ {
+ SCOPED_TRACE("LinearEulerExplicitModified not computing the compliance matrix");
+ doSolveTest<OdeSolverLinearEulerExplicitModified>(false);
+ }
+}
+
+namespace
+{
+template <class T>
+void doComputeMatricesTest()
+{
+ MassPoint m;
+ T solver(&m);
+ MassPointState state;
+ double dt = 1e-3;
+
+ m.updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Matrix expectedSystemMatrix = m.getM() / dt;
+ EXPECT_NO_THROW(solver.computeMatrices(dt, state));
+ EXPECT_TRUE(solver.getSystemMatrix().isApprox(expectedSystemMatrix));
+ EXPECT_TRUE(solver.getComplianceMatrix().isApprox(expectedSystemMatrix.inverse()));
+}
+};
+
+TEST(OdeSolverEulerExplicitModified, ComputeMatricesTest)
+{
+ {
SCOPED_TRACE("EulerExplicitModified");
- doSolveTest<OdeSolverEulerExplicitModified>();
+ doComputeMatricesTest<OdeSolverEulerExplicitModified>();
}
+
{
SCOPED_TRACE("LinearEulerExplicitModified");
- doSolveTest<OdeSolverLinearEulerExplicitModified>();
+ doComputeMatricesTest<OdeSolverLinearEulerExplicitModified>();
}
}
diff --git a/SurgSim/Math/UnitTests/OdeSolverEulerExplicitTests.cpp b/SurgSim/Math/UnitTests/OdeSolverEulerExplicitTests.cpp
index 3ac254b..2a1501d 100644
--- a/SurgSim/Math/UnitTests/OdeSolverEulerExplicitTests.cpp
+++ b/SurgSim/Math/UnitTests/OdeSolverEulerExplicitTests.cpp
@@ -31,12 +31,15 @@ namespace SurgSim
namespace Math
{
+namespace
+{
template<class T>
void doConstructorTest()
{
MassPoint m;
ASSERT_NO_THROW({T solver(&m);});
}
+};
TEST(OdeSolverEulerExplicit, ConstructorTest)
{
@@ -50,20 +53,23 @@ TEST(OdeSolverEulerExplicit, ConstructorTest)
}
}
+namespace
+{
template<class T>
-void doSolveTest()
+void doSolveTest(bool computeCompliance)
{
// 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);
+ auto solver = std::make_shared<T>(&m);
+ m.setOdeSolver(solver);
// 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);});
+ ASSERT_NO_THROW({solver->solve(1e-3, state0, &state1, computeCompliance);});
EXPECT_EQ(defaultState, state0);
EXPECT_NE(defaultState, state1);
EXPECT_TRUE(state1.getVelocities().isApprox(m.m_gravity * 1e-3 + state0.getVelocities()));
@@ -71,7 +77,7 @@ void doSolveTest()
// v(2) = g.dt + v(1)
// x(2) = v(1).dt + x(1)
- ASSERT_NO_THROW({solver.solve(1e-3, state1, &state2);});
+ ASSERT_NO_THROW({solver->solve(1e-3, state1, &state2, computeCompliance);});
EXPECT_NE(defaultState, state1);
EXPECT_NE(defaultState, state2);
EXPECT_NE(state2, state1);
@@ -82,12 +88,13 @@ void doSolveTest()
{
MassPoint m(0.1);
MassPointState defaultState, state0, state1, state2;
- T solver(&m);
+ auto solver = std::make_shared<T>(&m);
+ m.setOdeSolver(solver);
// 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);});
+ ASSERT_NO_THROW({solver->solve(1e-3, state0, &state1, computeCompliance);});
EXPECT_EQ(defaultState, state0);
EXPECT_NE(defaultState, state1);
Vector3d acceleration0 = m.m_gravity - 0.1 * state0.getVelocities() / m.m_mass;
@@ -96,7 +103,7 @@ void doSolveTest()
// 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);});
+ ASSERT_NO_THROW({solver->solve(1e-3, state1, &state2, computeCompliance);});
EXPECT_NE(defaultState, state1);
EXPECT_NE(defaultState, state2);
EXPECT_NE(state1, state2);
@@ -105,16 +112,56 @@ void doSolveTest()
EXPECT_TRUE(state2.getPositions().isApprox(state1.getVelocities() * 1e-3 + state1.getPositions()));
}
}
+};
TEST(OdeSolverEulerExplicit, SolveTest)
{
{
+ SCOPED_TRACE("EulerExplicit computing the compliance matrix");
+ doSolveTest<OdeSolverEulerExplicit>(true);
+ }
+ {
+ SCOPED_TRACE("EulerExplicit not computing the compliance matrix");
+ doSolveTest<OdeSolverEulerExplicit>(false);
+ }
+ {
+ SCOPED_TRACE("LinearEulerExplicit computing the compliance matrix");
+ doSolveTest<OdeSolverLinearEulerExplicit>(true);
+ }
+ {
+ SCOPED_TRACE("LinearEulerExplicit not computing the compliance matrix");
+ doSolveTest<OdeSolverLinearEulerExplicit>(false);
+ }
+}
+
+namespace
+{
+template <class T>
+void doComputeMatricesTest()
+{
+ MassPoint m;
+ T solver(&m);
+ MassPointState state;
+ double dt = 1e-3;
+
+ m.updateFMDK(state, ODEEQUATIONUPDATE_M);
+ Matrix expectedSystemMatrix = m.getM() / dt;
+ EXPECT_NO_THROW(solver.computeMatrices(dt, state));
+ EXPECT_TRUE(solver.getSystemMatrix().isApprox(expectedSystemMatrix));
+ EXPECT_TRUE(solver.getComplianceMatrix().isApprox(expectedSystemMatrix.inverse()));
+}
+};
+
+TEST(OdeSolverEulerExplicit, ComputeMatricesTest)
+{
+ {
SCOPED_TRACE("EulerExplicit");
- doSolveTest<OdeSolverEulerExplicit>();
+ doComputeMatricesTest<OdeSolverEulerExplicit>();
}
+
{
SCOPED_TRACE("LinearEulerExplicit");
- doSolveTest<OdeSolverLinearEulerExplicit>();
+ doComputeMatricesTest<OdeSolverLinearEulerExplicit>();
}
}
diff --git a/SurgSim/Math/UnitTests/OdeSolverEulerImplicitTests.cpp b/SurgSim/Math/UnitTests/OdeSolverEulerImplicitTests.cpp
index 40cd227..ced9c83 100644
--- a/SurgSim/Math/UnitTests/OdeSolverEulerImplicitTests.cpp
+++ b/SurgSim/Math/UnitTests/OdeSolverEulerImplicitTests.cpp
@@ -31,12 +31,15 @@ namespace SurgSim
namespace Math
{
+namespace
+{
template<class T>
void doConstructorTest()
{
MassPoint m;
ASSERT_NO_THROW({T solver(&m);});
}
+};
TEST(OdeSolverEulerImplicit, ConstructorTest)
{
@@ -50,20 +53,24 @@ TEST(OdeSolverEulerImplicit, ConstructorTest)
}
}
+namespace
+{
template<class T>
-void doSolveTest()
+void doSolveTest(bool computeCompliance)
{
// 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);
+ auto solver = std::make_shared<T>(&m);
+ m.setOdeSolver(solver);
+ solver->setNewtonRaphsonMaximumIteration(1);
// 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);});
+ ASSERT_NO_THROW({solver->solve(1e-3, state0, &state1, computeCompliance);});
EXPECT_EQ(defaultState, state0);
EXPECT_NE(defaultState, state1);
EXPECT_TRUE(state1.getVelocities().isApprox(m.m_gravity * 1e-3 + state0.getVelocities()));
@@ -71,7 +78,7 @@ void doSolveTest()
// v(2) = g.dt + v(1)
// x(2) = v(2).dt + x(1)
- ASSERT_NO_THROW({solver.solve(1e-3, state1, &state2);});
+ ASSERT_NO_THROW({solver->solve(1e-3, state1, &state2, computeCompliance);});
EXPECT_NE(defaultState, state1);
EXPECT_NE(defaultState, state2);
EXPECT_NE(state2, state1);
@@ -82,12 +89,14 @@ void doSolveTest()
{
MassPoint m(0.1);
MassPointState defaultState, state0, state1, state2;
- T solver(&m);
+ auto solver = std::make_shared<T>(&m);
+ m.setOdeSolver(solver);
+ solver->setNewtonRaphsonMaximumIteration(1);
// 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);});
+ ASSERT_NO_THROW({solver->solve(1e-3, state0, &state1, computeCompliance);});
EXPECT_EQ(defaultState, state0);
EXPECT_NE(defaultState, state1);
Matrix33d systemInverse = Matrix33d::Identity() * 1.0 / (1.0 + 1e-3 * 0.1 / m.m_mass);
@@ -96,7 +105,7 @@ void doSolveTest()
// 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);});
+ ASSERT_NO_THROW({solver->solve(1e-3, state1, &state2, computeCompliance);});
EXPECT_NE(defaultState, state1);
EXPECT_NE(defaultState, state2);
EXPECT_NE(state1, state2);
@@ -104,16 +113,141 @@ void doSolveTest()
EXPECT_TRUE(state2.getPositions().isApprox(state2.getVelocities() * 1e-3 + state1.getPositions()));
}
}
+};
TEST(OdeSolverEulerImplicit, SolveTest)
{
{
+ SCOPED_TRACE("EulerImplicit computing the compliance matrix");
+ doSolveTest<OdeSolverEulerImplicit>(true);
+ }
+ {
+ SCOPED_TRACE("EulerImplicit not computing the compliance matrix");
+ doSolveTest<OdeSolverEulerImplicit>(false);
+ }
+
+ {
+ SCOPED_TRACE("LinearEulerImplicit computing the compliance matrix");
+ doSolveTest<OdeSolverLinearEulerImplicit>(true);
+ }
+ {
+ SCOPED_TRACE("LinearEulerImplicit not computing the compliance matrix");
+ doSolveTest<OdeSolverLinearEulerImplicit>(false);
+ }
+}
+
+namespace
+{
+template<class T>
+void doComplexNonLinearOdeTest(size_t numNewtonRaphsonIteration, bool expectExactSolution)
+{
+ OdeComplexNonLinear odeEquation;
+ MassPointState state0, state1, state2;
+ state0.getPositions().setLinSpaced(1.4, 5.67);
+ state0.getVelocities().setLinSpaced(-0.4, -0.3);
+ double dt = 1e-3;
+ auto solver = std::make_shared<T>(&odeEquation);
+ odeEquation.setOdeSolver(solver);
+ solver->setNewtonRaphsonMaximumIteration(numNewtonRaphsonIteration);
+ solver->setNewtonRaphsonEpsilonConvergence(1e-13);
+
+ ASSERT_NO_THROW({solver->solve(dt, state0, &state1);});
+ // We do 2 iterations as Linear ode solver will do the 1st iteration as a normal non-linear ode solver
+ ASSERT_NO_THROW({solver->solve(dt, state1, &state2);});
+
+ EXPECT_NE(state0, state1);
+ EXPECT_NE(state1, state2);
+
+ // The new state should verify the following equation:
+ // {x(t+dt) = x(t) + dt.v(t+dt)
+ // {v(t+dt) = v(t) + dt.M^-1.f(x(t+dt), v(t+dt))
+ // with the notes that M = Id and f(x,v)=x.v^2
+ // BUT the function f is non-linear enough that a single iteration of the Newton-Raphson won't produce
+ // a solution close enough to the real solution of the problem. More iterations would be required to
+ // refine the solution.
+ auto xt = state1.getPositions();
+ auto vt = state1.getVelocities();
+ auto xt_plus_dt = state2.getPositions();
+ auto vt_plus_dt = state2.getVelocities();
+ odeEquation.updateFMDK(state2, ODEEQUATIONUPDATE_F);
+ auto ft_plus_dt = odeEquation.getF();
+ Vector expectedVelocity = vt + dt * ft_plus_dt;
+
+ // This is always true by construction
+ EXPECT_TRUE(xt_plus_dt.isApprox(xt + dt * vt_plus_dt));
+
+ if (!expectExactSolution)
+ {
+ EXPECT_FALSE(xt_plus_dt.isApprox(xt + dt * expectedVelocity));
+ EXPECT_FALSE(vt_plus_dt.isApprox(expectedVelocity));
+ }
+ else
+ {
+ EXPECT_TRUE(xt_plus_dt.isApprox(xt + dt * expectedVelocity));
+ EXPECT_TRUE(vt_plus_dt.isApprox(expectedVelocity));
+ }
+}
+};
+
+TEST(OdeSolverEulerImplicit, VerifyComplexNonLinearOdeTest)
+{
+ // OdeSolverEulerImplicit should succeed with enough number of Newton-Raphson iterations.
+ {
+ SCOPED_TRACE("A single Newton-Raphson iteration, using OdeSolverEulerImplicit");
+
+ doComplexNonLinearOdeTest<OdeSolverEulerImplicit>(1, false);
+ }
+
+ {
+ SCOPED_TRACE("Multiple Newton-Raphson iterations, using OdeSolverEulerImplicit");
+
+ doComplexNonLinearOdeTest<OdeSolverEulerImplicit>(10, true);
+ }
+
+ // OdeSolverLinearEulerImplicit should fail no matter the number of Newton-Raphson iterations
+ // it just does not handle non-linear problems.
+ {
+ SCOPED_TRACE("A single Newton-Raphson iteration, using OdeSolverLinearEulerImplicit");
+
+ doComplexNonLinearOdeTest<OdeSolverLinearEulerImplicit>(1, false);
+ }
+
+ {
+ SCOPED_TRACE("Multiple Newton-Raphson iterations, using OdeSolverLinearEulerImplicit");
+
+ doComplexNonLinearOdeTest<OdeSolverLinearEulerImplicit>(10, false);
+ }
+}
+
+namespace
+{
+template <class T>
+void doComputeMatricesTest()
+{
+ MassPoint m;
+ auto solver = std::make_shared<T>(&m);
+ m.setOdeSolver(solver);
+ MassPointState state;
+ double dt = 1e-3;
+
+ m.updateFMDK(state, ODEEQUATIONUPDATE_M | ODEEQUATIONUPDATE_D | ODEEQUATIONUPDATE_K);
+ Matrix expectedSystemMatrix = m.getM() / dt + m.getD() + dt * m.getK();
+ EXPECT_NO_THROW(solver->computeMatrices(dt, state));
+ EXPECT_TRUE(solver->getSystemMatrix().isApprox(expectedSystemMatrix));
+ EXPECT_TRUE(solver->getComplianceMatrix().isApprox(expectedSystemMatrix.inverse()));
+}
+};
+
+TEST(OdeSolverEulerImplicit, ComputeMatricesTest)
+{
+ {
SCOPED_TRACE("EulerImplicit");
- doSolveTest<OdeSolverEulerImplicit>();
+ doComputeMatricesTest<OdeSolverEulerImplicit>();
}
+
{
SCOPED_TRACE("LinearEulerImplicit");
- doSolveTest<OdeSolverLinearEulerImplicit>();
+ doComputeMatricesTest<OdeSolverLinearEulerImplicit>();
}
}
diff --git a/SurgSim/Math/UnitTests/OdeSolverRungeKutta4Tests.cpp b/SurgSim/Math/UnitTests/OdeSolverRungeKutta4Tests.cpp
index 6ae7165..e4a1347 100644
--- a/SurgSim/Math/UnitTests/OdeSolverRungeKutta4Tests.cpp
+++ b/SurgSim/Math/UnitTests/OdeSolverRungeKutta4Tests.cpp
@@ -30,12 +30,15 @@ namespace SurgSim
namespace Math
{
+namespace
+{
template<class T>
void doConstructorTest()
{
MassPoint m;
ASSERT_NO_THROW({T solver(&m);});
}
+};
TEST(OdeSolverRungeKutta4, ConstructorTest)
{
@@ -53,16 +56,16 @@ namespace
{
struct RungeKuttaState
{
- RungeKuttaState(){}
- RungeKuttaState(const Vector& p, const Vector& v) : position(p), velocity(v){}
+ 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){}
+ RungeKuttaDerivedState() {}
+ RungeKuttaDerivedState(const Vector& v, const Vector& a) : velocity(v), acceleration(a) {}
Vector velocity;
Vector acceleration;
};
@@ -101,14 +104,13 @@ void integrateRK4(double dt, const MassPoint& m, const RungeKuttaState& yn, Rung
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));
+ (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));
+ (k1.acceleration + k4.acceleration + 2.0 * (k2.acceleration + k3.acceleration));
}
-};
template<class T>
-void doSolveTest()
+void doSolveTest(bool computeCompliance)
{
Vector deltaWithoutViscosity;
Vector deltaWithViscosity;
@@ -124,7 +126,7 @@ void doSolveTest()
newState = defaultState;
T solver(&m);
- ASSERT_NO_THROW({solver.solve(dt, currentState, &newState);});
+ ASSERT_NO_THROW({solver.solve(dt, currentState, &newState, computeCompliance);});
EXPECT_EQ(defaultState, currentState);
EXPECT_NE(defaultState, newState);
@@ -150,7 +152,7 @@ void doSolveTest()
newState = defaultState;
T solver(&m);
- ASSERT_NO_THROW({solver.solve(dt, currentState, &newState);});
+ ASSERT_NO_THROW({solver.solve(dt, currentState, &newState, computeCompliance);});
EXPECT_EQ(defaultState, currentState);
EXPECT_NE(defaultState, newState);
@@ -180,7 +182,8 @@ void doSolveTest()
currentState.getPositions().setLinSpaced(1.0, 3.0);
currentState.getVelocities().setConstant(1.0);
- T solver(&m);
+ auto solver = std::make_shared<T>(&m);
+ m.setOdeSolver(solver);
RungeKuttaState yn(currentState.getPositions(), currentState.getVelocities());
RungeKuttaState yn_plus_1, yn_plus_2;
@@ -190,14 +193,14 @@ void doSolveTest()
// 1st time step
integrateRK4(dt, m, yn, &yn_plus_1);
- ASSERT_NO_THROW({solver.solve(dt, currentState, &newState);});
+ ASSERT_NO_THROW({solver->solve(dt, currentState, &newState, computeCompliance);});
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);});
+ ASSERT_NO_THROW({solver->solve(dt, newState, &newState2, computeCompliance);});
EXPECT_TRUE(newState2.getPositions().isApprox(yn_plus_2.position));
EXPECT_TRUE(newState2.getVelocities().isApprox(yn_plus_2.velocity));
@@ -213,8 +216,8 @@ void doSolveTest()
currentState.getPositions().setLinSpaced(1.0, 3.0);
currentState.getVelocities().setConstant(1.0);
- T solver(&m);
-
+ auto solver = std::make_shared<T>(&m);
+ m.setOdeSolver(solver);
RungeKuttaState yn(currentState.getPositions(), currentState.getVelocities());
RungeKuttaState yn_plus_1, yn_plus_2;
@@ -223,27 +226,69 @@ void doSolveTest()
// 1st time step
integrateRK4(dt, m, yn, &yn_plus_1);
- ASSERT_NO_THROW({solver.solve(dt, currentState, &newState);});
+ ASSERT_NO_THROW({solver->solve(dt, currentState, &newState, computeCompliance);});
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);});
+ ASSERT_NO_THROW({solver->solve(dt, newState, &newState2, computeCompliance);});
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("OdeSolverRungeKutta4 computing the compliance matrix");
+ doSolveTest<OdeSolverRungeKutta4>(true);
}
{
- SCOPED_TRACE("OdeSolverLinearRungeKutta4");
- doSolveTest<OdeSolverLinearRungeKutta4>();
+ SCOPED_TRACE("OdeSolverRungeKutta4 not computing the compliance matrix");
+ doSolveTest<OdeSolverRungeKutta4>(false);
+ }
+
+ {
+ SCOPED_TRACE("OdeSolverLinearRungeKutta4 computing the compliance matrix");
+ doSolveTest<OdeSolverLinearRungeKutta4>(true);
+ }
+ {
+ SCOPED_TRACE("OdeSolverLinearRungeKutta4 not computing the compliance matrix");
+ doSolveTest<OdeSolverLinearRungeKutta4>(false);
+ }
+}
+
+namespace
+{
+template <class T>
+void doComputeMatricesTest()
+{
+ MassPoint m;
+ auto solver = std::make_shared<T>(&m);
+ m.setOdeSolver(solver);
+ MassPointState state;
+ double dt = 1e-3;
+
+ m.updateFMDK(state, ODEEQUATIONUPDATE_M);
+ Matrix expectedSystemMatrix = m.getM() / dt;
+ EXPECT_NO_THROW(solver->computeMatrices(dt, state));
+ EXPECT_TRUE(solver->getSystemMatrix().isApprox(expectedSystemMatrix));
+ EXPECT_TRUE(solver->getComplianceMatrix().isApprox(expectedSystemMatrix.inverse()));
+}
+};
+
+TEST(OdeSolverRungeKutta4, ComputeMatricesTest)
+{
+ {
+ SCOPED_TRACE("RungeKutta4");
+ doComputeMatricesTest<OdeSolverRungeKutta4>();
+ }
+
+ {
+ SCOPED_TRACE("LinearRungeKutta4");
+ doComputeMatricesTest<OdeSolverLinearRungeKutta4>();
}
}
diff --git a/SurgSim/Math/UnitTests/OdeSolverStaticTests.cpp b/SurgSim/Math/UnitTests/OdeSolverStaticTests.cpp
index 9ec7fe6..ea43fcc 100644
--- a/SurgSim/Math/UnitTests/OdeSolverStaticTests.cpp
+++ b/SurgSim/Math/UnitTests/OdeSolverStaticTests.cpp
@@ -31,13 +31,15 @@ namespace SurgSim
namespace Math
{
+namespace
+{
template<class T>
void doConstructorTest()
{
MassPointsForStatic m;
ASSERT_NO_THROW({T solver(&m);});
}
-
+};
TEST(OdeSolverStatic, ConstructorTest)
{
{
@@ -50,33 +52,83 @@ TEST(OdeSolverStatic, ConstructorTest)
}
}
+namespace
+{
template<class T>
-void doSolveTest()
+void doSolveTest(bool computeCompliance)
{
MassPointsForStatic m;
MassPointsStateForStatic defaultState, currentState, newState;
- T solver(&m);
- ASSERT_NO_THROW({solver.solve(1e-3, currentState, &newState);});
+ auto solver = std::make_shared<T>(&m);
+ EXPECT_NO_THROW(m.setOdeSolver(solver));
+
+ ASSERT_NO_THROW({solver->solve(1e-3, currentState, &newState, computeCompliance);});
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;
+ Eigen::SparseLU<SparseMatrix> eigenSolver;
+ const Vector& Fext = m.getExternalForces();
+ m.updateFMDK(currentState, ODEEQUATIONUPDATE_K);
+ eigenSolver.compute(m.getK());
+ Eigen::VectorXd expectedDeltaX = eigenSolver.solve(Fext);
Vector expectedX = defaultState.getPositions() + expectedDeltaX;
EXPECT_TRUE(newState.getPositions().isApprox(expectedX));
}
+};
TEST(OdeSolverStatic, SolveTest)
{
{
+ SCOPED_TRACE("Static computing the compliance matrix");
+ doSolveTest<OdeSolverStatic>(true);
+ }
+ {
+ SCOPED_TRACE("Static not computing the compliance matrix");
+ doSolveTest<OdeSolverStatic>(false);
+ }
+
+ {
+ SCOPED_TRACE("LinearStatic computing the compliance matrix");
+ doSolveTest<OdeSolverLinearStatic>(true);
+ }
+ {
+ SCOPED_TRACE("LinearStatic not computing the compliance matrix");
+ doSolveTest<OdeSolverLinearStatic>(false);
+ }
+}
+
+namespace
+{
+template <class T>
+void doComputeMatricesTest()
+{
+ MassPointsForStatic m;
+ auto solver = std::make_shared<T>(&m);
+ EXPECT_NO_THROW(m.setOdeSolver(solver));
+ MassPointsStateForStatic state;
+ double dt = 1e-3;
+
+ m.updateFMDK(state, ODEEQUATIONUPDATE_K);
+ Matrix expectedSystemMatrix = m.getK();
+ EXPECT_NO_THROW(solver->computeMatrices(dt, state));
+ EXPECT_TRUE(solver->getSystemMatrix().isApprox(expectedSystemMatrix));
+ EXPECT_TRUE(m.applyCompliance(state, Matrix::Identity(state.getNumDof(),
+ state.getNumDof())).isApprox(expectedSystemMatrix.inverse()));
+}
+};
+
+TEST(OdeSolverStatic, ComputeMatricesTest)
+{
+ {
SCOPED_TRACE("Static");
- doSolveTest<OdeSolverStatic>();
+ doComputeMatricesTest<OdeSolverStatic>();
}
+
{
SCOPED_TRACE("LinearStatic");
- doSolveTest<OdeSolverLinearStatic>();
+ doComputeMatricesTest<OdeSolverLinearStatic>();
}
}
diff --git a/SurgSim/Math/UnitTests/OdeSolverTests.cpp b/SurgSim/Math/UnitTests/OdeSolverTests.cpp
index ea5264f..9ba4d2c 100644
--- a/SurgSim/Math/UnitTests/OdeSolverTests.cpp
+++ b/SurgSim/Math/UnitTests/OdeSolverTests.cpp
@@ -32,7 +32,7 @@ namespace Math
namespace
{
- const std::string name = "MockOdeSolver";
+const std::string name = "MockOdeSolver";
};
class MockOdeSolver : public OdeSolver
@@ -50,14 +50,13 @@ public:
{
}
- /// 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)
+ void solve(double dt, const OdeState& currentState, OdeState* newState, bool computeCompliance = true) override
{
- this->m_systemMatrix.setIdentity();
- this->m_compliance.setIdentity();
+ }
+
+ void assembleLinearSystem(double dt, const OdeState& state, const OdeState& newState, bool computeRHS) override
+ {
+
}
};
@@ -65,46 +64,26 @@ TEST(OdeSolver, ConstructorTest)
{
// OdeEquation is tested separately and is considered valid to use here.
MassPoint m;
+ std::shared_ptr<MockOdeSolver> solver;
- 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({solver = std::make_shared<MockOdeSolver>(&m); });
+ EXPECT_NE(nullptr, solver->getLinearSolver());
+ EXPECT_NE(nullptr, std::dynamic_pointer_cast<LinearSparseSolveAndInverseLU>(solver->getLinearSolver()));
- 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());
+
+ EXPECT_NE(nullptr, solver.getLinearSolver());
+ EXPECT_NE(nullptr, std::dynamic_pointer_cast<LinearSparseSolveAndInverseLU>(solver.getLinearSolver()));
+ EXPECT_NO_THROW(solver.setLinearSolver(std::make_shared<LinearSparseSolveAndInverseLU>()));
+ EXPECT_NE(nullptr, solver.getLinearSolver());
+ EXPECT_NE(nullptr, std::dynamic_pointer_cast<LinearSparseSolveAndInverseLU>(solver.getLinearSolver()));
}
}; // namespace Math
diff --git a/SurgSim/Math/UnitTests/OdeStateTests.cpp b/SurgSim/Math/UnitTests/OdeStateTests.cpp
index c4f3e9d..6e3e3e1 100644
--- a/SurgSim/Math/UnitTests/OdeStateTests.cpp
+++ b/SurgSim/Math/UnitTests/OdeStateTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -24,7 +24,7 @@ using SurgSim::Math::OdeState;
namespace
{
- const double epsilon = 1e-10;
+const double epsilon = 1e-10;
};
TEST(OdeStateTest, ConstructorTest)
@@ -72,7 +72,7 @@ TEST(OdeStateTest, GetPositionsTest)
OdeState state1, state2;
state1.setNumDof(3u, 3u);
state2.setNumDof(3u, 3u);
- for(size_t i = 0; i < state1.getNumDof(); i++)
+ for (size_t i = 0; i < state1.getNumDof(); i++)
{
state1.getPositions()[i] = static_cast<double>(i);
state2.getPositions()[i] = 0.0;
@@ -85,7 +85,7 @@ TEST(OdeStateTest, GetPositionsTest)
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++)
+ 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]);
@@ -101,7 +101,7 @@ TEST(OdeStateTest, GetVelocitiesTest)
OdeState state1, state2;
state1.setNumDof(3u, 3u);
state2.setNumDof(3u, 3u);
- for(size_t i = 0; i < state1.getNumDof(); i++)
+ for (size_t i = 0; i < state1.getNumDof(); i++)
{
state1.getVelocities()[i] = static_cast<double>(i);
state2.getVelocities()[i] = 0.0;
@@ -114,7 +114,7 @@ TEST(OdeStateTest, GetVelocitiesTest)
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++)
+ 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]);
@@ -219,10 +219,10 @@ TEST(OdeStateTest, ResetTest)
OdeState state1, state2;
state1.setNumDof(3u, 3u);
state2.setNumDof(3u, 3u);
- for(size_t i = 0; i < state1.getNumDof(); i++)
+ 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.getVelocities()[i] = 2.0 * static_cast<double>(i);
}
state1.addBoundaryCondition(0u, 0u);
state1.addBoundaryCondition(state1.getNumNodes() - 1u, 2u);
@@ -242,10 +242,10 @@ TEST(OdeStateTest, CopyConstructorAndAssignmentTest)
{
OdeState state, stateAssigned;
state.setNumDof(3u, 3u);
- for(size_t i = 0; i < state.getNumDof(); i++)
+ 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.getVelocities()[i] = 2.0 * static_cast<double>(i);
}
state.addBoundaryCondition(0u, 0u);
state.addBoundaryCondition(state.getNumNodes() - 1u, 2u);
@@ -260,12 +260,12 @@ TEST(OdeStateTest, CopyConstructorAndAssignmentTest)
ASSERT_EQ(9, stateCopied.getVelocities().size());
ASSERT_EQ(state.getVelocities().size(), stateCopied.getVelocities().size());
- for(size_t i = 0; i < stateCopied.getNumDof(); i++)
+ 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);
+ EXPECT_NEAR(2.0 * static_cast<double>(i), stateCopied.getVelocities()[i], epsilon);
}
ASSERT_EQ(2u, stateCopied.getNumBoundaryConditions());
@@ -288,12 +288,12 @@ TEST(OdeStateTest, CopyConstructorAndAssignmentTest)
ASSERT_EQ(9, stateAssigned.getVelocities().size());
ASSERT_EQ(state.getVelocities().size(), stateAssigned.getVelocities().size());
- for(size_t i = 0; i < stateAssigned.getNumDof(); i++)
+ 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);
+ EXPECT_NEAR(2.0 * static_cast<double>(i), stateAssigned.getVelocities()[i], epsilon);
}
ASSERT_EQ(2u, stateAssigned.getNumBoundaryConditions());
@@ -360,41 +360,100 @@ TEST(OdeStateTest, ApplyBoundaryConditionsToMatrixTest)
namespace
{
-void testIsValidWith(double invalidNumber)
+void testOdeStateValidityWith(double number, bool isValid)
{
- 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());
+ OdeState stateWithNumberOnPosition;
+ stateWithNumberOnPosition.setNumDof(3u, 3u);
+ stateWithNumberOnPosition.getPositions().setOnes();
+ stateWithNumberOnPosition.getPositions()[2] = number;
+ if (isValid)
+ {
+ EXPECT_TRUE(stateWithNumberOnPosition.isValid());
+ }
+ else
+ {
+ EXPECT_FALSE(stateWithNumberOnPosition.isValid());
+ }
+
+ OdeState stateWithNumberOnVelocity;
+ stateWithNumberOnVelocity.setNumDof(3u, 3u);
+ stateWithNumberOnVelocity.getVelocities().setOnes();
+ stateWithNumberOnVelocity.getVelocities()[2] = number;
+ if (isValid)
+ {
+ EXPECT_TRUE(stateWithNumberOnVelocity.isValid());
+ }
+ else
+ {
+ EXPECT_FALSE(stateWithNumberOnVelocity.isValid());
+ }
}
}; // anonymous namespace
TEST(OdeStateTest, IsValidTest)
{
- OdeState validState;
- validState.setNumDof(3u, 3u);
- validState.getPositions().setOnes();
- EXPECT_TRUE(validState.isValid());
+ {
+ SCOPED_TRACE("Test with invalid number INF");
+ testOdeStateValidityWith(std::numeric_limits<double>::infinity(), false);
+ }
+
+ {
+ SCOPED_TRACE("Test with invalid number QuietNaN");
+ testOdeStateValidityWith(std::numeric_limits<double>::quiet_NaN(), false);
+ }
+
+ {
+ SCOPED_TRACE("Test with invalid number SignalingNaN");
+ testOdeStateValidityWith(std::numeric_limits<double>::signaling_NaN(), false);
+ }
+
+ {
+ SCOPED_TRACE("Test with valid numbers");
+ testOdeStateValidityWith(4.5e-34, true);
+ }
+}
+
+TEST(OdeStateTest, Interpolation)
+{
+ OdeState state1;
+ OdeState state2;
+ OdeState expected;
+ state1.setNumDof(3u, 3u);
+ state2.setNumDof(3u, 3u);
+ expected.setNumDof(3u, 3u);
+ double t = 0.25;
+ for (size_t i = 0; i < state1.getNumDof(); i++)
+ {
+ double num = static_cast<double>(i);
+ state1.getVelocities()[i] = 1.0;
+ state2.getVelocities()[i] = num;
+ expected.getVelocities()[i] = 1.0 + (num - 1.0) * t;
+ state1.getPositions()[i] = num * 2;
+ state2.getPositions()[i] = 0.0;
+ expected.getPositions()[i] = num * 2 + (0.0 - num * 2) * t;
+ }
+
+ auto result = state1.interpolate(state2, t);
+ ASSERT_TRUE(result.isValid());
+ for (size_t i = 0; i < expected.getNumDof(); i++)
{
- SCOPED_TRACE("Test with invalid INF");
- testIsValidWith(std::numeric_limits<double>::infinity());
+ EXPECT_NEAR(expected.getPositions()[i], result.getPositions()[i], epsilon);
+ EXPECT_NEAR(expected.getVelocities()[i], result.getVelocities()[i], epsilon);
}
+ result = state1.interpolate(state2, 0.0);
+ for (size_t i = 0; i < expected.getNumDof(); i++)
{
- SCOPED_TRACE("Test with invalid QuietNaN");
- testIsValidWith(std::numeric_limits<double>::quiet_NaN());
+ EXPECT_NEAR(state1.getPositions()[i], result.getPositions()[i], epsilon);
+ EXPECT_NEAR(state1.getVelocities()[i], result.getVelocities()[i], epsilon);
}
+ result = state1.interpolate(state2, 1.0);
+ for (size_t i = 0; i < expected.getNumDof(); i++)
{
- SCOPED_TRACE("Test with invalid SignalingNaN");
- testIsValidWith(std::numeric_limits<double>::signaling_NaN());
+ EXPECT_NEAR(state2.getPositions()[i], result.getPositions()[i], epsilon);
+ EXPECT_NEAR(state2.getVelocities()[i], result.getVelocities()[i], epsilon);
}
+
}
diff --git a/SurgSim/Math/UnitTests/ParticlesShapeTests.cpp b/SurgSim/Math/UnitTests/ParticlesShapeTests.cpp
new file mode 100644
index 0000000..1a61dcf
--- /dev/null
+++ b/SurgSim/Math/UnitTests/ParticlesShapeTests.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/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/NormalData.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/Shapes.h"
+#include "SurgSim/Math/Valid.h"
+
+
+namespace
+{
+const double epsilon = 1e-10;
+}
+
+namespace SurgSim
+{
+namespace Math
+{
+
+TEST(ParticlesShapeTests, CanConstruct)
+{
+ ASSERT_NO_THROW({ ParticlesShape particles; });
+ ASSERT_NO_THROW({ ParticlesShape particles(0.01); });
+
+}
+
+TEST(ParticlesShapeTests, CopyConstruct)
+{
+ {
+ DataStructures::Vertices<DataStructures::EmptyData> otherVertices;
+ otherVertices.addVertex(DataStructures::Vertex<DataStructures::EmptyData>(Vector3d::Constant(3.0)));
+
+ ParticlesShape particles(otherVertices);
+ EXPECT_EQ(1, particles.getNumVertices());
+ EXPECT_TRUE(particles.getVertex(0).position.isApprox(Vector3d::Constant(3.0)));
+ }
+ {
+ DataStructures::Vertices<DataStructures::NormalData> otherVertices;
+ otherVertices.addVertex(DataStructures::Vertex<DataStructures::NormalData>(Vector3d::Constant(3.0)));
+
+ ParticlesShape particles(otherVertices);
+ EXPECT_EQ(1, particles.getNumVertices());
+ EXPECT_TRUE(particles.getVertex(0).position.isApprox(Vector3d::Constant(3.0)));
+ }
+}
+
+TEST(ParticlesShapeTests, DefaultProperties)
+{
+ ParticlesShape particles;
+ EXPECT_NEAR(0.0, particles.getRadius(), epsilon);
+ EXPECT_EQ(SHAPE_TYPE_PARTICLES, particles.getType());
+ EXPECT_NEAR(0.0, particles.getVolume(), epsilon);
+ EXPECT_FALSE(isValid(particles.getCenter()));
+ EXPECT_TRUE(particles.getSecondMomentOfVolume().isZero());
+ EXPECT_NE(nullptr, particles.getAabbTree());
+
+ EXPECT_TRUE(particles.isTransformable());
+}
+
+TEST(ParticlesShapeTests, SetGetRadius)
+{
+ ParticlesShape particles;
+ particles.setRadius(0.1);
+ EXPECT_NEAR(0.1, particles.getRadius(), epsilon);
+}
+
+TEST(ParticlesShapeTests, GeometricProperties)
+{
+ SphereShape unitSphere(1.0);
+ ParticlesShape particles(1.0);
+
+ particles.addVertex(ParticlesShape::VertexType(Vector3d::Zero()));
+ particles.update();
+ EXPECT_TRUE(particles.getCenter().isApprox(Vector3d::Zero()));
+ EXPECT_NEAR(unitSphere.getVolume(), particles.getVolume(), epsilon);
+ EXPECT_TRUE(particles.getSecondMomentOfVolume().isApprox(unitSphere.getSecondMomentOfVolume()))
+ << unitSphere.getSecondMomentOfVolume() << std::endl << particles.getSecondMomentOfVolume();
+
+ particles.addVertex(ParticlesShape::VertexType(Vector3d::Ones()));
+ particles.update();
+ EXPECT_TRUE(particles.getCenter().isApprox(Vector3d::Ones() * 0.5));
+ EXPECT_NEAR(2.0 * unitSphere.getVolume(), particles.getVolume(), epsilon);
+ Matrix33d distanceSkew = makeSkewSymmetricMatrix(Vector3d::Ones().eval());
+ EXPECT_TRUE(particles.getSecondMomentOfVolume().isApprox(2.0 * unitSphere.getSecondMomentOfVolume()
+ - unitSphere.getVolume() * distanceSkew * distanceSkew));
+
+ particles.addVertex(ParticlesShape::VertexType(-Vector3d::Ones()));
+ particles.update();
+ EXPECT_TRUE(particles.getCenter().isApprox(Vector3d::Zero()));
+ EXPECT_NEAR(3.0 * unitSphere.getVolume(), particles.getVolume(), epsilon);
+ EXPECT_TRUE(particles.getSecondMomentOfVolume().isApprox(3 * unitSphere.getSecondMomentOfVolume()
+ - 2.0 * unitSphere.getVolume() * distanceSkew * distanceSkew));
+}
+
+TEST(ParticlesShapeTests, Serialization)
+{
+ const double radius = 0.2;
+ {
+ YAML::Node node;
+ node["SurgSim::Math::ParticlesShape"]["Radius"] = radius;
+
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = node.as<std::shared_ptr<Shape>>());
+
+ EXPECT_EQ("SurgSim::Math::ParticlesShape", shape->getClassName());
+ EXPECT_NEAR(radius, shape->getValue<double>("Radius"), epsilon);
+ EXPECT_TRUE(shape->isValid());
+ }
+
+ {
+ std::shared_ptr<Shape> shape;
+ ASSERT_NO_THROW(shape = Shape::getFactory().create("SurgSim::Math::ParticlesShape"));
+ shape->setValue("Radius", 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::ParticlesShape"].IsDefined());
+ auto data = node["SurgSim::Math::ParticlesShape"];
+ EXPECT_EQ(1u, data.size());
+
+ std::shared_ptr<ParticlesShape> particlesShape;
+ ASSERT_NO_THROW(particlesShape = std::dynamic_pointer_cast<ParticlesShape>(node.as<std::shared_ptr<Shape>>()));
+ EXPECT_EQ("SurgSim::Math::ParticlesShape", particlesShape->getClassName());
+ EXPECT_NEAR(radius, particlesShape->getValue<double>("Radius"), epsilon);
+ EXPECT_TRUE(particlesShape->isValid());
+ }
+}
+
+
+};
+};
+
diff --git a/SurgSim/Math/UnitTests/PolynomialRootTests.cpp b/SurgSim/Math/UnitTests/PolynomialRootTests.cpp
new file mode 100644
index 0000000..bd9a742
--- /dev/null
+++ b/SurgSim/Math/UnitTests/PolynomialRootTests.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 Polynomial functions.
+
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/PolynomialRoots.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+namespace
+{
+double epsilon = 1.0e-9;
+}
+
+TEST(PolynomialRootsTest, PolynomialDegree1Roots)
+{
+ // Degenerate Linear, infinite roots
+ SurgSim::Math::Polynomial<double, 1> degenerate(epsilon / 2.0, epsilon / 2.0);
+ PolynomialRoots<double, 1> degenerateRoots(degenerate, 1.0e-09);
+ EXPECT_TRUE(degenerateRoots.isDegenerate());
+ EXPECT_EQ(-1, degenerateRoots.getNumRoots());
+ EXPECT_THROW(degenerateRoots[0], SurgSim::Framework::AssertionFailure);
+
+ // Semi-degenerate Linear, no roots
+ SurgSim::Math::Polynomial<double, 1> constantPoly(2.0, epsilon / 2.0);
+ PolynomialRoots<double, 1>constantPolyRoots(constantPoly, 1.0e-09);
+ EXPECT_FALSE(constantPolyRoots.isDegenerate());
+ EXPECT_EQ(0, constantPolyRoots.getNumRoots());
+ EXPECT_THROW(constantPolyRoots[0], SurgSim::Framework::AssertionFailure);
+
+ // Linear, one root
+ SurgSim::Math::Polynomial<double, 1> linearPoly(-24.0, 2.0);
+ PolynomialRoots<double, 1>linearPolyRoots(linearPoly, 1.0e-09);
+ EXPECT_FALSE(linearPolyRoots.isDegenerate());
+ EXPECT_EQ(1, linearPolyRoots.getNumRoots());
+ EXPECT_NEAR(12.0, linearPolyRoots[0], epsilon);
+ EXPECT_THROW(linearPolyRoots[1], SurgSim::Framework::AssertionFailure);
+};
+
+TEST(PolynomialRootsTest, PolynomialDegree2Roots)
+{
+ // Degenerate Linear, infinite roots
+ SurgSim::Math::Polynomial<double, 2> degenerate(epsilon / 2.0, epsilon / 2.0, epsilon / 2.0);
+ PolynomialRoots<double, 2>degenerateRoots(degenerate, 1.0e-09);
+ EXPECT_TRUE(degenerateRoots.isDegenerate());
+ EXPECT_EQ(-1, degenerateRoots.getNumRoots());
+ EXPECT_THROW(degenerateRoots[0], SurgSim::Framework::AssertionFailure);
+
+ // Semi-degenerate Linear, no roots
+ SurgSim::Math::Polynomial<double, 2> constantPoly(2.0, epsilon / 2.0, epsilon / 2.0);
+ PolynomialRoots<double, 2>constantPolyRoots(constantPoly, 1.0e-09);
+ EXPECT_FALSE(constantPolyRoots.isDegenerate());
+ EXPECT_EQ(0, constantPolyRoots.getNumRoots());
+ EXPECT_THROW(constantPolyRoots[0], SurgSim::Framework::AssertionFailure);
+
+ // Linear, one root
+ SurgSim::Math::Polynomial<double, 2> linearPoly(-24.0, 2.0, epsilon / 2.0);
+ PolynomialRoots<double, 2>linearPolyRoots(linearPoly, 1.0e-09);
+ EXPECT_FALSE(linearPolyRoots.isDegenerate());
+ EXPECT_EQ(1, linearPolyRoots.getNumRoots());
+ EXPECT_NEAR(12.0, linearPolyRoots[0], epsilon);
+ EXPECT_THROW(linearPolyRoots[1], SurgSim::Framework::AssertionFailure);
+
+ // Quadratic, two imaginary roots
+ SurgSim::Math::Polynomial<double, 2> imaginaryPoly(1.0, epsilon / 8.0, 1.0);
+ PolynomialRoots<double, 2>imaginaryPolyRoots(imaginaryPoly, 1.0e-09);
+ EXPECT_FALSE(imaginaryPolyRoots.isDegenerate());
+ EXPECT_EQ(0, imaginaryPolyRoots.getNumRoots());
+ EXPECT_THROW(imaginaryPolyRoots[0], SurgSim::Framework::AssertionFailure);
+
+ // Quadratic, one (duplicate) root
+ SurgSim::Math::Polynomial<double, 2> oneRootPoly(288.0, -48.0, 2.0);
+ PolynomialRoots<double, 2>oneRootPolyRoots(oneRootPoly, 1.0e-09);
+ EXPECT_FALSE(oneRootPolyRoots.isDegenerate());
+ EXPECT_EQ(1, oneRootPolyRoots.getNumRoots());
+ EXPECT_NEAR(12.0, oneRootPolyRoots[0], epsilon);
+ EXPECT_THROW(oneRootPolyRoots[1], SurgSim::Framework::AssertionFailure);
+
+ // Quadratic
+ SurgSim::Math::Polynomial<double, 2> quadraticPoly(-8.0, 6.0, 2.0);
+ PolynomialRoots<double, 2>quadraticPolyRoots(quadraticPoly, 1.0e-09);
+ EXPECT_FALSE(quadraticPolyRoots.isDegenerate());
+ EXPECT_EQ(2, quadraticPolyRoots.getNumRoots());
+ EXPECT_NEAR(-4.0, quadraticPolyRoots[0], epsilon);
+ EXPECT_NEAR(1.0, quadraticPolyRoots[1], epsilon);
+ EXPECT_THROW(quadraticPolyRoots[2], SurgSim::Framework::AssertionFailure);
+
+ // Quadratic (ascending order => coefficient of higher order < 0)
+ // Discriminant = 49 => 2 roots that should be ordered this way:
+ // (-9 - 7) / -4 = 4
+ // (-9 + 7) / -4 = 1/2
+ // But will be reordered to be in ascending order
+ SurgSim::Math::Polynomial<double, 2> quadraticPolyOrdered(-4.0, 9.0, -2.0);
+ PolynomialRoots<double, 2>quadraticPolyOrderedRoots(quadraticPolyOrdered, 1.0e-09);
+ EXPECT_FALSE(quadraticPolyOrderedRoots.isDegenerate());
+ EXPECT_EQ(2, quadraticPolyOrderedRoots.getNumRoots());
+ EXPECT_NEAR(0.5, quadraticPolyOrderedRoots[0], epsilon);
+ EXPECT_NEAR(4.0, quadraticPolyOrderedRoots[1], epsilon);
+ EXPECT_THROW(quadraticPolyOrderedRoots[2], SurgSim::Framework::AssertionFailure);
+};
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/UnitTests/PolynomialTests.cpp b/SurgSim/Math/UnitTests/PolynomialTests.cpp
new file mode 100644
index 0000000..bb3eb67
--- /dev/null
+++ b/SurgSim/Math/UnitTests/PolynomialTests.cpp
@@ -0,0 +1,377 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 Polynomial functions.
+
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/Polynomial.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+namespace
+{
+double epsilon = 1.0e-10;
+
+template <int degree>
+void checkDiscriminant(Polynomial<double, degree>* poly, double expected)
+{
+}
+template <>
+void checkDiscriminant(Polynomial<double, 2>* poly, double expected)
+{
+ EXPECT_DOUBLE_EQ(expected, poly->discriminant());
+}
+}
+
+class PolynomialUtilityTests : public ::testing::Test
+{
+};
+
+template <typename T>
+class PolynomialTests : public ::testing::Test
+{
+public:
+
+ void initializeConstructor(Polynomial<double, 0>* poly)
+ {
+ typedef Polynomial<double, 0> PolynomialType;
+ EXPECT_NO_THROW(PolynomialType polyNew);
+ EXPECT_NO_THROW(PolynomialType polyNew(7.0));
+ PolynomialType polyNew(1.0);
+ *poly = polyNew;
+ }
+
+ void initializeConstructor(Polynomial<double, 1>* poly)
+ {
+ typedef Polynomial<double, 1> PolynomialType;
+ EXPECT_NO_THROW(PolynomialType polyNew);
+ EXPECT_NO_THROW(PolynomialType polyNew(7.0, 8));
+ PolynomialType polyNew(1.0, 2.0);
+ *poly = polyNew;
+ }
+
+ void initializeConstructor(Polynomial<double, 2>* poly)
+ {
+ typedef Polynomial<double, 2> PolynomialType;
+ EXPECT_NO_THROW(PolynomialType polyNew);
+ EXPECT_NO_THROW(PolynomialType polyNew(7.0, 8.0, 9.0));
+ PolynomialType polyNew(1.0, 2.0, 3.0);
+ *poly = polyNew;
+ }
+
+ void initializeConstructor(Polynomial<double, 3>* poly)
+ {
+ typedef Polynomial<double, 3> PolynomialType;
+ EXPECT_NO_THROW(PolynomialType polyNew);
+ EXPECT_NO_THROW(PolynomialType polyNew(7.0, 8.0, 9.0, 10.0));
+ PolynomialType polyNew(1.0, 2.0, 3.0, 4.0);
+ *poly = polyNew;
+ }
+
+ template <int degree>
+ void setPolynomialFromOffset(int offset, Polynomial<double, degree>* poly)
+ {
+ for (size_t counter = 0; counter <= degree; ++counter)
+ {
+ (*poly)[counter] = static_cast<double>(counter + offset);
+ }
+ }
+
+ template <int degree>
+ void setPolynomialToSmallValue(Polynomial<double, degree>* poly)
+ {
+ for (size_t counter = 0; counter <= degree; ++counter)
+ {
+ (*poly)[counter] = epsilon / 2.0;
+ }
+ }
+
+ template <int degree>
+ void checkPolynomialConstructor(Polynomial<double, degree>* poly)
+ {
+ // Check get ... up to degree
+ for (size_t counter = 0; counter <= degree; ++counter)
+ {
+ EXPECT_DOUBLE_EQ(counter + 1.0, poly->getCoefficient(counter));
+ EXPECT_DOUBLE_EQ(counter + 1.0, (*poly)[counter]);
+ }
+
+ // Check get ... beyond degree
+ for (size_t counter = degree + 1; counter < 5; ++counter)
+ {
+ EXPECT_DOUBLE_EQ(0.0, poly->getCoefficient(counter));
+ EXPECT_THROW((*poly)[counter], SurgSim::Framework::AssertionFailure);
+ }
+
+ // Check set up to degree
+ for (size_t counter = 0; counter <= degree; ++counter)
+ {
+ poly->setCoefficient(counter, static_cast<double>(counter));
+ EXPECT_DOUBLE_EQ(static_cast<double>(counter), poly->getCoefficient(counter));
+ (*poly)[counter] = static_cast<double>(counter + 1);
+ EXPECT_DOUBLE_EQ(static_cast<double>(counter + 1), poly->getCoefficient(counter));
+ }
+
+ // Check set beyond degree
+ EXPECT_THROW(poly->setCoefficient(degree + 1, 12.0), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW((*poly)[degree + 1] = 12, SurgSim::Framework::AssertionFailure);
+ }
+
+ template <int degree>
+ void checkPolynomialArithmetic(const Polynomial<double, degree>& poly1,
+ const Polynomial<double, degree>& poly2)
+ {
+ Polynomial<double, degree> scratch;
+
+ // Check evaluate ...
+ double evaluate = static_cast<double>(degree + 1);
+ for (int counter = degree - 1; counter >= 0; --counter)
+ {
+ evaluate = (0.5 * evaluate) + counter + 1;
+ }
+ EXPECT_NEAR(evaluate, poly1.evaluate(0.5), epsilon);
+
+ // Check negation ...
+ scratch = -poly1;
+ for (size_t counter = 0; counter <= degree; ++counter)
+ {
+ EXPECT_DOUBLE_EQ(-1 * (static_cast<double>(counter) + 1.0), scratch.getCoefficient(counter));
+ EXPECT_DOUBLE_EQ(-1 * (static_cast<double>(counter) + 1.0), scratch[counter]);
+ }
+
+ // Check add ...
+ scratch = poly1 + poly2;
+ for (size_t counter = 0; counter <= degree; ++counter)
+ {
+ EXPECT_DOUBLE_EQ(2 * static_cast<double>(counter) + 3.0, scratch.getCoefficient(counter));
+ EXPECT_DOUBLE_EQ(2 * static_cast<double>(counter) + 3.0, scratch[counter]);
+ }
+
+ scratch = poly1;
+ scratch += poly2;
+ for (size_t counter = 0; counter <= degree; ++counter)
+ {
+ EXPECT_DOUBLE_EQ(2 * static_cast<double>(counter) + 3.0, scratch.getCoefficient(counter));
+ EXPECT_DOUBLE_EQ(2 * static_cast<double>(counter) + 3.0, scratch[counter]);
+ }
+
+ // Check subtract ...
+ scratch = poly1 - poly2;
+ for (size_t counter = 0; counter <= degree; ++counter)
+ {
+ EXPECT_DOUBLE_EQ(-1.0, scratch.getCoefficient(counter));
+ EXPECT_DOUBLE_EQ(-1.0, scratch[counter]);
+ }
+
+ scratch = poly1;
+ scratch -= poly2;
+ for (size_t counter = 0; counter <= degree; ++counter)
+ {
+ EXPECT_DOUBLE_EQ(-1.0, scratch.getCoefficient(counter));
+ EXPECT_DOUBLE_EQ(-1.0, scratch[counter]);
+ }
+
+ // Check isApprox ...
+ scratch = poly1;
+ for (size_t counter = 0; counter <= degree; ++counter)
+ {
+ scratch[counter] += epsilon / 2.0;
+ }
+ EXPECT_TRUE(scratch.isApprox(poly1, epsilon));
+ scratch[0] = poly1[0] + 2.0 * epsilon;
+ for (size_t counter = 0; counter < degree; ++counter)
+ {
+ EXPECT_FALSE(scratch.isApprox(poly1, epsilon));
+ scratch[counter] = poly1[counter] + epsilon / 2.0;
+ scratch[counter + 1] = poly1[counter + 1] + 2.0 * epsilon;
+ }
+ EXPECT_FALSE(scratch.isApprox(poly1, epsilon));
+ }
+
+ template <int degree>
+ void checkPolynomialDerivative(const Polynomial<double, degree>& poly1)
+ {
+ if (degree != 0)
+ {
+ // Check derivative ...
+ auto smallScratch = poly1.derivative();
+ for (size_t counter = 0; counter < degree; ++counter)
+ {
+ EXPECT_DOUBLE_EQ((counter + 2.0) * (counter + 1.0), smallScratch.getCoefficient(counter));
+ EXPECT_DOUBLE_EQ((counter + 2.0) * (counter + 1.0), smallScratch[counter]);
+ }
+ EXPECT_DOUBLE_EQ(0.0, smallScratch.getCoefficient(degree));
+ EXPECT_THROW(smallScratch[degree], SurgSim::Framework::AssertionFailure);
+ }
+ }
+
+ template <int degree>
+ void checkIsNearZero(Polynomial<double, degree>* poly1)
+ {
+ EXPECT_TRUE(poly1->isNearZero(epsilon));
+ for (size_t counter = 0; counter <= degree; counter++)
+ {
+ poly1->setCoefficient(counter, 2 * epsilon);
+ EXPECT_FALSE(poly1->isNearZero(epsilon));
+ poly1->setCoefficient(counter, epsilon / 2.0);
+ EXPECT_TRUE(poly1->isNearZero(epsilon));
+ (*poly1)[counter] = 2 * epsilon;
+ EXPECT_FALSE(poly1->isNearZero(epsilon));
+ (*poly1)[counter] = epsilon / 2.0;
+ EXPECT_TRUE(poly1->isNearZero(epsilon));
+ }
+ }
+};
+
+template <typename T>
+class PolynomialDerivativeTests : public PolynomialTests<T>
+{
+};
+
+typedef ::testing::Types<Polynomial<double, 0>,
+ Polynomial<double, 1>,
+ Polynomial<double, 2>,
+ Polynomial<double, 3>> PolynomialTypes;
+
+typedef ::testing::Types <
+Polynomial<double, 1>,
+ Polynomial<double, 2>,
+ Polynomial<double, 3 >> PolynomialDerivativeTypes;
+
+TYPED_TEST_CASE(PolynomialTests, PolynomialTypes);
+TYPED_TEST_CASE(PolynomialDerivativeTests, PolynomialDerivativeTypes);
+
+TYPED_TEST(PolynomialTests, InitializationTests)
+{
+ EXPECT_NO_THROW(TypeParam poly);
+ TypeParam poly;
+ this->initializeConstructor(&poly);
+ this->checkPolynomialConstructor(&poly);
+};
+
+TYPED_TEST(PolynomialTests, ArithmeticTests)
+{
+ TypeParam poly1;
+ this->setPolynomialFromOffset(1, &poly1);
+ TypeParam poly2;
+ this->setPolynomialFromOffset(2, &poly2);
+ this->checkPolynomialArithmetic(poly1, poly2);
+};
+
+TYPED_TEST(PolynomialDerivativeTests, DerivativeTests)
+{
+ TypeParam poly1;
+ this->setPolynomialFromOffset(1, &poly1);
+ this->checkPolynomialDerivative(poly1);
+};
+
+TYPED_TEST(PolynomialTests, NearZeroTests)
+{
+ TypeParam poly1;
+ this->setPolynomialToSmallValue(&poly1);
+ this->checkIsNearZero(&poly1);
+};
+
+TYPED_TEST(PolynomialTests, DiscriminantTests)
+{
+ TypeParam poly1;
+ {
+ SCOPED_TRACE("0 + 1x + 2x^2");
+ this->setPolynomialFromOffset(0, &poly1);
+ // discriminant = 1 - 4*0 = 1
+ checkDiscriminant(&poly1, 1.0);
+ }
+ {
+ SCOPED_TRACE("1 + 2x + 3x^2");
+ this->setPolynomialFromOffset(1, &poly1);
+ // discriminant = 4 - 4*3 = -8
+ checkDiscriminant(&poly1, -8.0);
+ }
+ {
+ SCOPED_TRACE("2 + 3x + 4x^2");
+ this->setPolynomialFromOffset(2, &poly1);
+ // discriminant = 9 - 4*8 = -23
+ checkDiscriminant(&poly1, -23.0);
+ }
+};
+
+TEST_F(PolynomialUtilityTests, UtilityTests)
+{
+ Polynomial<double, 0> p0_1(1.0);
+ Polynomial<double, 0> p0_2(2.0);
+ Polynomial<double, 1> p1_1(1.0, 2.0);
+ Polynomial<double, 1> p1_2(2.0, 3.0);
+ Polynomial<double, 2> p2_1(1.0, 2.0, 3.0);
+ Polynomial<double, 2> p2_2(2.0, 3.0, 4.0);
+ Polynomial<double, 3> p3_1(1.0, 2.0, 3.0, 4.0);
+ Polynomial<double, 3> p3_2(2.0, 3.0, 4.0, 5.0);
+
+ // degree n * degree m
+ {
+ auto result = p0_2 * p3_2;
+ EXPECT_TRUE(result.isApprox(Polynomial<double, 3>(4.0, 6.0, 8.0, 10.0), epsilon));
+ }
+
+ // degree 1 * degree 1
+ {
+ auto result = p1_1 * p1_2;
+ EXPECT_TRUE(result.isApprox(Polynomial<double, 2>(2.0, 7.0, 6.0), epsilon));
+ }
+
+ // degree 2 * degree 1
+ {
+ auto result = p2_1 * p1_2;
+ EXPECT_TRUE(result.isApprox(Polynomial<double, 3>(2.0, 7.0, 12.0, 9.0), epsilon));
+ }
+
+ // degree 1 * degree 2
+ {
+ auto result = p1_1 * p2_2;
+ EXPECT_TRUE(result.isApprox(Polynomial<double, 3>(2.0, 7.0, 10.0, 8.0), epsilon));
+ }
+
+ // degree 0^2
+ {
+ auto result_p0_1 = square(p0_1);
+ EXPECT_TRUE(result_p0_1.isApprox(Polynomial<double, 0>(1.0), epsilon));
+ auto result_p0_2 = square(p0_2);
+ EXPECT_TRUE(result_p0_2.isApprox(Polynomial<double, 0>(4.0), epsilon));
+ }
+
+ // degree 1^2
+ {
+ auto result = square(p1_2);
+ EXPECT_TRUE(result.isApprox(Polynomial<double, 2>(4.0, 12.0, 9.0), epsilon));
+ }
+
+ // Output
+ {
+ // Output
+ std::ostringstream intervalOutput;
+ intervalOutput << p3_1;
+ EXPECT_EQ("(4*x^3 + 3*x^2 + 2*x + 1)", intervalOutput.str());
+ }
+};
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/UnitTests/PolynomialValuesTests.cpp b/SurgSim/Math/UnitTests/PolynomialValuesTests.cpp
new file mode 100644
index 0000000..17ea35a
--- /dev/null
+++ b/SurgSim/Math/UnitTests/PolynomialValuesTests.cpp
@@ -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.
+
+/// \file
+/// Tests for the Polynomial functions.
+
+#include <array>
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/PolynomialValues.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+namespace
+{
+double epsilon = 1.0e-9;
+}
+
+TEST(PolynomialValuesTest, PolnomialDegree0Values)
+{
+ SurgSim::Math::Polynomial<double, 0> constantPoly(-24.0);
+ SurgSim::Math::Interval<double> interval(-1.0, 10.0);
+ PolynomialValues<double, 0> constantValues(constantPoly);
+ EXPECT_DOUBLE_EQ(constantPoly.getCoefficient(0), constantValues.getPolynomial().getCoefficient(0));
+ EXPECT_TRUE(Interval<double>(-24.0, -24.0).isApprox(constantValues.valuesOverInterval(interval), epsilon));
+};
+
+TEST(PolynomialValuesTest, PolnomialDegree1Values)
+{
+ SurgSim::Math::Polynomial<double, 1> linearPoly(-24.0, 2.0);
+ SurgSim::Math::Interval<double> interval(-1.0, 10.0);
+ PolynomialValues<double, 1> linearValues(linearPoly);
+ EXPECT_DOUBLE_EQ(linearPoly.getCoefficient(0), linearValues.getPolynomial().getCoefficient(0));
+ EXPECT_DOUBLE_EQ(linearPoly.getCoefficient(1), linearValues.getPolynomial().getCoefficient(1));
+ EXPECT_TRUE(Interval<double>(-26.0, -4.0).isApprox(linearValues.valuesOverInterval(interval), epsilon));
+};
+
+TEST(PolynomialValuesTest, PolnomialDegree2Values)
+{
+ SurgSim::Math::Polynomial<double, 2> quadraticPoly(-8.0, 6.0, 2.0);
+ SurgSim::Math::Interval<double> interval1(-1.0, 10.0);
+ SurgSim::Math::Interval<double> interval2(-2.0, 10.0);
+ PolynomialValues<double, 2> quadraticValues(quadraticPoly);
+ EXPECT_DOUBLE_EQ(quadraticPoly.getCoefficient(0), quadraticValues.getPolynomial().getCoefficient(0));
+ EXPECT_DOUBLE_EQ(quadraticPoly.getCoefficient(1), quadraticValues.getPolynomial().getCoefficient(1));
+ EXPECT_DOUBLE_EQ(quadraticPoly.getCoefficient(2), quadraticValues.getPolynomial().getCoefficient(2));
+ EXPECT_NEAR(6.0, quadraticValues.getDerivative().getCoefficient(0), epsilon);
+ EXPECT_NEAR(4.0, quadraticValues.getDerivative().getCoefficient(1), epsilon);
+ EXPECT_EQ(1, quadraticValues.getLocationsOfExtrema().getNumRoots());
+ EXPECT_NEAR(-1.5, quadraticValues.getLocationsOfExtrema()[0], epsilon);
+ EXPECT_TRUE(Interval<double>(-12.0, 252.0).isApprox(quadraticValues.valuesOverInterval(interval1), epsilon));
+ EXPECT_TRUE(Interval<double>(-12.5, 252.0).isApprox(quadraticValues.valuesOverInterval(interval2), epsilon));
+};
+
+TEST(PolynomialValuesTest, PolnomialDegree3Values)
+{
+ SurgSim::Math::Polynomial<double, 3> cubicPoly(7.0, -63.0, -6.0, 1.0);
+ SurgSim::Math::Interval<double> interval1(-10.0, 20.0);
+ SurgSim::Math::Interval<double> interval2(-4.0, 10.0);
+ PolynomialValues<double, 3> cubicValues(cubicPoly);
+ EXPECT_DOUBLE_EQ(cubicPoly.getCoefficient(0), cubicValues.getPolynomial().getCoefficient(0));
+ EXPECT_DOUBLE_EQ(cubicPoly.getCoefficient(1), cubicValues.getPolynomial().getCoefficient(1));
+ EXPECT_DOUBLE_EQ(cubicPoly.getCoefficient(2), cubicValues.getPolynomial().getCoefficient(2));
+ EXPECT_DOUBLE_EQ(cubicPoly.getCoefficient(3), cubicValues.getPolynomial().getCoefficient(3));
+ EXPECT_NEAR(-63.0, cubicValues.getDerivative().getCoefficient(0), epsilon);
+ EXPECT_NEAR(-12.0, cubicValues.getDerivative().getCoefficient(1), epsilon);
+ EXPECT_NEAR(3.0, cubicValues.getDerivative().getCoefficient(2), epsilon);
+ EXPECT_EQ(2, cubicValues.getLocationsOfExtrema().getNumRoots());
+ EXPECT_NEAR(-3.0, cubicValues.getLocationsOfExtrema()[0], epsilon);
+ EXPECT_NEAR(7.0, cubicValues.getLocationsOfExtrema()[1], epsilon);
+ EXPECT_TRUE(Interval<double>(-963.0, 4347.0).isApprox(cubicValues.valuesOverInterval(interval1), epsilon));
+ EXPECT_TRUE(Interval<double>(-385.0, 115.0).isApprox(cubicValues.valuesOverInterval(interval2), epsilon));
+};
+
+TEST(PolynomialValuesTest, ValuesOverIntervalTests)
+{
+ SurgSim::Math::Polynomial<double, 0> constantPoly(-24.0);
+ SurgSim::Math::Polynomial<double, 1> linearPoly(-24.0, 2.0);
+ SurgSim::Math::Polynomial<double, 2> quadraticPoly(-8.0, 6.0, 2.0);
+ SurgSim::Math::Polynomial<double, 3> cubicPoly(7.0, -63.0, -6.0, 1.0);
+ SurgSim::Math::Interval<double> interval(-1.0, 10.0);
+ EXPECT_TRUE(Interval<double>(-24.0, -24.0).isApprox(valuesOverInterval(constantPoly, interval), epsilon));
+ EXPECT_TRUE(Interval<double>(-26.0, -4.0).isApprox(valuesOverInterval(linearPoly, interval), epsilon));
+ EXPECT_TRUE(Interval<double>(-12.0, 252.0).isApprox(valuesOverInterval(quadraticPoly, interval), epsilon));
+ EXPECT_TRUE(Interval<double>(-385.0, 63.0).isApprox(valuesOverInterval(cubicPoly, interval), epsilon));
+};
+
+}; // namespace Math
+}; // namespace SurgSim
diff --git a/SurgSim/Math/UnitTests/QuaternionTests.cpp b/SurgSim/Math/UnitTests/QuaternionTests.cpp
index 4f1d06f..d6bb1ad 100644
--- a/SurgSim/Math/UnitTests/QuaternionTests.cpp
+++ b/SurgSim/Math/UnitTests/QuaternionTests.cpp
@@ -173,6 +173,7 @@ TYPED_TEST(QuaternionTests, SetToZero)
// Test conversion to and from yaml node
TYPED_TEST(QuaternionTests, YamlConvert)
{
+ typedef typename TestFixture::AngleAxis AngleAxis;
typedef typename TestFixture::Quaternion Quaternion;
typedef typename TestFixture::Scalar T;
@@ -187,6 +188,15 @@ TYPED_TEST(QuaternionTests, YamlConvert)
ASSERT_NO_THROW({Quaternion expected = node.as<Quaternion>();});
EXPECT_TRUE(quaternion.isApprox(node.as<Quaternion>()));
+
+ // test decoding from AngleAxis
+ AngleAxis angleAxis;
+ angleAxis.angle() = T(0.1);
+ angleAxis.axis()[0] = T(0.1);
+ angleAxis.axis()[1] = T(1.2);
+ angleAxis.axis()[2] = T(2.3);
+ YAML::Node angleAxisNode = YAML::convert<AngleAxis>::encode(angleAxis);
+ EXPECT_TRUE(Quaternion(angleAxis).isApprox(angleAxisNode.as<Quaternion>()));
}
diff --git a/SurgSim/Math/UnitTests/ScalarTests.cpp b/SurgSim/Math/UnitTests/ScalarTests.cpp
new file mode 100644
index 0000000..4a4f8f0
--- /dev/null
+++ b/SurgSim/Math/UnitTests/ScalarTests.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.
+
+/// \file
+/// Tests for the Scalar functions.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Scalar.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+
+class ScalarTests : public ::testing::Test {};
+
+
+TEST(ScalarTests, TwoEntriesTests)
+{
+ int minInt = 7;
+ int maxInt = 52;
+ int valueInt;
+ int epsilonInt = 5;
+
+ float minFloat = 7.0;
+ float maxFloat = 52.0;
+ float valueFloat;
+ float epsilonFloat = 5.0;
+
+ double minDouble = 7.0;
+ double maxDouble = 52.0;
+ double valueDouble;
+ double epsilonDouble = 5.0;
+
+ {
+ SCOPED_TRACE("Test < minimum");
+
+ valueInt = 6;
+ EXPECT_EQ(minInt, Math::clamp(valueInt, minInt, maxInt, epsilonInt));
+
+ valueFloat = 6.0;
+ EXPECT_FLOAT_EQ(minFloat, Math::clamp(valueFloat, minFloat, maxFloat, epsilonFloat));
+
+ valueDouble = 6.0;
+ EXPECT_DOUBLE_EQ(minDouble, Math::clamp(valueDouble, minDouble, maxDouble, epsilonDouble));
+ }
+
+ {
+ SCOPED_TRACE("Test value = minimum + epsilon");
+
+ valueInt = 12;
+ EXPECT_EQ(minInt, Math::clamp(valueInt, minInt, maxInt, epsilonInt));
+
+ valueFloat = 12.0;
+ EXPECT_FLOAT_EQ(minFloat, Math::clamp(valueFloat, minFloat, maxFloat, epsilonFloat));
+
+ valueDouble = 12.0;
+ EXPECT_DOUBLE_EQ(minDouble, Math::clamp(valueDouble, minDouble, maxDouble, epsilonDouble));
+ }
+
+ {
+ SCOPED_TRACE("Test value > minimum + epsilon");
+
+ valueInt = 13;
+ EXPECT_EQ(13, Math::clamp(valueInt, minInt, maxInt, epsilonInt));
+
+ valueFloat = static_cast<float>(12.0 + 1.0e-04);
+ EXPECT_FLOAT_EQ(static_cast<float>(12.0 + 1.0e-04), Math::clamp(valueFloat, minFloat, maxFloat, epsilonFloat));
+
+ valueDouble = 12.0 + 1.0e-12;
+ EXPECT_DOUBLE_EQ(12.0 + 1.0e-12, Math::clamp(valueDouble, minDouble, maxDouble, epsilonDouble));
+ }
+
+ {
+ SCOPED_TRACE("Test > maximum");
+
+ valueInt = 54;
+ EXPECT_EQ(maxInt, Math::clamp(valueInt, minInt, maxInt, epsilonInt));
+
+ valueFloat = 54.0;
+ EXPECT_FLOAT_EQ(maxFloat, Math::clamp(valueFloat, minFloat, maxFloat, epsilonFloat));
+
+ valueDouble = 54.0;
+ EXPECT_DOUBLE_EQ(maxDouble, Math::clamp(valueDouble, minDouble, maxDouble, epsilonDouble));
+ }
+
+ {
+ SCOPED_TRACE("Test value = maximum - epsilon");
+
+ valueInt = 47;
+ EXPECT_EQ(maxInt, Math::clamp(valueInt, minInt, maxInt, epsilonInt));
+
+ valueFloat = 47.0;
+ EXPECT_FLOAT_EQ(maxFloat, Math::clamp(valueFloat, minFloat, maxFloat, epsilonFloat));
+
+ valueDouble = 47.0;
+ EXPECT_DOUBLE_EQ(maxDouble, Math::clamp(valueDouble, minDouble, maxDouble, epsilonDouble));
+ }
+
+ {
+ SCOPED_TRACE("Test value < maximum - epsilon");
+
+ valueInt = 46;
+ EXPECT_EQ(46, Math::clamp(valueInt, minInt, maxInt, epsilonInt));
+
+ valueFloat = static_cast<float>(47.0 - 1.0e-04);
+ EXPECT_FLOAT_EQ(static_cast<float>(47.0 - 1.0e-04), Math::clamp(valueFloat, minFloat, maxFloat, epsilonFloat));
+
+ valueDouble = 47.0 - 1.0e-12;
+ EXPECT_DOUBLE_EQ(47.0 - 1.0e-12, Math::clamp(valueDouble, minDouble, maxDouble, epsilonDouble));
+ }
+
+ {
+ SCOPED_TRACE("Test maximum - epsilon < value > minimum + epsilon ");
+
+ valueInt = 36;
+ epsilonInt = 30;
+ EXPECT_EQ(maxInt, Math::clamp(valueInt, minInt, maxInt, epsilonInt));
+
+ valueFloat = 36.0;
+ epsilonFloat = 30;
+ EXPECT_FLOAT_EQ(maxFloat, Math::clamp(valueFloat, minFloat, maxFloat, epsilonFloat));
+
+ valueDouble = 36.0;
+ epsilonDouble = 30.0;
+ EXPECT_DOUBLE_EQ(maxDouble, Math::clamp(valueDouble, minDouble, maxDouble, epsilonDouble));
+ }
+ {
+ SCOPED_TRACE("Test matrix implementation for all cases ");
+
+ Math::Matrix33d m1 = Math::Matrix33d::Zero();
+ m1(0, 0) = 12.0;
+ m1(0, 1) = 6.0;
+ m1(0, 2) = 12.0 + 1.0e-12;
+ m1(1, 0) = 54.0;
+ m1(1, 1) = 47.0;
+ m1(1, 2) = 47.0 - 1.0e-12;
+ m1(2, 0) = 36.0;
+
+ Math::Matrix33d backup = m1;
+ double epsilonDouble = 5.0;
+
+ Math::Matrix33d result = m1.unaryExpr(clampOperator<double>(minDouble, maxDouble, epsilonDouble));
+ for (size_t row = 0; row < 3; ++row)
+ {
+ for (size_t column = 0; column < 3; ++column)
+ {
+ EXPECT_DOUBLE_EQ(Math::clamp(backup.coeffRef(row, column), minDouble, maxDouble, epsilonDouble),
+ result.coeff(row, column));
+ }
+ }
+
+ epsilonDouble = 30.0;
+
+ result = m1.unaryExpr(clampOperator<double>(minDouble, maxDouble, epsilonDouble));
+ for (size_t row = 0; row < 3; ++row)
+ {
+ for (size_t column = 0; column < 3; ++column)
+ {
+ EXPECT_DOUBLE_EQ(Math::clamp(backup.coeffRef(row, column), minDouble, maxDouble, epsilonDouble),
+ result.coeff(row, column));
+ }
+ }
+ }
+};
+
+}; // namespace Math
+
+}; // namespace SurgSim
diff --git a/SurgSim/Math/UnitTests/SegmentMeshShapeTests.cpp b/SurgSim/Math/UnitTests/SegmentMeshShapeTests.cpp
new file mode 100644
index 0000000..4f8613c
--- /dev/null
+++ b/SurgSim/Math/UnitTests/SegmentMeshShapeTests.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 <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/AabbTree.h"
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Aabb.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/SegmentMeshShape.h"
+#include "SurgSim/Math/Vector.h"
+
+using SurgSim::DataStructures::EmptyData;
+using SurgSim::DataStructures::SegmentMeshPlain;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::SegmentMeshShape;
+using SurgSim::Math::Quaterniond;
+using SurgSim::Math::Vector3d;
+
+class SegmentMeshShapeTest : public ::testing::Test
+{
+public:
+ std::shared_ptr<SegmentMeshPlain> build(const Vector3d& start, const Vector3d& direction,
+ size_t numVertices = 10) const
+ {
+ auto mesh = std::make_shared<SegmentMeshPlain>();
+
+ // Add the vertices
+ for (size_t i = 0; i < numVertices; ++i)
+ {
+ mesh->addVertex(SegmentMeshPlain::VertexType(start + direction * static_cast<double>(i)));
+ }
+
+ // Define the edges
+ for (size_t i = 0; i < numVertices - 1; ++i)
+ {
+ std::array<size_t, 2> indices = {{i, i + 1}};
+ mesh->addEdge(SegmentMeshPlain::EdgeType(indices));
+ }
+ return mesh;
+ }
+};
+
+TEST_F(SegmentMeshShapeTest, EmptyMeshTest)
+{
+ SegmentMeshPlain emptyMesh;
+ EXPECT_NO_THROW(SurgSim::Math::SegmentMeshShape(emptyMesh, 1.0));
+}
+
+TEST_F(SegmentMeshShapeTest, MeshTypeTest)
+{
+ SegmentMeshPlain emptyMesh;
+ SurgSim::Math::SegmentMeshShape segmentShape(emptyMesh, 1.0);
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_SEGMENTMESH, segmentShape.getType());
+}
+
+TEST_F(SegmentMeshShapeTest, ValidTest)
+{
+ SegmentMeshPlain emptyMesh;
+ SurgSim::Math::SegmentMeshShape shape(emptyMesh, 1.0);
+ EXPECT_TRUE(shape.isValid());
+ shape.setRadius(SurgSim::Math::Geometry::DistanceEpsilon / 2.0);
+ EXPECT_FALSE(shape.isValid());
+
+ EXPECT_TRUE(shape.isTransformable());
+}
+
+TEST_F(SegmentMeshShapeTest, AabbTest)
+{
+ std::shared_ptr<SegmentMeshPlain> mesh = build(Vector3d(-10.0, -10.0, -10.0), Vector3d(10.0, 10.0, 10.0), 3);
+ std::shared_ptr<SegmentMeshShape> shape = std::make_shared<SegmentMeshShape>(*mesh, 3.0);
+
+ SurgSim::Math::Aabbd expected(Vector3d(-13.0, -13.0, -13.0), Vector3d(13.0, 13.0, 13.0));
+
+ shape->update();
+ EXPECT_TRUE(expected.isApprox(shape->getAabbTree()->getAabb()));
+}
+
+TEST_F(SegmentMeshShapeTest, TransformTest)
+{
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::makeRotationQuaternion;
+
+ std::shared_ptr<SegmentMeshPlain> mesh = build(Vector3d(-10.0, -10.0, -10.0), Vector3d(10.0, 10.0, 10.0), 3);
+ std::shared_ptr<SegmentMeshShape> shape = std::make_shared<SegmentMeshShape>(*mesh, 3.0);
+
+ // Transform into a new mesh
+ auto newShape = std::dynamic_pointer_cast<SegmentMeshShape>(shape->getTransformed(
+ makeRigidTransform(makeRotationQuaternion(M_PI_2,
+ Vector3d(0.0, 1.0, 0.0)), Vector3d(0.0, 10.0, 0.0))));
+
+ EXPECT_TRUE(Vector3d(-10.0, 0.0, 10.0).isApprox(newShape->getVertex(0).position));
+ EXPECT_TRUE(Vector3d(0.0, 10.0, 0.0).isApprox(newShape->getVertex(1).position));
+ EXPECT_TRUE(Vector3d(10.0, 20.0, -10.0).isApprox(newShape->getVertex(2).position));
+
+ // Transform existing mesh
+ mesh->transform(
+ makeRigidTransform(makeRotationQuaternion(M_PI_2, Vector3d(0.0, 1.0, 0.0)), Vector3d(0.0, 10.0, 0.0)));
+
+ // Verify
+ EXPECT_TRUE(Vector3d(-10.0, 0.0, 10.0).isApprox(mesh->getVertex(0).position));
+ EXPECT_TRUE(Vector3d(0.0, 10.0, 0.0).isApprox(mesh->getVertex(1).position));
+ EXPECT_TRUE(Vector3d(10.0, 20.0, -10.0).isApprox(mesh->getVertex(2).position));
+}
+
+TEST_F(SegmentMeshShapeTest, LoadShape)
+{
+ auto mesh = std::make_shared<SegmentMeshShape>();
+ SurgSim::Framework::ApplicationData data("config.txt");
+
+ mesh->load("segmentmesh.ply", data);
+
+ ASSERT_EQ(4u, mesh->getNumVertices());
+ ASSERT_EQ(3u, mesh->getNumEdges());
+
+ auto edge = mesh->getEdge(0);
+ ASSERT_EQ(0u, edge.verticesId[0]);
+ ASSERT_EQ(1u, edge.verticesId[1]);
+
+ edge = mesh->getEdge(1);
+ ASSERT_EQ(1u, edge.verticesId[0]);
+ ASSERT_EQ(2u, edge.verticesId[1]);
+
+ edge = mesh->getEdge(2);
+ ASSERT_EQ(2u, edge.verticesId[0]);
+ ASSERT_EQ(3u, edge.verticesId[1]);
+
+ EXPECT_DOUBLE_EQ(1.234, mesh->getRadius());
+}
diff --git a/SurgSim/Math/UnitTests/ShapeTests.cpp b/SurgSim/Math/UnitTests/ShapeTests.cpp
index 8586127..0a17547 100644
--- a/SurgSim/Math/UnitTests/ShapeTests.cpp
+++ b/SurgSim/Math/UnitTests/ShapeTests.cpp
@@ -1,17 +1,17 @@
-//// This file is a part of the OpenSurgSim project.
-//// Copyright 2013, SimQuest Solutions Inc.
-////
-//// Licensed under the Apache License, Version 2.0 (the "License");
-//// you may not use this file except in compliance with the License.
-//// You may obtain a copy of the License at
-////
-//// http://www.apache.org/licenses/LICENSE-2.0
-////
-//// Unless required by applicable law or agreed to in writing, software
-//// distributed under the License is distributed on an "AS IS" BASIS,
-//// WITHOUT 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>
@@ -22,23 +22,19 @@
#include "SurgSim/Math/Shape.h"
#include "SurgSim/Math/Shapes.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/DataStructures/OctreeNode.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;
+namespace
+{
+const double epsilon = 1e-10;
}
+namespace SurgSim
+{
+namespace Math
+{
+
class ShapeTest : public ::testing::Test
{
public:
@@ -135,17 +131,22 @@ TEST_F(ShapeTest, Sphere)
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;
+ 0.0, coef, 0.0,
+ 0.0, 0.0, coef;
double volume = sphere.getVolume();
Vector3d center = sphere.getCenter();
Matrix33d inertia = sphere.getSecondMomentOfVolume() * m_rho;
+ Math::Aabbd aabb(Vector3d(-m_radius, -m_radius, -m_radius));
+ aabb.extend(Vector3d(m_radius, m_radius, m_radius));
+
EXPECT_NEAR(expectedVolume, volume, epsilon);
EXPECT_TRUE(center.isZero());
EXPECT_TRUE(expectedInertia.isApprox(inertia));
EXPECT_TRUE(sphere.isValid());
+ EXPECT_FALSE(sphere.isTransformable());
+ EXPECT_TRUE(aabb.isApprox(sphere.getBoundingBox()));
}
TEST_F(ShapeTest, BoxSerializationTest)
@@ -222,16 +223,21 @@ TEST_F(ShapeTest, Box)
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);
+ 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;
+ Aabbd aabb(size / 2.0);
+ aabb.extend(-(size / 2.0));
+
EXPECT_NEAR(expectedVolume, volume, epsilon);
EXPECT_TRUE(center.isZero());
EXPECT_TRUE(expectedInertia.isApprox(inertia));
+ EXPECT_FALSE(box.isTransformable());
+ EXPECT_TRUE(aabb.isApprox(box.getBoundingBox()));
}
TEST_F(ShapeTest, CylinderSerializationTest)
@@ -305,17 +311,21 @@ TEST_F(ShapeTest, Cylinder)
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;
+ 0.0, coefDir, 0.0,
+ 0.0, 0.0, coef;
double volume = cylinder.getVolume();
Vector3d center = cylinder.getCenter();
Matrix33d inertia = cylinder.getSecondMomentOfVolume() * m_rho;
+ Aabbd aabb(Vector3d(-m_radius, -m_length / 2.0, -m_radius));
+ aabb.extend(Vector3d(m_radius, m_length / 2.0, m_radius));
EXPECT_NEAR(expectedVolume, volume, epsilon);
EXPECT_TRUE(center.isZero());
EXPECT_TRUE(expectedInertia.isApprox(inertia));
EXPECT_TRUE(cylinder.isValid());
+ EXPECT_FALSE(cylinder.isTransformable());
+ EXPECT_TRUE(aabb.isApprox(cylinder.getBoundingBox()));
}
TEST_F(ShapeTest, CapsuleSerializationTest)
@@ -396,17 +406,23 @@ TEST_F(ShapeTest, Capsule)
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;
+ 0.0, coefDir, 0.0,
+ 0.0, 0.0, coef;
double volume = capsule.getVolume();
Vector3d center = capsule.getCenter();
Matrix33d inertia = capsule.getSecondMomentOfVolume() * m_rho;
+ Aabbd aabb(Vector3d(-m_radius, -m_length / 2.0 - m_radius, -m_radius));
+ aabb.extend(Vector3d(m_radius, m_length / 2.0 + m_radius, m_radius));
+
+
EXPECT_NEAR(expectedVolume, volume, epsilon);
EXPECT_TRUE(center.isZero());
EXPECT_TRUE(expectedInertia.isApprox(inertia));
EXPECT_TRUE(capsule.isValid());
+ EXPECT_FALSE(capsule.isTransformable());
+ EXPECT_TRUE(aabb.isApprox(capsule.getBoundingBox()));
}
TEST_F(ShapeTest, DoubleSidedPlaneShapeSerializationTest)
@@ -439,7 +455,7 @@ TEST_F(ShapeTest, DoubleSidedPlaneShapeSerializationTest)
std::shared_ptr<DoubleSidedPlaneShape> doubleSidedPlaneShape;
ASSERT_NO_THROW(doubleSidedPlaneShape =
- std::dynamic_pointer_cast<DoubleSidedPlaneShape>(node.as<std::shared_ptr<Shape>>()));
+ std::dynamic_pointer_cast<DoubleSidedPlaneShape>(node.as<std::shared_ptr<Shape>>()));
EXPECT_EQ("SurgSim::Math::DoubleSidedPlaneShape", doubleSidedPlaneShape->getClassName());
EXPECT_TRUE(doubleSidedPlaneShape->isValid());
}
@@ -457,98 +473,98 @@ TEST_F(ShapeTest, DoubleSidedPlaneShape)
EXPECT_NEAR(0.0, doubleSidedPlaneShape.getD(), epsilon);
EXPECT_TRUE(doubleSidedPlaneShape.getNormal().isApprox(Vector3d(0.0, 1.0, 0.0)));
EXPECT_TRUE(doubleSidedPlaneShape.isValid());
+ EXPECT_FALSE(doubleSidedPlaneShape.isTransformable());
+
+ // There is no sense to trying to build the Bounding box here
+ EXPECT_TRUE(doubleSidedPlaneShape.getBoundingBox().isEmpty());
}
-TEST_F(ShapeTest, OctreeSerializationTest)
+TEST_F(ShapeTest, OctreeShapeSerializationTest)
{
- const std::string fileName = "OctreeShapeData/staple.vox";
+ const std::string fileName = "Geometry/staple.ply";
SurgSim::Framework::Runtime runtime("config.txt");
- {
- YAML::Node node;
- node["SurgSim::Math::OctreeShape"]["FileName"] = fileName;
+ auto shape = std::make_shared<OctreeShape>();
- std::shared_ptr<Shape> shape;
- ASSERT_NO_THROW(shape = node.as<std::shared_ptr<Shape>>());
+ ASSERT_NO_THROW(shape->loadOctree(fileName));
- EXPECT_EQ("SurgSim::Math::OctreeShape", shape->getClassName());
- EXPECT_EQ(fileName, shape->getValue<std::string>("FileName"));
- EXPECT_TRUE(shape->isValid());
- }
+ YAML::Node node = YAML::convert<std::shared_ptr<SurgSim::Math::Shape>>::encode(shape);
- {
- std::shared_ptr<Shape> shape;
- ASSERT_NO_THROW(shape = Shape::getFactory().create("SurgSim::Math::OctreeShape"));
- shape->setValue("FileName", fileName);
- EXPECT_TRUE(shape->isValid());
+ EXPECT_TRUE(node[shape->getClassName()].IsMap());
+ EXPECT_TRUE(node[shape->getClassName()]["Octree"].IsMap());
- YAML::Node node;
- ASSERT_NO_THROW(node = shape);
- EXPECT_TRUE(node.IsMap());
- EXPECT_EQ(1u, node.size());
+ std::shared_ptr<SurgSim::Math::Shape> decodedShape;
+ ASSERT_NO_THROW(decodedShape = node.as<std::shared_ptr<SurgSim::Math::Shape>>());
+ ASSERT_NE(nullptr, decodedShape);
- ASSERT_TRUE(node["SurgSim::Math::OctreeShape"].IsDefined());
- auto data = node["SurgSim::Math::OctreeShape"];
- EXPECT_EQ(1u, data.size());
+ auto convertedShape = std::dynamic_pointer_cast<OctreeShape>(decodedShape);
+ ASSERT_NE(nullptr, convertedShape);
- 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());
- }
+ ASSERT_NE(nullptr, convertedShape->getOctree());
+ EXPECT_EQ(fileName, convertedShape->getOctree()->getFileName());
}
TEST_F(ShapeTest, OctreeShape)
{
OctreeShape::NodeType::AxisAlignedBoundingBox boundingBox(Vector3d::Zero(), m_size);
std::shared_ptr<OctreeShape::NodeType> node = std::make_shared<OctreeShape::NodeType>(boundingBox);
+ SurgSim::Framework::Runtime runtime("config.txt");
+
{
- ASSERT_NO_THROW({OctreeShape octree; EXPECT_FALSE(octree.isValid());});
- ASSERT_NO_THROW({OctreeShape octree(*node); EXPECT_TRUE(octree.isValid());});
+ ASSERT_NO_THROW({OctreeShape shape; EXPECT_FALSE(shape.isValid());});
+ ASSERT_NO_THROW({OctreeShape shape(*node); EXPECT_TRUE(shape.isValid());});
}
{
- OctreeShape octree;
- EXPECT_EQ(nullptr, octree.getRootNode());
- octree.setRootNode(node);
- EXPECT_EQ(node, octree.getRootNode());
- EXPECT_TRUE(octree.isValid());
+ OctreeShape shape;
+ EXPECT_NE(nullptr, shape.getOctree());
+ shape.setOctree(node);
+ EXPECT_EQ(node, shape.getOctree());
+ EXPECT_TRUE(shape.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("Normal Loading");
+ const std::string fileName = "Geometry/staple.ply";
+ OctreeShape shape;
+ EXPECT_NO_THROW(shape.setOctree(node));
+ EXPECT_NO_THROW(shape.loadOctree(fileName));
+ EXPECT_EQ(fileName, shape.getOctree()->getFileName());
+
+ EXPECT_EQ(shape.getClassName(), "SurgSim::Math::OctreeShape");
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_OCTREE, shape.getType());
+ EXPECT_THROW(shape.getVolume(), SurgSim::Framework::AssertionFailure);
+ EXPECT_TRUE(shape.getCenter().isApprox(Vector3d::Zero(), epsilon));
+ EXPECT_THROW(shape.getSecondMomentOfVolume(), SurgSim::Framework::AssertionFailure);
+ EXPECT_EQ(fileName, shape.getOctree()->getFileName());
+ EXPECT_TRUE(shape.isValid());
+ EXPECT_TRUE(shape.getOctree()->getBoundingBox().isApprox(shape.getBoundingBox()));
+ }
+
+ {
+ SCOPED_TRACE("Alternative load through property");
+ const std::string fileName = "Geometry/staple.ply";
+ OctreeShape shape;
+ EXPECT_NO_THROW(shape.setValue("OctreeFileName", fileName));
+ EXPECT_EQ(fileName, shape.getOctree()->getFileName());
}
{
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));
+ OctreeShape shape;
+ EXPECT_ANY_THROW(shape.loadOctree(fileName));
}
{
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));
+ const std::string fileName = "Geometry/invalid-staple.ply";
+ OctreeShape shape;
+ EXPECT_ANY_THROW(shape.loadOctree(fileName));
}
}
@@ -599,4 +615,10 @@ TEST_F(ShapeTest, PlaneShape)
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
+ EXPECT_FALSE(planeShape.isTransformable());
+ EXPECT_TRUE(planeShape.getBoundingBox().isEmpty());
+}
+
+
+}
+}
diff --git a/SurgSim/Math/UnitTests/SparseMatrixTests.cpp b/SurgSim/Math/UnitTests/SparseMatrixTests.cpp
new file mode 100644
index 0000000..ecdce26
--- /dev/null
+++ b/SurgSim/Math/UnitTests/SparseMatrixTests.cpp
@@ -0,0 +1,363 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2012-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file SparseMatrixTests.cpp
+/// Tests that exercise the functionality of our sparse matrices, which come
+/// straight from Eigen.
+
+#include <gtest/gtest.h>
+#include <tuple>
+
+#include "SurgSim/Math/SparseMatrix.h"
+
+using std::tuple;
+using std::tuple_element;
+using SurgSim::Math::Matrix;
+
+template <size_t N> class TypeValue
+{
+public:
+ static const size_t value = N;
+};
+template <size_t N> const size_t TypeValue<N>::value;
+
+template <typename Tuple>
+class SparseMatrices : public ::testing::Test
+{
+public:
+ typedef typename tuple_element<0, Tuple>::type T;
+ static const int Opt = tuple_element<1, Tuple>::type::value;
+ typedef typename tuple_element<2, Tuple>::type I;
+
+ void SetUp() override
+ {
+ m_columnId = 6;
+ m_rowId = 11;
+
+ m_matrixTooSmall.resize(m_n - 1, m_m - 1);
+ m_matrixTooSmall.setZero();
+
+ m_matrixWithoutExtraCoefficients.resize(18, 18);
+ m_matrixWithoutExtraCoefficients.setZero();
+ m_matrixMissingCoefficients.resize(18, 18);
+ m_matrixMissingCoefficients.setZero();
+ m_matrixWithoutExtraCoefficientsExpected.resize(18, 18);
+ m_matrixWithoutExtraCoefficientsExpected.setZero();
+ for (I i = 0; i < m_rowId; i++)
+ {
+ for (I j = 0; j < m_columnId; j++)
+ {
+ m_matrixWithoutExtraCoefficients.insert(i, j) = 1.0;
+ m_matrixMissingCoefficients.insert(i, j) = 1.0;
+ m_matrixWithoutExtraCoefficientsExpected.insert(i, j) = 1.0;
+ }
+ for (I j = m_columnId + m_m; j < 18; j++)
+ {
+ m_matrixWithoutExtraCoefficients.insert(i, j) = 1.0;
+ m_matrixMissingCoefficients.insert(i, j) = 1.0;
+ m_matrixWithoutExtraCoefficientsExpected.insert(i, j) = 1.0;
+ }
+ }
+ for (I i = m_rowId; i < m_rowId + m_n; i++)
+ {
+ for (I j = m_columnId; j < m_columnId + m_m; j++)
+ {
+ m_matrixWithoutExtraCoefficients.insert(i , j) = 1.0;
+ if (i % 2 == 0 || j % 2 == 0)
+ {
+ m_matrixMissingCoefficients.insert(i, j) = 1.0;
+ }
+ m_matrixWithoutExtraCoefficientsExpected.insert(i, j) =
+ (i - m_rowId == 0 && j - m_columnId == 3 ? 2 :
+ static_cast<T>(i - m_rowId + 1) * static_cast<T>(j - m_columnId + 1));
+ }
+ }
+ for (I i = m_rowId + m_n; i < 18; i++)
+ {
+ for (I j = 0; j < m_columnId; j++)
+ {
+ m_matrixWithoutExtraCoefficients.insert(i, j) = 1.0;
+ m_matrixMissingCoefficients.insert(i, j) = 1.0;
+ m_matrixWithoutExtraCoefficientsExpected.insert(i, j) = 1.0;
+ }
+ for (I j = m_columnId + m_m; j < 18; j++)
+ {
+ m_matrixWithoutExtraCoefficients.insert(i, j) = 1.0;
+ m_matrixMissingCoefficients.insert(i, j) = 1.0;
+ m_matrixWithoutExtraCoefficientsExpected.insert(i, j) = 1.0;
+ }
+ }
+ m_matrixWithExtraCoefficients = m_matrixWithoutExtraCoefficients;
+ m_matrixWithExtraCoefficientsExpected = m_matrixWithoutExtraCoefficientsExpected;
+ addElementOnBlockRowsCols(&m_matrixWithExtraCoefficients);
+ addElementOnBlockRowsCols(&m_matrixWithExtraCoefficientsExpected);
+
+ m_matrixWithoutExtraCoefficients.makeCompressed();
+ m_matrixMissingCoefficients.makeCompressed();
+ m_matrixWithoutExtraCoefficientsExpected.makeCompressed();
+ }
+
+ void addElementOnBlockRowsCols(Eigen::SparseMatrix<T, Opt, I>* m)
+ {
+ m->insert(m_rowId, m_columnId - 3) = 1.0;
+ m->insert(m_rowId, m_columnId - 1) = 1.0;
+ m->insert(m_rowId - 4, m_columnId) = 1.0;
+ m->insert(m_rowId - 2, m_columnId) = 1.0;
+ m->insert(m_rowId - 3, m_columnId + 1) = 1.0;
+ m->makeCompressed();
+ }
+
+ void TestSetWithSearchDynamic(const Eigen::Ref<const Matrix>& sub, bool subTooSmall = false,
+ bool success = true)
+ {
+ using SurgSim::Math::blockWithSearch;
+ using SurgSim::Math::Operation;
+
+ SetUp();
+
+ if (subTooSmall)
+ {
+ EXPECT_THROW((blockWithSearch(sub, m_rowId, m_columnId, m_n, m_m,\
+ &m_matrixWithExtraCoefficients,\
+ &Operation<Matrix, Eigen::SparseMatrix<T, Opt, I>>::assign)),\
+ SurgSim::Framework::AssertionFailure);
+ }
+ else
+ {
+ // No recipient specified
+ EXPECT_THROW((blockWithSearch<Opt, I>(sub, m_rowId, m_columnId, m_n, m_m,\
+ nullptr, &Operation<Matrix, Eigen::SparseMatrix<T, Opt, I>>::assign)),\
+ SurgSim::Framework::AssertionFailure);
+
+ // Recipient too small
+ EXPECT_THROW((blockWithSearch(sub, m_rowId, m_columnId, m_n, m_m, &m_matrixTooSmall,\
+ &Operation<Matrix, Eigen::SparseMatrix<T, Opt, I>>::assign)),\
+ SurgSim::Framework::AssertionFailure);
+
+ // Recipient does not have all the block coefficients (missing coefficients in the block)
+ EXPECT_THROW((blockWithSearch(sub, m_rowId, m_columnId, m_n, m_m, &m_matrixMissingCoefficients,\
+ &Operation<Matrix, Eigen::SparseMatrix<T, Opt, I>>::assign)),\
+ SurgSim::Framework::AssertionFailure);
+
+ // Recipient is correct and sub is correct
+ EXPECT_NO_THROW((blockWithSearch(sub, m_rowId, m_columnId, m_n, m_m, &m_matrixWithExtraCoefficients,\
+ &Operation<Matrix, Eigen::SparseMatrix<T, Opt, I>>::assign)));
+
+ if (success)
+ {
+ EXPECT_TRUE(m_matrixWithExtraCoefficients.isApprox(m_matrixWithExtraCoefficientsExpected));
+ }
+ else
+ {
+ EXPECT_FALSE(m_matrixWithExtraCoefficients.isApprox(m_matrixWithExtraCoefficientsExpected));
+ }
+ }
+ }
+
+ void TestAddWithSearchDynamic(const Eigen::Ref<const Matrix>& sub, bool subTooSmall = false,
+ bool success = true)
+ {
+ using SurgSim::Math::blockWithSearch;
+ using SurgSim::Math::Operation;
+
+ SetUp();
+
+ if (subTooSmall)
+ {
+ EXPECT_THROW((blockWithSearch(sub, m_rowId, m_columnId, m_n, m_m, &m_matrixWithExtraCoefficients,\
+ &Operation<Matrix, Eigen::SparseMatrix<T, Opt, I>>::add)),\
+ SurgSim::Framework::AssertionFailure);
+ }
+ else
+ {
+ // No recipient specified
+ EXPECT_THROW((blockWithSearch<Opt, I>(sub, m_rowId, m_columnId, m_n, m_m, nullptr,\
+ &Operation<Matrix, Eigen::SparseMatrix<T, Opt, I>>::add)),\
+ SurgSim::Framework::AssertionFailure);
+
+ // Recipient too small
+ EXPECT_THROW((blockWithSearch(sub, m_rowId, m_columnId, m_n, m_m, &m_matrixTooSmall,
+ &Operation<Matrix, Eigen::SparseMatrix<T, Opt, I>>::add)),\
+ SurgSim::Framework::AssertionFailure);
+
+ // Recipient does not have all the block coefficients (missing coefficients in the block)
+ EXPECT_THROW((blockWithSearch(sub, m_rowId, m_columnId, m_n, m_m, &m_matrixMissingCoefficients,
+ &Operation<Matrix, Eigen::SparseMatrix<T, Opt, I>>::add)),\
+ SurgSim::Framework::AssertionFailure);
+
+ // Recipient is correct and sub is correct
+ EXPECT_NO_THROW((blockWithSearch(sub, m_rowId, m_columnId, m_n, m_m, &m_matrixWithExtraCoefficients,
+ &Operation<Matrix, Eigen::SparseMatrix<T, Opt, I>>::add)));
+
+ Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> dense(m_matrixWithExtraCoefficients);
+ Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> denseSub(sub);
+ auto ones = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Ones(m_n, m_m);
+ auto expectedMatrix = denseSub.block(0, 0, m_n, m_m) + ones;
+ if (success)
+ {
+ EXPECT_TRUE((dense.block(m_rowId, m_columnId, m_n, m_m).isApprox(expectedMatrix)));
+ }
+ else
+ {
+ EXPECT_FALSE((dense.block(m_rowId, m_columnId, m_n, m_m).isApprox(expectedMatrix)));
+ }
+ }
+ }
+
+ template <class T, int n, int m, int Opt>
+ Eigen::Matrix<T, n, m, Opt> getStaticMatrix()
+ {
+ Eigen::Matrix<T, n, 1> vec1 = Eigen::Matrix<T, n, 1>::LinSpaced(1.0, n);
+ Eigen::Matrix<T, 1, m> vec2 = Eigen::Matrix<T, 1, m>::LinSpaced(1.0, m);
+ Eigen::Matrix<T, n, m, Opt> result = vec1 * vec2;
+ if (m >= 4)
+ {
+ result(0, 3) = 2;
+ }
+ return result;
+ }
+
+ template <class T, int n, int m, int Opt>
+ Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Opt> getDynamicMatrix()
+ {
+ Eigen::Matrix<T, Eigen::Dynamic, 1> vec1 = Eigen::Matrix<T, Eigen::Dynamic, 1>::LinSpaced(n, 1.0, n);
+ Eigen::Matrix<T, 1, Eigen::Dynamic> vec2 = Eigen::Matrix<T, 1, Eigen::Dynamic>::LinSpaced(m, 1.0, m);
+ Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Opt> result = vec1 * vec2;
+ if (m >= 4)
+ {
+ result(0, 3) = 2;
+ }
+ return result;
+ }
+
+ template <class T, int n, int m, int Opt>
+ Eigen::SparseMatrix<T, Opt> getSparseMatrix()
+ {
+ typedef typename Eigen::SparseVector<T, Eigen::ColMajor>::Index Index1;
+ typedef typename Eigen::SparseVector<T, Eigen::RowMajor>::Index Index2;
+
+ Eigen::SparseVector<T, Eigen::ColMajor> vec1(n);
+ Eigen::SparseVector<T, Eigen::RowMajor> vec2(m);
+ for (Index1 i = 0; i < n; i++)
+ {
+ vec1.insert(i) = static_cast<T>(i + 1);
+ }
+ for (Index2 i = 0; i < m; i++)
+ {
+ vec2.insert(i) = static_cast<T>(i + 1);
+ }
+
+ Eigen::SparseMatrix<T, Opt> result(vec1 * vec2);
+ if (m >= 4)
+ {
+ result.coeffRef(0, 3) = 2;
+ }
+ return result;
+ }
+
+ template <class T, int n, int Opt>
+ Eigen::SparseVector<T, Opt> getSparseVector()
+ {
+ typedef typename Eigen::SparseVector<T, Opt>::Index Index;
+
+ Eigen::SparseVector<T, Opt> vec(n);
+ for (Index i = 0; i < n; i++)
+ {
+ vec.insert(i) = static_cast<T>(i + 1);
+ }
+
+ return vec;
+ }
+
+protected:
+ Eigen::SparseMatrix<T, Opt, I> m_matrixWithoutExtraCoefficients;
+ Eigen::SparseMatrix<T, Opt, I> m_matrixWithoutExtraCoefficientsExpected;
+ Eigen::SparseMatrix<T, Opt, I> m_matrixWithExtraCoefficients;
+ Eigen::SparseMatrix<T, Opt, I> m_matrixWithExtraCoefficientsExpected;
+ Eigen::SparseMatrix<T, Opt, I> m_matrixTooSmall;
+ Eigen::SparseMatrix<T, Opt, I> m_matrixMissingCoefficients;
+
+ I m_rowId, m_columnId;
+ static const I m_n = 4;
+ static const I m_m = 4;
+};
+template <typename Tuple> const typename tuple_element<2, Tuple>::type SparseMatrices<Tuple>::m_n;
+template <typename Tuple> const typename tuple_element<2, Tuple>::type SparseMatrices<Tuple>::m_m;
+
+template <typename Tuple>
+const int SparseMatrices<Tuple>::Opt;
+
+typedef ::testing::Types <
+tuple<double, TypeValue<Eigen::ColMajor>, int>,
+ tuple<double, TypeValue<Eigen::RowMajor>, int>,
+ tuple<double, TypeValue<Eigen::ColMajor>, ptrdiff_t>,
+ tuple<double, TypeValue<Eigen::RowMajor>, ptrdiff_t>> MyTypes;
+TYPED_TEST_CASE(SparseMatrices, MyTypes);
+
+TYPED_TEST(SparseMatrices, setSubMatrixWithSearchDynamicCall)
+{
+ typedef typename tuple_element<0, TypeParam>::type T;
+ const int Opt = tuple_element<1, TypeParam>::type::value;
+ const int OtherOpt = (Opt == Eigen::ColMajor ? Eigen::RowMajor : Eigen::ColMajor);
+
+ {
+ SCOPED_TRACE("Test with static dense input sub-matrix");
+ this->TestSetWithSearchDynamic(this->template getStaticMatrix<T, 3, 4, Opt>(), true); ///< Sub too small (1D)
+ this->TestSetWithSearchDynamic(this->template getStaticMatrix<T, 3, 3, Opt>(), true); ///< Sub too small (2D)
+ this->TestSetWithSearchDynamic(this->template getStaticMatrix<T, 4, 4, Opt>());
+ this->TestSetWithSearchDynamic(this->template getStaticMatrix<T, 4, 4, OtherOpt>());
+ this->TestSetWithSearchDynamic(this->template getStaticMatrix<T, 6, 4, Opt>()); ///< Sub larger (1D)
+ this->TestSetWithSearchDynamic(this->template getStaticMatrix<T, 5, 6, Opt>()); ///< Sub larger (2D)
+ }
+
+ {
+ SCOPED_TRACE("Test with dynamic dense input sub-matrix");
+ this->TestSetWithSearchDynamic(this->template getDynamicMatrix<T, 3, 4, Opt>(), true); ///< Sub too small (1D)
+ this->TestSetWithSearchDynamic(this->template getDynamicMatrix<T, 3, 3, Opt>(), true); ///< Sub too small (2D)
+ this->TestSetWithSearchDynamic(this->template getDynamicMatrix<T, 4, 4, Opt>());
+ this->TestSetWithSearchDynamic(this->template getDynamicMatrix<T, 4, 4, OtherOpt>());
+ this->TestSetWithSearchDynamic(this->template getDynamicMatrix<T, 6, 4, Opt>()); ///< Sub larger (1D)
+ this->TestSetWithSearchDynamic(this->template getDynamicMatrix<T, 5, 6, Opt>()); ///< Sub larger (2D)
+ }
+}
+
+TYPED_TEST(SparseMatrices, addSubMatrixWithSearchDynamicCall)
+{
+ typedef typename tuple_element<0, TypeParam>::type T;
+ const int Opt = tuple_element<1, TypeParam>::type::value;
+ const int OtherOpt = (Opt == Eigen::ColMajor ? Eigen::RowMajor : Eigen::ColMajor);
+
+ {
+ SCOPED_TRACE("Test with static dense input sub-matrix");
+ this->TestAddWithSearchDynamic(this->template getStaticMatrix<T, 3, 4, Opt>(), true); ///< Sub too small (1D)
+ this->TestAddWithSearchDynamic(this->template getStaticMatrix<T, 3, 3, Opt>(), true); ///< Sub too small (2D)
+ this->TestAddWithSearchDynamic(this->template getStaticMatrix<T, 4, 4, Opt>());
+ this->TestAddWithSearchDynamic(this->template getStaticMatrix<T, 4, 4, OtherOpt>());
+ this->TestAddWithSearchDynamic(this->template getStaticMatrix<T, 6, 4, Opt>()); ///< Sub larger (1D)
+ this->TestAddWithSearchDynamic(this->template getStaticMatrix<T, 5, 6, Opt>()); ///< Sub larger (2D)
+ }
+
+ {
+ SCOPED_TRACE("Test with dynamic dense input sub-matrix");
+ this->TestAddWithSearchDynamic(this->template getDynamicMatrix<T, 3, 4, Opt>(), true); ///< Sub too small (1D)
+ this->TestAddWithSearchDynamic(this->template getDynamicMatrix<T, 3, 3, Opt>(), true); ///< Sub too small (2D)
+ this->TestAddWithSearchDynamic(this->template getDynamicMatrix<T, 4, 4, Opt>());
+ this->TestAddWithSearchDynamic(this->template getDynamicMatrix<T, 4, 4, OtherOpt>());
+ this->TestAddWithSearchDynamic(this->template getDynamicMatrix<T, 6, 4, Opt>()); ///< Sub larger (1D)
+ this->TestAddWithSearchDynamic(this->template getDynamicMatrix<T, 5, 6, Opt>()); ///< Sub larger (2D)
+ }
+}
+
+
diff --git a/SurgSim/Math/UnitTests/SurfaceMeshShapeTests.cpp b/SurgSim/Math/UnitTests/SurfaceMeshShapeTests.cpp
index d8c1024..b81c7eb 100644
--- a/SurgSim/Math/UnitTests/SurfaceMeshShapeTests.cpp
+++ b/SurgSim/Math/UnitTests/SurfaceMeshShapeTests.cpp
@@ -16,6 +16,7 @@
#include <gtest/gtest.h>
#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/Framework/Runtime.h"
#include "SurgSim/Math/MathConvert.h"
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/SurfaceMeshShape.h"
@@ -55,27 +56,27 @@ public:
double m_expectedVolume;
Matrix33d m_expectedMatrix;
- TriangleMeshPlain buildDiskZ(const Quaterniond& q, const Vector3d& center, double radius) const
+ std::shared_ptr<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;
+ auto disk = std::make_shared<TriangleMeshPlain>();
// Add the center point
- disk.addVertex(TriangleMeshPlain::VertexType(center));
+ 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));
+ 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));
+ disk->addTriangle(TriangleMeshPlain::TriangleType(indices));
}
return disk;
}
@@ -93,8 +94,8 @@ TEST_F(SurfaceMeshShapeTest, EmptyMeshTest)
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);
+ std::shared_ptr<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));
@@ -105,8 +106,8 @@ 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);
+ std::shared_ptr<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();
@@ -118,9 +119,10 @@ TEST_F(SurfaceMeshShapeTest, NonAlignedDiskShapeTest)
TEST_F(SurfaceMeshShapeTest, SerializationTest)
{
- const std::string fileName = "MeshShapeData/staple_collision.ply";
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ const std::string fileName = "Geometry/staple_collision.ply";
auto surfaceMeshShape = std::make_shared<SurgSim::Math::SurfaceMeshShape>();
- surfaceMeshShape->setFileName(fileName);
+ surfaceMeshShape->load(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.
@@ -138,7 +140,7 @@ TEST_F(SurfaceMeshShapeTest, SerializationTest)
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());
+ EXPECT_EQ(surfaceMeshShape->getNumVertices(), newSurfaceMesh->getNumVertices());
+ EXPECT_EQ(surfaceMeshShape->getNumEdges(), newSurfaceMesh->getNumEdges());
+ EXPECT_EQ(surfaceMeshShape->getNumTriangles(), newSurfaceMesh->getNumTriangles());
}
diff --git a/SurgSim/Math/UnitTests/TriangleCapsuleContactCalculationTests.cpp b/SurgSim/Math/UnitTests/TriangleCapsuleContactCalculationTests.cpp
new file mode 100644
index 0000000..e79c85f
--- /dev/null
+++ b/SurgSim/Math/UnitTests/TriangleCapsuleContactCalculationTests.cpp
@@ -0,0 +1,483 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/MockCapsule.h"
+#include "SurgSim/Math/UnitTests/MockTriangle.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+class TriangleCapsuleContactCalculationTest : public ::testing::Test
+{
+protected:
+ typedef std::tuple<std::string, // String to describe the scenario.
+ MockTriangle, // The triangle.
+ MockCapsule, // The capsule.
+ bool, // Flag to indicate if the two shapes 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 triangle.
+ Vector3d> // Expected penetration point in the capsule.
+ TriangleCapsuleTestCase;
+
+ SurgSim::Math::RigidTransform3d buildRigidTransform(double angle, double axisX, double axisY, double axisZ,
+ double translationX, double translationY, double translationZ)
+ {
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::makeRotationQuaternion;
+ return makeRigidTransform(makeRotationQuaternion(angle, Vector3d(axisX, axisY, axisZ).normalized()),
+ Vector3d(translationX, translationY, translationZ));
+ }
+
+ void SetUp() override
+ {
+ m_transforms.push_back(buildRigidTransform(0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0));
+ m_transforms.push_back(buildRigidTransform(1.234, 17.04, 2.047, 3.052, 23.34, 42.45, 83.68));
+ m_transforms.push_back(buildRigidTransform(-5.34, 41.03, -2.52, -3.84, -3.45, 66.47, 29.34));
+ m_transforms.push_back(buildRigidTransform(0.246, -9.42, -4.86, 2.469, 37.68, -34.6, -17.1));
+ m_transforms.push_back(buildRigidTransform(-0.85, 3.344, 8.329, -97.4, 9.465, 0.275, -95.9));
+ }
+
+ 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 testTriangleCapsuleContactCalculation(const TriangleCapsuleTestCase& data)
+ {
+ MockTriangle t = std::get<1>(data);
+ MockCapsule c = std::get<2>(data);
+ bool contactExpected = std::get<3>(data);
+ bool checkForPenetrationPoints = std::get<4>(data);
+ Vector3d expectedTPoint = std::get<5>(data);
+ Vector3d expectedCPoint = std::get<6>(data);
+
+ double expectedPenetrationDepth = (expectedCPoint - expectedTPoint).norm();
+ double penetrationDepth;
+ Vector3d tPoint, cPoint, normal, cPointAxis;
+ bool contactFound = false;
+ std::string traceMessage[12] = {"Triangle vs Capsule",
+ "Triangle (vertices shifted once) vs Capsule",
+ "Triangle (vertices shifted twice) vs Capsule",
+ "Triangle vs Capsule (vertices interchanged)",
+ "Triangle (vertices shifted once) vs Capsule (vertices interchanged)",
+ "Triangle (vertices shifted twice) vs Capsule (vertices interchanged)",
+ "Reversed Triangle vs Capsule",
+ "Reversed Triangle (vertices shifted once) vs Capsule",
+ "Reversed Triangle (vertices shifted twice) vs Capsule",
+ "Reversed Triangle vs Capsule (vertices interchanged)",
+ "Reversed Triangle (vertices shifted once) vs Capsule (vertices interchanged)",
+ "Reversed Triangle (vertices shifted twice) vs Capsule (vertices interchanged)"
+ };
+ for (int count = 0; count < 12; ++count)
+ {
+ SCOPED_TRACE(std::get<0>(data) + ": " + traceMessage[count]);
+
+ switch (count)
+ {
+ case 0:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleCapsule(t.v0, t.v1, t.v2, t.n, c.v0, c.v1, c.r,
+ &penetrationDepth, &tPoint, &cPoint, &normal, &cPointAxis));
+ break;
+ case 1:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleCapsule(t.v1, t.v2, t.v0, t.n, c.v0, c.v1, c.r,
+ &penetrationDepth, &tPoint, &cPoint, &normal, &cPointAxis));
+ break;
+ case 2:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleCapsule(t.v2, t.v0, t.v1, t.n, c.v0, c.v1, c.r,
+ &penetrationDepth, &tPoint, &cPoint, &normal, &cPointAxis));
+ break;
+ case 3:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleCapsule(t.v0, t.v1, t.v2, t.n, c.v1, c.v0, c.r,
+ &penetrationDepth, &tPoint, &cPoint, &normal, &cPointAxis));
+ break;
+ case 4:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleCapsule(t.v1, t.v2, t.v0, t.n, c.v1, c.v0, c.r,
+ &penetrationDepth, &tPoint, &cPoint, &normal, &cPointAxis));
+ break;
+ case 5:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleCapsule(t.v2, t.v0, t.v1, t.n, c.v1, c.v0, c.r,
+ &penetrationDepth, &tPoint, &cPoint, &normal, &cPointAxis));
+ break;
+ case 6:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleCapsule(t.v2, t.v1, t.v0, c.v0, c.v1, c.r,
+ &penetrationDepth, &tPoint, &cPoint, &normal, &cPointAxis));
+ break;
+ case 7:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleCapsule(t.v1, t.v0, t.v2, c.v0, c.v1, c.r,
+ &penetrationDepth, &tPoint, &cPoint, &normal, &cPointAxis));
+ break;
+ case 8:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleCapsule(t.v0, t.v2, t.v1, c.v0, c.v1, c.r,
+ &penetrationDepth, &tPoint, &cPoint, &normal, &cPointAxis));
+ break;
+ case 9:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleCapsule(t.v2, t.v1, t.v0, c.v1, c.v0, c.r,
+ &penetrationDepth, &tPoint, &cPoint, &normal, &cPointAxis));
+ break;
+ case 10:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleCapsule(t.v1, t.v0, t.v2, c.v1, c.v0, c.r,
+ &penetrationDepth, &tPoint, &cPoint, &normal, &cPointAxis));
+ break;
+ case 11:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleCapsule(t.v0, t.v2, t.v1, c.v1, c.v0, c.r,
+ &penetrationDepth, &tPoint, &cPoint, &normal, &cPointAxis));
+ break;
+ }
+
+ EXPECT_EQ(contactExpected, contactFound);
+ if (contactFound)
+ {
+ if (checkForPenetrationPoints && count < 6)
+ {
+ EXPECT_NEAR(expectedPenetrationDepth, penetrationDepth, Geometry::DistanceEpsilon);
+ checkEqual(expectedTPoint, tPoint);
+ checkEqual(expectedCPoint, cPoint);
+ }
+
+ // Check that tPoint is on the plane of t.
+ double tSignedDistance = std::abs(tPoint.dot(t.n) - t.v0.dot(t.n));
+ EXPECT_NEAR(tSignedDistance, 0.0, Geometry::DistanceEpsilon);
+
+ // Check that cPoint is on the surface of c.
+ Vector3d result;
+ double cDistance = SurgSim::Math::distancePointSegment(cPoint, c.v0, c.v1, &result) - c.r;
+ EXPECT_LE(std::abs(cDistance), Geometry::DistanceEpsilon);
+
+ // Check that tPoint is inside t.
+ EXPECT_TRUE(SurgSim::Math::isPointInsideTriangle(tPoint, t.v0, t.v1, t.v2));
+
+ // Check if the penetration depth when applied as correction, separates the shapes.
+ // First move the shapes apart by just short of the penetration depth, to make sure
+ // the shapes are still colliding.
+ {
+ Vector3d tP, cP;
+ Vector3d correction = normal * (0.5 * penetrationDepth - Geometry::DistanceEpsilon);
+ MockTriangle correctedT(t);
+ correctedT.translate(correction);
+ MockCapsule correctedC(c);
+ correctedC.translate(-correction);
+ auto correctedDistance = distanceSegmentTriangle(correctedC.v0, correctedC.v1, correctedT.v0,
+ correctedT.v1, correctedT.v2, correctedT.n, &cP, &tP) - c.r;
+ EXPECT_TRUE(correctedDistance >= -4.0 * Geometry::DistanceEpsilon)
+ << "correctedDistance = " << correctedDistance;
+ EXPECT_TRUE(correctedDistance <= Geometry::DistanceEpsilon)
+ << "correctedDistance = " << correctedDistance;
+ }
+ // Now move the shapes apart by just a little farther than the penetration depth, to establish
+ // that the shapes are not colliding.
+ {
+ Vector3d tP, cP;
+ Vector3d correction = normal * (0.5 * penetrationDepth + Geometry::DistanceEpsilon);
+ MockTriangle correctedT(t);
+ correctedT.translate(correction);
+ MockCapsule correctedC(c);
+ correctedC.translate(-correction);
+ auto correctedDistance = distanceSegmentTriangle(correctedC.v0, correctedC.v1, correctedT.v0,
+ correctedT.v1, correctedT.v2, correctedT.n, &cP, &tP) - c.r;
+ EXPECT_TRUE(correctedDistance <= 4.0 * Geometry::DistanceEpsilon)
+ << "correctedDistance = " << correctedDistance;
+ EXPECT_TRUE(correctedDistance >= -Geometry::DistanceEpsilon)
+ << "correctedDistance = " << correctedDistance;
+ }
+ }
+ }
+ }
+
+ void testTriangleCapsuleContactCalculation(std::string scenario, Vector3d cv0, Vector3d cv1, bool contactFound,
+ bool checkPenetrationPoints = false, Vector3d pointOnTriangle = Vector3d::Zero(),
+ Vector3d pointOnCapsule = Vector3d::Zero())
+ {
+ MockTriangle t(Vector3d(5, -5, 0), Vector3d(0, 5, 0), Vector3d(-5, -5, 0));
+ MockCapsule c(MockCapsule(cv0, cv1, 0.5));
+
+ for (const auto& transform : m_transforms)
+ {
+ MockTriangle transformedT(t);
+ transformedT.transform(transform);
+ MockCapsule transformedC(c);
+ transformedC.transform(transform);
+ testTriangleCapsuleContactCalculation(TriangleCapsuleTestCase(scenario, transformedT, transformedC,
+ contactFound, checkPenetrationPoints, (transform * pointOnTriangle).eval(),
+ (transform * pointOnCapsule).eval()));
+ }
+ }
+
+private:
+ // List of random transformations.
+ std::vector<SurgSim::Math::RigidTransform3d> m_transforms;
+};
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase1)
+{
+ testTriangleCapsuleContactCalculation("(Perpendicular) Capsule far away from triangle",
+ Vector3d(0.0, 0.0, 10.0), Vector3d(0.0, 0.0, 5.0), false);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase2)
+{
+ testTriangleCapsuleContactCalculation("(Perpendicular) Capsule far away from triangle",
+ Vector3d(0.0, 0.0, 10.0), Vector3d(0.0, 0.0, 5.0), false);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase3)
+{
+ testTriangleCapsuleContactCalculation("(Perpendicular) Capsule just away triangle",
+ Vector3d(0.0, 0.0, 10.0), Vector3d(0.0, 0.0, 0.500001), false);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase4)
+{
+ testTriangleCapsuleContactCalculation("(Perpendicular) Capsule just touching triangle",
+ Vector3d(0.0, 0.0, 10.0), Vector3d(0.0, 0.0, 0.49999), true, true, Vector3d::Zero(), Vector3d(0, 0, -0.00001));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase5)
+{
+ testTriangleCapsuleContactCalculation("(Perpendicular) Capsule axis just away from triangle",
+ Vector3d(0.0, 0.0, 10.0), Vector3d(0.0, 0.0, 0.0001), true, true, Vector3d::Zero(), Vector3d(0, 0, -0.4999));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase6)
+{
+ testTriangleCapsuleContactCalculation("(Perpendicular) Capsule axis just touching triangle",
+ Vector3d(0.0, 0.0, 10.0), Vector3d(0.0, 0.0, -0.0001), true, true, Vector3d::Zero(), Vector3d(0, 0, -0.5001));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase7)
+{
+ testTriangleCapsuleContactCalculation("(Angled) Capsule far away from triangle",
+ Vector3d(1.0, 1.0, 10.0), Vector3d(0.0, 0.0, 5.0), false);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase8)
+{
+ testTriangleCapsuleContactCalculation("(Angled) Capsule just away triangle",
+ Vector3d(1.0, 1.0, 10.0), Vector3d(0.0, 0.0, 0.500001), false);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase9)
+{
+ testTriangleCapsuleContactCalculation("(Angled) Capsule just touching triangle",
+ Vector3d(1.0, 1.0, 10.0), Vector3d(0.0, 0.0, 0.49999), true, true, Vector3d::Zero(), Vector3d(0, 0, -0.00001));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase10)
+{
+ testTriangleCapsuleContactCalculation("(Angled) Capsule axis just away from triangle",
+ Vector3d(1.0, 1.0, 10.0), Vector3d(0.0, 0.0, 0.0001), true, true, Vector3d::Zero(), Vector3d(0, 0, -0.4999));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase11)
+{
+ testTriangleCapsuleContactCalculation("(Angled) Capsule axis just touching triangle",
+ Vector3d(1.0, 1.0, 10.0), Vector3d(0.0, 0.0, -0.0001), true, true, Vector3d::Zero(), Vector3d(0, 0, -0.5001));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase12)
+{
+ testTriangleCapsuleContactCalculation("Capsule axis through triangle",
+ Vector3d(0.0, 6.0, 1.0), Vector3d(0.0, -6.0, -1.0), true, false);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase13)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 1) Capsule far away from triangle",
+ Vector3d(4.0, 0.0, 5.0), Vector3d(2.4, 0.0, 2.0), false);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase14)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 1) Capsule just away triangle",
+ Vector3d(4.0, 0.0, 5.0), Vector3d(2.4, 0.0, 0.500001), false);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase15)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 1) Capsule just touching triangle",
+ Vector3d(4.0, 0.0, 5.0), Vector3d(2.4, 0.0, 0.49999), true,
+ true, Vector3d(2.4, 0.0, 0.0), Vector3d(2.4, 0.0,-0.00001));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase16)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 1) Capsule axis just away from triangle",
+ Vector3d(4.0, 0.0, 5.0), Vector3d(2.4, 0.0, 0.0001), true,
+ true, Vector3d(2.4, 0.0, 0.0), Vector3d(2.4, 0.0, -0.4999));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase17)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 1) Capsule axis just touching triangle",
+ Vector3d(4.0, 0.0, 5.0), Vector3d(2.4, 0.0, -0.0001), true,
+ true, Vector3d(2.4, 0.0, 0.0), Vector3d(2.4, 0.0, -0.5001));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase18)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 1) Capsule axis inside triangle",
+ Vector3d(4.0, 0.0, 5.0), Vector3d(2.4, 0.0, -0.5), true);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase19)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 2) Capsule far away from triangle",
+ Vector3d(-4.0, 0.0, 5.0), Vector3d(-2.4, 0.0, 2.0), false);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase20)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 2) Capsule just away triangle",
+ Vector3d(-4.0, 0.0, 5.0), Vector3d(-2.4, 0.0, 0.500001), false);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase21)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 2) Capsule just touching triangle",
+ Vector3d(-4.0, 0.0, 5.0), Vector3d(-2.4, 0.0, 0.49999), true,
+ true, Vector3d(-2.4, 0.0, 0.0), Vector3d(-2.4, 0.0, -0.00001));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase22)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 2) Capsule axis just away from triangle",
+ Vector3d(-4.0, 0.0, 5.0), Vector3d(-2.4, 0.0, 0.0001), true,
+ true, Vector3d(-2.4, 0.0, 0.0), Vector3d(-2.4, 0.0, -0.4999));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase23)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 2) Capsule axis just touching triangle",
+ Vector3d(-4.0, 0.0, 5.0), Vector3d(-2.4, 0.0, -0.0001), true,
+ true, Vector3d(-2.4, 0.0, 0.0), Vector3d(-2.4, 0.0, -0.5001));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase24)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 2) Capsule axis inside triangle",
+ Vector3d(-4.0, 0.0, 5.0), Vector3d(-2.4, 0.0, -0.5), true);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase25)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 3) Capsule far away from triangle",
+ Vector3d(0.0, -6.0, 5.0), Vector3d(0.0, -4.8, 2.0), false);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase26)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 3) Capsule just away triangle",
+ Vector3d(0.0, -6.0, 5.0), Vector3d(0.0, -4.8, 0.500001), false);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase27)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 3) Capsule just touching triangle",
+ Vector3d(0.0, -6.0, 5.0), Vector3d(0.0, -4.8, 0.49999), true,
+ true, Vector3d(0,-4.8,0), Vector3d(0,-4.8,-0.00001));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase28)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 3) Capsule axis just away from triangle",
+ Vector3d(0.0, -6.0, 5.0), Vector3d(0.0, -4.8, 0.0001), true,
+ true, Vector3d(0.0, -4.8, 0.0), Vector3d(0.0, -4.8, -0.4999));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase29)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 3) Capsule axis just touching triangle",
+ Vector3d(0.0, -6.0, 5.0), Vector3d(0.0, -4.8, -0.0001), true,
+ true, Vector3d(0.0 ,-4.8, 0.0), Vector3d(0.0, -4.8, -0.5001));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase30)
+{
+ testTriangleCapsuleContactCalculation("(Angled, near edge 3) Capsule axis inside triangle",
+ Vector3d(0.0, -6.0, 5.0), Vector3d(0.0, -4.8, -0.5), true);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase31)
+{
+ testTriangleCapsuleContactCalculation("(Angled, at center) Capsule axis inside triangle",
+ Vector3d(0.0, 0.0, -0.01), Vector3d(0.0, -0.1, 3.0), true);
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase32)
+{
+ MockTriangle t(Vector3d(0.0085640543450962274, -0.17216352338611343, 0.032793871473806267),
+ Vector3d(0.17462358883922274, -0.16815838417660384, 0.13533384863957792),
+ Vector3d(0.16119856859908893, -0.21686057517329485, 0.072690609592390418));
+ MockCapsule c(Vector3d(0.18735007841383616, -0.28690200129741239, 0.11717293620266563),
+ Vector3d(0.10383485572751404, -0.17364876935318688, 0.046339120022104330), 1e-5);
+
+ testTriangleCapsuleContactCalculation(
+ TriangleCapsuleTestCase("Failing test 1", t, c, true, false, Vector3d(), Vector3d()));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase33)
+{
+ MockTriangle t(Vector3d(5, -5, 0), Vector3d(0, 5, 0), Vector3d(-5, -5, 0));
+ MockCapsule c(Vector3d(0, -4, -1), Vector3d(0, -5 + 1e-9, 0), 1e-9);
+
+ testTriangleCapsuleContactCalculation(
+ TriangleCapsuleTestCase("Failing test 2", t, c, true, false, Vector3d(), Vector3d()));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase34)
+{
+ MockTriangle t(Vector3d(0005.55605,0087.7003,-0047.2398), Vector3d(0000.110703,0113.919,-0039.387),
+ Vector3d(-0005.57197,0087.8735,-0047.1883));
+ MockCapsule c(Vector3d(0,0115.5,-0045.), Vector3d(0,0110.,-0040.), 0000.1);
+
+ testTriangleCapsuleContactCalculation(
+ TriangleCapsuleTestCase("Failing test 3", t, c, true, false, Vector3d(), Vector3d()));
+}
+
+TEST_F(TriangleCapsuleContactCalculationTest, TestCase35)
+{
+ MockTriangle t(Vector3d(0.013092,0.108735,-0.0290345), Vector3d(0.0077754,0.135282,-0.0222713),
+ Vector3d(0.00196512,0.108977,-0.0290394));
+ MockCapsule c(Vector3d(0.00207912,0.109781,-0.0298305), Vector3d(0.00309017,0.109511,-0.0247458), 0.0001);
+
+ testTriangleCapsuleContactCalculation(
+ TriangleCapsuleTestCase("Failing test 4", t, c, true, false, Vector3d(), Vector3d()));
+}
+
+}
+}
diff --git a/SurgSim/Math/UnitTests/TriangleTriangleSeparatingAxisContactCalculationTests.cpp b/SurgSim/Math/UnitTests/TriangleTriangleSeparatingAxisContactCalculationTests.cpp
new file mode 100644
index 0000000..e7a1e69
--- /dev/null
+++ b/SurgSim/Math/UnitTests/TriangleTriangleSeparatingAxisContactCalculationTests.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.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/UnitTests/TriangleTriangleTestParameters.h"
+
+namespace SurgSim
+{
+namespace Math
+{
+
+class TriangleTriangleSeparatingAxisContactCalculationTest :
+ 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);
+
+ 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 twice"
+ };
+ for (int count = 0; count < 6; ++count)
+ {
+ SCOPED_TRACE(traceMessage[count]);
+
+ switch (count)
+ {
+ case 0:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleTriangleSeparatingAxis(t0.v0, t0.v1, t0.v2,
+ t1.v0, t1.v1, t1.v2, &penetrationDepth, &t0Point, &t1Point, &normal););
+ break;
+ case 1:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleTriangleSeparatingAxis(t0.v1, t0.v2, t0.v0,
+ t1.v0, t1.v1, t1.v2, &penetrationDepth, &t0Point, &t1Point, &normal););
+ break;
+ case 2:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleTriangleSeparatingAxis(t0.v2, t0.v0, t0.v1,
+ t1.v0, t1.v1, t1.v2, &penetrationDepth, &t0Point, &t1Point, &normal););
+ break;
+ case 3:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleTriangleSeparatingAxis(t1.v0, t1.v1, t1.v2,
+ t0.v0, t0.v1, t0.v2, &penetrationDepth, &t1Point, &t0Point, &normal););
+ break;
+ case 4:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleTriangleSeparatingAxis(t1.v1, t1.v2, t1.v0,
+ t0.v0, t0.v1, t0.v2, &penetrationDepth, &t1Point, &t0Point, &normal););
+ break;
+ case 5:
+ EXPECT_NO_THROW(
+ contactFound = calculateContactTriangleTriangleSeparatingAxis(t1.v2, t1.v0, t1.v1,
+ t0.v0, t0.v1, t0.v2, &penetrationDepth, &t1Point, &t0Point, &normal););
+ break;
+ }
+
+ EXPECT_EQ(contactExpected, contactFound);
+ if (contactFound)
+ {
+ // 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(TriangleTriangleSeparatingAxisContactCalculationTest, TestCases)
+{
+ for (auto it = m_testCases.begin(); it != m_testCases.end(); ++it)
+ {
+ testTriangleTriangleContactCalculation(*it);
+ }
+}
+
+}
+}
diff --git a/SurgSim/Math/UnitTests/TriangleTriangleTestParameters.h b/SurgSim/Math/UnitTests/TriangleTriangleTestParameters.h
index 3ddb889..caf0e02 100644
--- a/SurgSim/Math/UnitTests/TriangleTriangleTestParameters.h
+++ b/SurgSim/Math/UnitTests/TriangleTriangleTestParameters.h
@@ -233,6 +233,18 @@ protected:
Vector3d t1p;
m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, true, false, t0p, t1p));
}
+ {
+ std::string scenario = "Failed case in Vascular";
+ MockTriangle t0(Vector3d(0.12419200000000000, -0.0027260000000000001, -0.039763000000000000),
+ Vector3d(0.10276100000000001, -0.0020000000000000000, -0.034617000000000002),
+ Vector3d(0.10060200000000000, -0.0026749999999999999, -0.036646999999999999));
+ Vector3d t0p;
+ MockTriangle t1(Vector3d(0.13072899974405072, -0.0023620000000000000, -0.033049000000000002),
+ Vector3d(0.12155499974405069, -0.0017220000000000000, -0.039350000000000003),
+ Vector3d(0.12419199974405069, -0.0027260000000000001, -0.039763000000000000));
+ Vector3d t1p;
+ m_testCases.push_back(TriangleTriangleTestCase(scenario, t0, t1, false, false, t0p, t1p));
+ }
}
};
diff --git a/SurgSim/Math/UnitTests/VectorTests.cpp b/SurgSim/Math/UnitTests/VectorTests.cpp
index 17cb45d..6d5bc27 100644
--- a/SurgSim/Math/UnitTests/VectorTests.cpp
+++ b/SurgSim/Math/UnitTests/VectorTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2012-2013, SimQuest Solutions Inc.
+// Copyright 2012-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -17,12 +17,12 @@
/// Tests that exercise the functionality of our vector typedefs, which come
/// straight from Eigen.
+#include <gtest/gtest.h>
+#include <math.h>
#include <vector>
-#include <math.h>
-#include "SurgSim/Math/Vector.h"
#include "SurgSim/Math/MathConvert.h"
-#include "gtest/gtest.h"
+#include "SurgSim/Math/Vector.h"
// Define test fixture class templates.
// We don't really need fixtures as such, but the templatization encodes type.
@@ -45,7 +45,7 @@ public:
// 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;
+ SurgSim::Math::Vector2f> Vector2Variants;
TYPED_TEST_CASE(Vector2Tests, Vector2Variants);
@@ -58,7 +58,7 @@ public:
// 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;
+ SurgSim::Math::Vector3f> Vector3Variants;
TYPED_TEST_CASE(Vector3Tests, Vector3Variants);
@@ -71,7 +71,7 @@ public:
// 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;
+ SurgSim::Math::Vector4f> Vector4Variants;
TYPED_TEST_CASE(Vector4Tests, Vector4Variants);
@@ -84,7 +84,7 @@ public:
// 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;
+ SurgSim::Math::Vector6f> Vector6Variants;
TYPED_TEST_CASE(Vector6Tests, Vector6Variants);
@@ -104,18 +104,18 @@ public:
// 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;
+ 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;
+ Eigen::VectorXf,
+ SurgSim::Math::Vector> AllDynamicVectorVariants;
TYPED_TEST_CASE(AllDynamicVectorTests, AllDynamicVectorVariants);
template <class T>
@@ -133,18 +133,18 @@ public:
};
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;
+ 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;
+ Eigen::VectorXf,
+ SurgSim::Math::Vector> UnalignedDynamicVectorVariants;
TYPED_TEST_CASE(UnalignedDynamicVectorTests, UnalignedDynamicVectorVariants);
@@ -284,7 +284,7 @@ TYPED_TEST(Vector2Tests, ShiftCommaInitialization)
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;
+ EXPECT_NEAR(2.3, vector.sum(), 5e-6) << "initialization was incorrect: " << vector;
}
/// Test setting the vector using the << syntax.
@@ -296,7 +296,7 @@ TYPED_TEST(Vector3Tests, ShiftCommaInitialization)
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;
+ EXPECT_NEAR(3.6, vector.sum(), 5e-6) << "initialization was incorrect: " << vector;
}
/// Test setting the vector using the << syntax.
@@ -308,7 +308,7 @@ TYPED_TEST(Vector4Tests, ShiftCommaInitialization)
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;
+ EXPECT_NEAR(5.0, vector.sum(), 5e-6) << "initialization was incorrect: " << vector;
}
/// Test setting the vector using the << syntax.
@@ -320,8 +320,8 @@ TYPED_TEST(Vector6Tests, ShiftCommaInitialization)
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;
+ static_cast<T>(1.5), static_cast<T>(1.6);
+ EXPECT_NEAR(8.1, vector.sum(), 5e-6) << "initialization was incorrect: " << vector;
}
/// Test getting a zero value usable in expressions.
@@ -390,12 +390,13 @@ TYPED_TEST(AllVectorTests, SetFromArray)
// 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) };
+ 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.";
+ EXPECT_NEAR(static_cast<T>(0.1 + i * 1.1), vector[i], 1e-6) << i << " wasn't properly initialized.";
}
}
@@ -406,7 +407,8 @@ TYPED_TEST(AllVectorTests, YamlConvert)
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) };
+ static_cast<T>(4.0), static_cast<T>(4.3), static_cast<T>(4.6)
+ };
Vector original(testData);
YAML::Node node;
@@ -429,17 +431,19 @@ TYPED_TEST(AllVectorTests, Assign)
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) };
+ 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) };
+ 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);
+ 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);
+ 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);
@@ -455,13 +459,14 @@ TYPED_TEST(AllVectorTests, Negate)
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) };
+ 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);
+ double expectedSum = SIZE * (SIZE * 0.05 + 2.05);
Vector v(inputArray);
Vector n = -v;
- EXPECT_NEAR(static_cast<T>(-expectedSum), n.sum(), 1e-6);
+ EXPECT_NEAR(static_cast<T>(-expectedSum), n.sum(), 5e-6);
}
/// Addition.
@@ -472,14 +477,15 @@ TYPED_TEST(AllVectorTests, Add)
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) };
+ 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);
+ EXPECT_NEAR(static_cast<T>(2 * expectedSum) + SIZE, w.sum(), 5e-6);
}
/// Subtraction.
@@ -490,14 +496,15 @@ TYPED_TEST(AllVectorTests, Subtract)
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) };
+ 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);
+ 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);
+ EXPECT_NEAR(static_cast<T>(expectedSum) - SIZE, w.sum(), 5e-6);
}
/// Incrementing by a value.
@@ -508,13 +515,14 @@ TYPED_TEST(AllVectorTests, AddTo)
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) };
+ 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);
+ 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);
+ EXPECT_NEAR(static_cast<T>(expectedSum) + SIZE, v.sum(), 5e-6);
}
/// Decrementing by a value.
@@ -525,13 +533,14 @@ TYPED_TEST(AllVectorTests, SubtractFrom)
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) };
+ 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);
+ 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);
+ EXPECT_NEAR(static_cast<T>(expectedSum) - SIZE, v.sum(), 5e-6);
}
/// Vector-scalar multiplication.
@@ -542,14 +551,15 @@ TYPED_TEST(AllVectorTests, MultiplyVectorScalar)
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) };
+ 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);
+ 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);
+ EXPECT_NEAR(static_cast<T>(1.23 * expectedSum), w.sum(), 5e-6);
}
/// Scalar-vector multiplication.
@@ -560,14 +570,15 @@ TYPED_TEST(AllVectorTests, MultiplyScalarVector)
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) };
+ 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);
+ 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);
+ EXPECT_NEAR(static_cast<T>(1.23 * expectedSum), w.sum(), 5e-6);
}
/// Division by scalar.
@@ -578,14 +589,15 @@ TYPED_TEST(AllVectorTests, DivideScalar)
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) };
+ 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);
+ 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);
+ EXPECT_NEAR(static_cast<T>(expectedSum / 1.23), w.sum(), 5e-6);
}
/// Component-wise multiplication.
@@ -596,17 +608,18 @@ TYPED_TEST(AllVectorTests, ComponentwiseMultiply)
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) };
+ 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);
+ 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);
+ EXPECT_NEAR(static_cast<T>(expectedSumSquares), w.sum(), 5e-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);
+ EXPECT_NEAR(static_cast<T>(expectedSumSquares), w.sum(), 5e-6);
}
/// Component-wise division.
@@ -617,16 +630,17 @@ TYPED_TEST(AllVectorTests, ComponentwiseDivide)
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) };
+ 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;
+ 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);
+ EXPECT_NEAR(static_cast<T>(2 * SIZE), w.sum(), 5e-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);
+ EXPECT_NEAR(static_cast<T>(2 * SIZE), w.sum(), 5e-6);
}
/// Dot product.
@@ -637,12 +651,13 @@ TYPED_TEST(AllVectorTests, DotProduct)
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) };
+ 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);
+ 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);
+ EXPECT_NEAR(static_cast<T>(expectedSumSquares), v.dot(v), 5e-6);
}
/// Cross product.
@@ -671,9 +686,10 @@ TYPED_TEST(AllVectorTests, OuterProduct)
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) };
+ 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);
+ double expectedSumSquares = SIZE * (SIZE * (SIZE * 0.03 + 0.885) + 8.695);
Vector v(inputArray);
// You have to write out the outer product, like this:
@@ -681,7 +697,7 @@ TYPED_TEST(AllVectorTests, OuterProduct)
// TODO(bert): maybe needs better testing here?
Vector u = v / v.squaredNorm();
- EXPECT_NEAR(static_cast<T>(expectedSumSquares), (m*u).squaredNorm(), 1e-3);
+ EXPECT_NEAR(static_cast<T>(expectedSumSquares), (m * u).squaredNorm(), 1e-3);
}
/// Euclidean norm and its square.
@@ -692,13 +708,14 @@ TYPED_TEST(AllVectorTests, NormAndSquared)
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) };
+ 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);
+ 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);
+ EXPECT_NEAR(static_cast<T>(expectedSumSquares), v.squaredNorm(), 5e-6);
+ EXPECT_NEAR(sqrt(static_cast<T>(expectedSumSquares)), v.norm(), 5e-6);
}
/// L1 (Manhattan) norm and L_Infinity (largest absolute value) norm.
@@ -709,19 +726,20 @@ TYPED_TEST(AllVectorTests, L1NormAndLInfNorm)
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) };
+ 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);
+ 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);
+ EXPECT_NEAR(static_cast<T>(expectedSum), v.template lpNorm<1>(), 5e-6);
+ EXPECT_NEAR(static_cast<T>(expectedSum), w.template lpNorm<1>(), 5e-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.
@@ -732,21 +750,22 @@ TYPED_TEST(AllVectorTests, Normalize)
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) };
+ 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);
+ double expectedSumSquares = SIZE * (SIZE * (SIZE * 0.03 + 0.885) + 8.695);
Vector v(inputArray);
- EXPECT_NEAR(sqrt(expectedSumSquares), v.norm(), 1e-6);
+ EXPECT_NEAR(sqrt(expectedSumSquares), v.norm(), 5e-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);
+ EXPECT_NEAR(static_cast<T>(1), u.norm(), 5e-6);
+ EXPECT_NEAR(sqrt(static_cast<T>(expectedSumSquares)), v.norm(), 5e-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);
+ EXPECT_NEAR(static_cast<T>(1), v.norm(), 5e-6);
+ EXPECT_NEAR(static_cast<T>(0), (u - v).norm(), 5e-6);
}
/// Minimum and maximum elements.
@@ -757,11 +776,12 @@ TYPED_TEST(AllVectorTests, MinAndMax)
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) };
+ 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);
+ EXPECT_NEAR(inputArray[SIZE - 1], v.maxCoeff(), 1e-6);
}
// ==================== SUBVECTORS (EXTENDING/SHRINKING) ====================
@@ -782,7 +802,7 @@ TYPED_TEST(Vector2Tests, Extend2to3)
// 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;
+ EXPECT_NEAR(2.3, vector3.sum(), 5e-6) << "extending was incorrect: " << vector3;
}
/// Extending vectors using the head(r) syntax.
@@ -798,7 +818,7 @@ TYPED_TEST(Vector2Tests, DynamicExtend2to3)
Vector3 vector3;
vector3.head(2) = vector2;
vector3[2] = static_cast<T>(0);
- EXPECT_NEAR(2.3, vector3.sum(), 1e-6) << "extending was incorrect: " << vector3;
+ EXPECT_NEAR(2.3, vector3.sum(), 5e-6) << "extending was incorrect: " << vector3;
}
/// Extending vectors using the block<r,c>() syntax.
@@ -817,7 +837,7 @@ TYPED_TEST(Vector2Tests, BlockExtend2to3)
// 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;
+ EXPECT_NEAR(2.3, vector3.sum(), 5e-6) << "extending was incorrect: " << vector3;
}
/// Extending vectors using the block(i,j,r,c) syntax.
@@ -833,7 +853,7 @@ TYPED_TEST(Vector2Tests, DynamicBlockExtend2to3)
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;
+ EXPECT_NEAR(2.3, vector3.sum(), 5e-6) << "extending was incorrect: " << vector3;
}
/// Shrinking vectors using the head<r>() syntax.
@@ -851,7 +871,7 @@ TYPED_TEST(Vector2Tests, Shrink3to2)
// 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;
+ EXPECT_NEAR(2.3, vector2.sum(), 5e-6) << "shrinking was incorrect: " << vector2;
}
/// Extending vectors using the head<r>() syntax.
@@ -870,7 +890,7 @@ TYPED_TEST(Vector3Tests, Extend2to3)
// 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;
+ EXPECT_NEAR(2.3, vector3.sum(), 5e-6) << "extending was incorrect: " << vector3;
}
/// Shrinking vectors using the head<r>() syntax.
@@ -888,7 +908,7 @@ TYPED_TEST(Vector3Tests, Shrink3to2)
// 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;
+ EXPECT_NEAR(2.3, vector2.sum(), 5e-6) << "shrinking was incorrect" << vector2;
}
/// Extending vectors using the head<r>() syntax.
@@ -907,7 +927,7 @@ TYPED_TEST(Vector3Tests, Extend3to4)
// 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;
+ EXPECT_NEAR(3.6, vector4.sum(), 5e-6) << "extending was incorrect" << vector4;
}
/// Shrinking vectors using the head<r>() syntax.
@@ -925,7 +945,7 @@ TYPED_TEST(Vector3Tests, Shrink4to3)
// 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;
+ EXPECT_NEAR(3.6, vector3.sum(), 5e-6) << "shrinking was incorrect" << vector3;
}
/// Extending vectors using the head<r>() syntax.
@@ -944,7 +964,7 @@ TYPED_TEST(Vector4Tests, Extend3to4)
// 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;
+ EXPECT_NEAR(3.6, vector4.sum(), 5e-6) << "extending was incorrect" << vector4;
}
/// Shrinking vectors using the head<r>() syntax.
@@ -962,7 +982,7 @@ TYPED_TEST(Vector4Tests, Shrink4to3)
// 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;
+ EXPECT_NEAR(3.6, vector3.sum(), 5e-6) << "shrinking was incorrect" << vector3;
}
/// Extend Euclidean N-vector [a_i] to homogeneous (N+1)-vector [a_i 1].
@@ -971,10 +991,11 @@ 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;
+ 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) };
+ static_cast<T>(10.4), static_cast<T>(10.5), static_cast<T>(10.6)
+ };
Vector v(inputArray);
HVector h = v.homogeneous();
@@ -997,14 +1018,15 @@ TYPED_TEST(AllVectorTests, HomogeneousShrink)
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) };
+ 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.";
+ EXPECT_NEAR(inputArray[i] / 2, v[i], 1e-6) << "Euclidean form from homogeneous garbled.";
}
}
@@ -1020,9 +1042,10 @@ TYPED_TEST(AllVectorTests, TypeCasting)
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) };
+ 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));
+ 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
@@ -1044,7 +1067,8 @@ TYPED_TEST(AllVectorTests, ArrayReadWrite)
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) };
+ 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);
@@ -1067,9 +1091,11 @@ TYPED_TEST(AllVectorTests, Interpolate)
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) };
+ 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) };
+ static_cast<T>(5.1), static_cast<T>(8.9), static_cast<T>(1.5)
+ };
T interpArray[6];
Vector prev(prevArray);
@@ -1145,20 +1171,20 @@ TYPED_TEST(AllDynamicVectorTests, CanResize)
namespace
{
- template <class T>
- void testScalar(T valueExpected, T value){}
+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<double>(double valueExpected, double value)
+{
+ EXPECT_DOUBLE_EQ(valueExpected, value);
+}
- template <>
- void testScalar<float>(float valueExpected, float value)
- {
- EXPECT_FLOAT_EQ(valueExpected, value);
- }
+template <>
+void testScalar<float>(float valueExpected, float value)
+{
+ EXPECT_FLOAT_EQ(valueExpected, value);
+}
};
TYPED_TEST(AllDynamicVectorTests, addSubVector)
@@ -1166,10 +1192,14 @@ 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););
+ 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++)
@@ -1178,7 +1208,7 @@ TYPED_TEST(AllDynamicVectorTests, addSubVector)
}
for (int dofId = 6; dofId < 9; dofId++)
{
- testScalar(vInit[dofId] + v2Init[3 + dofId-6], v[dofId]);
+ testScalar(vInit[dofId] + v2Init[3 + dofId - 6], v[dofId]);
}
for (int dofId = 9; dofId < 18; dofId++)
{
@@ -1192,13 +1222,17 @@ TYPED_TEST(AllDynamicVectorTests, addSubVectorBlocks)
Vector v, vInit, v2, v2Init;
std::vector<size_t> nodeIds;
- v.resize(18); v.setRandom(); vInit = v;
- v2.resize(18); v2.setRandom(); v2Init = v2;
+ 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););
+ 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++)
@@ -1232,10 +1266,14 @@ 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););
+ 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++)
@@ -1244,7 +1282,7 @@ TYPED_TEST(AllDynamicVectorTests, setSubVector)
}
for (int dofId = 6; dofId < 9; dofId++)
{
- testScalar(v2Init[3 + dofId-6], v[dofId]);
+ testScalar(v2Init[3 + dofId - 6], v[dofId]);
}
for (int dofId = 9; dofId < 18; dofId++)
{
@@ -1257,7 +1295,9 @@ TYPED_TEST(AllDynamicVectorTests, getSubVector)
typedef typename TestFixture::Vector Vector;
Vector v, vInit;
- v.resize(18); v.setRandom(); vInit = v;
+ v.resize(18);
+ v.setRandom();
+ vInit = v;
Eigen::VectorBlock<Vector> subVector = SurgSim::Math::getSubVector(v, 2, 3);
EXPECT_TRUE(v.isApprox(vInit));
@@ -1275,8 +1315,11 @@ TYPED_TEST(AllDynamicVectorTests, getSubVectorBlocks)
Vector v, vInit, v2;
std::vector<size_t> nodeIds;
- v.resize(18); v.setRandom(); vInit = v;
- v2.resize(9); v2.setZero();
+ v.resize(18);
+ v.setRandom();
+ vInit = v;
+ v2.resize(9);
+ v2.setZero();
nodeIds.push_back(1);
nodeIds.push_back(3);
nodeIds.push_back(5);
@@ -1377,3 +1420,38 @@ TYPED_TEST(Vector3Tests, buildOrthonormalBasis)
testOrthonormalBasis(i, j, k);
}
}
+
+TYPED_TEST(Vector3Tests, robustCrossProduct)
+{
+ typedef typename TestFixture::Vector3 Vector3;
+ typedef typename Vector3::Scalar T;
+ T epsilon = static_cast<T>(1.0e02 * Eigen::NumTraits<T>::dummy_precision());
+
+ Vector3 p0(static_cast<T>(0.0), static_cast<T>(0.0), static_cast<T>(0.2));
+ Vector3 p1(static_cast<T>(0.0), static_cast<T>(0.1), static_cast<T>(0.2));
+
+ std::array<Vector3, 2> p;
+ p[0] = p0;
+ p[1] = p1;
+
+ {
+ SCOPED_TRACE("Safe cross product, p1q0 works.");
+ Vector3 q0(static_cast<T>(0.1), static_cast<T>(0.0), static_cast<T>(0.2));
+ Vector3 q1(static_cast<T>(0.0), static_cast<T>(0.2), static_cast<T>(0.2));
+ std::array<Vector3, 2> q;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_TRUE(SurgSim::Math::robustCrossProduct(p, q, epsilon).isApprox(Vector3(static_cast<T>(0.0),
+ static_cast<T>(0.0), static_cast<T>(-1.0)), epsilon));
+ }
+ {
+ SCOPED_TRACE("Safe cross product, p1q0 doesn't work, but p1q1 works.");
+ Vector3 q0(static_cast<T>(0.0), static_cast<T>(0.2), static_cast<T>(0.2));
+ Vector3 q1(static_cast<T>(0.1), static_cast<T>(0.0), static_cast<T>(0.2));
+ std::array<Vector3, 2> q;
+ q[0] = q0;
+ q[1] = q1;
+ EXPECT_TRUE(SurgSim::Math::robustCrossProduct(p, q, epsilon).isApprox(Vector3(static_cast<T>(0.0),
+ static_cast<T>(0.0), static_cast<T>(-1.0)), epsilon));
+ }
+}
diff --git a/SurgSim/Math/UnitTests/config.txt.in b/SurgSim/Math/UnitTests/config.txt.in
index 9c1cf05..104d701 100644
--- a/SurgSim/Math/UnitTests/config.txt.in
+++ b/SurgSim/Math/UnitTests/config.txt.in
@@ -1 +1,3 @@
-${CMAKE_CURRENT_BINARY_DIR}/Data/
\ No newline at end of file
+${CMAKE_CURRENT_BINARY_DIR}/MlcpTestData/
+${CMAKE_CURRENT_BINARY_DIR}/Data/
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data/
diff --git a/SurgSim/Math/Vector.h b/SurgSim/Math/Vector.h
index 68a919e..4b56be4 100644
--- a/SurgSim/Math/Vector.h
+++ b/SurgSim/Math/Vector.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2012-2013, SimQuest Solutions Inc.
+// Copyright 2012-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -19,6 +19,7 @@
#ifndef SURGSIM_MATH_VECTOR_H
#define SURGSIM_MATH_VECTOR_H
+#include <array>
#include <vector>
#include <Eigen/Core>
@@ -159,8 +160,8 @@ void getSubVector(const Vector& vector, const std::vector<size_t> blockIds, size
/// \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,
+ const Eigen::Matrix<T, size, 1, TOpt>& previous,
+ const Eigen::Matrix<T, size, 1, TOpt>& next,
T t)
{
return previous + t * (next - previous);
@@ -193,6 +194,40 @@ bool buildOrthonormalBasis(Eigen::Matrix<T, 3, 1, VOpt>* i,
return true;
}
+
+/// Calculate the best unit normal we can find in the direction of pXq for one of the endpoints of q.
+/// Try multiple arrangements of the end points to reduce the artifacts when three of the vertices may
+/// be nearly collinear.
+/// \param p segment p
+/// \param q segment q
+/// \param epsilon when the norm of p x q is above epsilon, the cross product is assumed to be valid.
+/// return the normalized cross product of p x q
+template <class T, int VOpt>
+Eigen::Matrix<T, 3, 1, VOpt> robustCrossProduct(const std::array<Eigen::Matrix<T, 3, 1, VOpt>, 2>& p,
+ const std::array<Eigen::Matrix<T, 3, 1, VOpt>, 2>& q,
+ T epsilon)
+{
+
+ auto p0p1 = p[1] - p[0];
+ auto p1q0 = q[0] - p[1];
+ auto p0q0 = q[0] - p[0];
+ auto p1q1 = q[1] - p[1];
+ auto pXq = p0p1.cross(p1q0);
+ auto norm = pXq.norm();
+ if (norm < epsilon)
+ {
+ pXq = p0p1.cross(p0q0);
+ norm = pXq.norm();
+ }
+ if (norm < epsilon)
+ {
+ pXq = p0p1.cross(p1q1);
+ norm = pXq.norm();
+ }
+ pXq *= static_cast<T>(1.0 / norm);
+ return pXq;
+}
+
}; // namespace Math
}; // namespace SurgSim
diff --git a/SurgSim/Particles/CMakeLists.txt b/SurgSim/Particles/CMakeLists.txt
new file mode 100644
index 0000000..010b33f
--- /dev/null
+++ b/SurgSim/Particles/CMakeLists.txt
@@ -0,0 +1,61 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2015, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT 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_PARTICLES_SOURCES
+ DefaultPointGenerator.cpp
+ Emitter.cpp
+ ParticlesCollisionRepresentation.cpp
+ PointGenerator.cpp
+ RandomBoxPointGenerator.cpp
+ RandomMeshPointGenerator.cpp
+ RandomPointGenerator.cpp
+ RandomSpherePointGenerator.cpp
+ Representation.cpp
+ Sink.cpp
+ SphRepresentation.cpp
+)
+
+set(SURGSIM_PARTICLES_HEADERS
+ DefaultPointGenerator.h
+ Emitter.h
+ Particles.h
+ ParticlesCollisionRepresentation.h
+ PointGenerator.h
+ RandomBoxPointGenerator.h
+ RandomMeshPointGenerator.h
+ RandomPointGenerator.h
+ RandomSpherePointGenerator.h
+ Representation.h
+ Sink.h
+ SphRepresentation.h
+)
+
+surgsim_add_library(
+ SurgSimParticles
+ "${SURGSIM_PARTICLES_SOURCES}"
+ "${SURGSIM_PARTICLES_HEADERS}"
+)
+
+SET(LIBS SurgSimMath)
+target_link_libraries(SurgSimParticles ${LIBS})
+
+if(BUILD_TESTING)
+ add_subdirectory(UnitTests)
+ if(BUILD_RENDER_TESTING)
+ add_subdirectory(RenderTests)
+ endif()
+endif()
+
+set_target_properties(SurgSimParticles PROPERTIES FOLDER "Particles")
diff --git a/SurgSim/Particles/DefaultPointGenerator.cpp b/SurgSim/Particles/DefaultPointGenerator.cpp
new file mode 100644
index 0000000..05d135b
--- /dev/null
+++ b/SurgSim/Particles/DefaultPointGenerator.cpp
@@ -0,0 +1,48 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Particles/DefaultPointGenerator.h"
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/Shape.h"
+
+namespace SurgSim
+{
+namespace Particles
+{
+using SurgSim::Math::Vector3d;
+
+DefaultPointGenerator::~DefaultPointGenerator()
+{
+}
+
+Vector3d DefaultPointGenerator::pointInShape(std::shared_ptr<SurgSim::Math::Shape> shape)
+{
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "PointGenerator does not support generating points in shape type: "<< shape->getType();
+
+ return Vector3d::Zero();
+}
+
+Vector3d DefaultPointGenerator::pointOnShape(std::shared_ptr<SurgSim::Math::Shape> shape)
+{
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "PointGenerator does not support generating points on the surface of shape type: "<< shape->getType();
+
+ return Vector3d::Zero();
+}
+
+}; // namespace Particles
+}; // namespace SurgSim
diff --git a/SurgSim/Particles/DefaultPointGenerator.h b/SurgSim/Particles/DefaultPointGenerator.h
new file mode 100644
index 0000000..a3160c5
--- /dev/null
+++ b/SurgSim/Particles/DefaultPointGenerator.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_PARTICLES_DEFAULTPOINTGENERATOR_H
+#define SURGSIM_PARTICLES_DEFAULTPOINTGENERATOR_H
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/PointGenerator.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+class Shape;
+}
+
+namespace Particles
+{
+
+/// DefaultPointGenerator, methods of this class will always return (0.0, 0.0, 0.0) and output a severe logging message.
+/// They are served more like a place holder (concrete implementation) to be used in ShapesPointGenerator.
+/// One should develop an actual XXXPointGenerator to produce points in/on the shape.
+class DefaultPointGenerator: public PointGenerator
+{
+public:
+ /// Destructor
+ virtual ~DefaultPointGenerator();
+
+ SurgSim::Math::Vector3d pointInShape(std::shared_ptr<SurgSim::Math::Shape> shape) override;
+ SurgSim::Math::Vector3d pointOnShape(std::shared_ptr<SurgSim::Math::Shape> shape) override;
+};
+
+}; // namespace Particles
+}; // namespace SurgSim
+
+#endif // SURGSIM_PARTICLES_DEFAULTPOINTGENERATOR_H
diff --git a/SurgSim/Particles/Emitter.cpp b/SurgSim/Particles/Emitter.cpp
new file mode 100644
index 0000000..20d8353
--- /dev/null
+++ b/SurgSim/Particles/Emitter.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.
+
+#include "SurgSim/Particles/Emitter.h"
+
+#include <utility>
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Particles/Representation.h"
+
+using SurgSim::Math::Vector3d;
+
+
+namespace SurgSim
+{
+
+namespace Particles
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Particles::Emitter, Emitter);
+
+Emitter::Emitter(const std::string& name) :
+ SurgSim::Framework::Behavior(name),
+ m_mode(EMIT_MODE_VOLUME),
+ m_rate(0.0),
+ m_lifetimeRange(std::make_pair(0.0, 0.0)),
+ m_velocityRange(std::make_pair(Vector3d::Zero(), Vector3d::Zero())),
+ m_particlesNotAdded(0.0),
+ m_localPose(SurgSim::Math::RigidTransform3d::Identity()),
+ m_logger(SurgSim::Framework::Logger::getLogger("Particles"))
+{
+ typedef std::pair<double, double> LifetimeRangeType;
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Emitter, LifetimeRangeType, LifetimeRange, getLifetimeRange, setLifetimeRange);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Emitter, int, Mode, getMode, setMode);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Emitter, double, Rate, getRate, setRate);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Emitter, std::shared_ptr<SurgSim::Math::Shape>, Shape, getShape, setShape);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Emitter, std::shared_ptr<SurgSim::Framework::Component>,
+ Target, getTarget, setTarget);
+ typedef std::pair<Vector3d, Vector3d> VelocityRangeType;
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Emitter, VelocityRangeType, VelocityRange, getVelocityRange, setVelocityRange);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Emitter, SurgSim::Math::RigidTransform3d, LocalPose, getLocalPose,
+ setLocalPose);
+
+ SURGSIM_ADD_RO_PROPERTY(Emitter, SurgSim::Math::RigidTransform3d, Pose, getPose);
+
+ std::random_device device;
+ m_generator.seed(device());
+ m_zeroOneDistribution.param(std::uniform_real_distribution<double>::param_type(0.0, 1.0));
+}
+
+Emitter::~Emitter()
+{
+}
+
+bool Emitter::doInitialize()
+{
+ return true;
+}
+
+bool Emitter::doWakeUp()
+{
+ if (m_target == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Emitters need a Representation to emit to.";
+ return false;
+ }
+ if (m_shape == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Emitters need a shape.";
+ return false;
+ }
+ return true;
+}
+
+void Emitter::update(double dt)
+{
+ Vector3d position, velocity;
+ double lifetime;
+ double particlesToAdd = m_rate * dt + m_particlesNotAdded;
+ size_t particlesAdded = 0;
+ for ( ; particlesAdded < std::floor(particlesToAdd); particlesAdded++)
+ {
+ if (m_mode == EMIT_MODE_VOLUME)
+ {
+ position = getPose() * m_pointGenerator.pointInShape(m_shape);
+ }
+ else
+ {
+ position = getPose() * m_pointGenerator.pointOnShape(m_shape);
+ }
+
+ velocity = Vector3d::NullaryExpr([this](int index){return m_zeroOneDistribution(m_generator);});
+ velocity = m_velocityRange.first + (m_velocityRange.second - m_velocityRange.first).cwiseProduct(velocity);
+
+ lifetime = m_zeroOneDistribution(m_generator);
+ lifetime = m_lifetimeRange.first + (m_lifetimeRange.second - m_lifetimeRange.first) * lifetime;
+
+ if (!m_target->addParticle(position, velocity, lifetime))
+ {
+ SURGSIM_LOG_DEBUG(m_logger) << "Unable to add particle to " << m_target->getName();
+ break;
+ }
+ }
+ m_particlesNotAdded = particlesToAdd - particlesAdded;
+}
+
+int Emitter::getTargetManagerType() const
+{
+ return SurgSim::Framework::MANAGER_TYPE_PHYSICS;
+}
+
+void Emitter::setShape(std::shared_ptr<SurgSim::Math::Shape> shape)
+{
+ m_shape = shape;
+}
+
+std::shared_ptr<SurgSim::Math::Shape> Emitter::getShape() const
+{
+ return m_shape;
+}
+
+void Emitter::setTarget(const std::shared_ptr<SurgSim::Framework::Component> target)
+{
+ m_target = SurgSim::Framework::checkAndConvert<SurgSim::Particles::Representation>(target,
+ "SurgSim::Particles::Representation");
+}
+
+const std::shared_ptr<SurgSim::Framework::Component> Emitter::getTarget()
+{
+ return m_target;
+}
+
+void Emitter::setMode(int mode)
+{
+ SURGSIM_ASSERT(0 <= mode && mode < EMIT_MODE_COUNT) << "Invalid emit mode";
+ m_mode = mode;
+}
+
+int Emitter::getMode() const
+{
+ return m_mode;
+}
+
+void Emitter::setRate(double rate)
+{
+ SURGSIM_ASSERT(rate >= 0.0) << "Emit rate must be non-negative";
+ m_rate = rate;
+}
+
+double Emitter::getRate() const
+{
+ return m_rate;
+}
+
+void Emitter::setLifetimeRange(const std::pair<double, double>& range)
+{
+ SURGSIM_ASSERT(0.0 < range.first && range.first <= range.second) <<
+ "Lower bound of lifetime must be greater than 0 and not greater than the upper bound of lifetime.";
+ m_lifetimeRange = range;
+}
+
+std::pair<double, double> Emitter::getLifetimeRange() const
+{
+ return m_lifetimeRange;
+}
+
+void Emitter::setVelocityRange(const std::pair<SurgSim::Math::Vector3d, SurgSim::Math::Vector3d>& range)
+{
+ SURGSIM_ASSERT((range.first.array() <= range.second.array()).all()) << "Minimum velocity must be less than maximum";
+ m_velocityRange = range;
+}
+
+const std::pair<SurgSim::Math::Vector3d, SurgSim::Math::Vector3d>& Emitter::getVelocityRange() const
+{
+ return m_velocityRange;
+}
+
+void Emitter::setLocalPose(const SurgSim::Math::RigidTransform3d& pose)
+{
+ m_localPose = pose;
+}
+
+SurgSim::Math::RigidTransform3d Emitter::getPose() const
+{
+ if (getSceneElement() != nullptr)
+ {
+ return getSceneElement()->getPose() * getLocalPose();
+ }
+ else
+ {
+ return getLocalPose();
+ }
+}
+
+SurgSim::Math::RigidTransform3d Emitter::getLocalPose() const
+{
+ return m_localPose;
+}
+
+
+}; // namespace Particles
+}; // namespace SurgSim
diff --git a/SurgSim/Particles/Emitter.h b/SurgSim/Particles/Emitter.h
new file mode 100644
index 0000000..25c6067
--- /dev/null
+++ b/SurgSim/Particles/Emitter.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_PARTICLES_EMITTER_H
+#define SURGSIM_PARTICLES_EMITTER_H
+
+#include <cmath>
+#include <memory>
+#include <random>
+
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/RandomPointGenerator.h"
+
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+class Logger;
+};
+
+namespace Math
+{
+class Shape;
+};
+
+namespace Particles
+{
+
+class Representation;
+
+/// Emitting modes of the Emitter
+enum EmitMode
+{
+ /// Emit particles from within the shapes volume
+ EMIT_MODE_VOLUME = 0,
+ /// Emit particles only from the surface
+ EMIT_MODE_SURFACE,
+ /// The number of EmitModes
+ EMIT_MODE_COUNT
+};
+
+SURGSIM_STATIC_REGISTRATION(Emitter);
+
+/// Emitter emits particles into a ParticleSystem
+class Emitter : public SurgSim::Framework::Behavior
+{
+public:
+ /// Constructor
+ /// \param name The Emitter's name
+ explicit Emitter(const std::string& name);
+
+ /// Destructor
+ virtual ~Emitter();
+
+ SURGSIM_CLASSNAME(SurgSim::Particles::Emitter);
+
+ void update(double dt) override;
+
+ int getTargetManagerType() const override;
+
+ /// Set the target to emit to.
+ /// \param target The ParticleSystem to emit to.
+ void setTarget(const std::shared_ptr<SurgSim::Framework::Component> target);
+
+ /// Get the target to emit to.
+ /// \return The ParticleSystem to emit to.
+ const std::shared_ptr<SurgSim::Framework::Component> getTarget();
+
+ /// Set the shape of this emitter.
+ /// \param shape Shape of this emitter.
+ void setShape(std::shared_ptr<SurgSim::Math::Shape> shape);
+
+ /// Get the shape of this emitter.
+ /// \return Shape of this emitter.
+ std::shared_ptr<SurgSim::Math::Shape> getShape() const;
+
+ /// Set the emit mode of this emitter.
+ /// \param mode The emit mode.
+ void setMode(int mode);
+
+ /// Get the emit mode of this emitter.
+ /// \return Emit mode of this emitter.
+ int getMode() const;
+
+ /// Set the emit rate of this emitter.
+ /// \param rate The rate of emitting [particles/s].
+ void setRate(double rate);
+
+ /// Get the emit rate of this emitter.
+ /// \return Emitting rate [particles/s].
+ double getRate() const;
+
+ /// Set the range of lifetimes of emitted particles.
+ /// Each emitted particle will have a randomly chosen lifetime in the supplied range.
+ /// \param range The shortest and longest lifetimes to produce.
+ void setLifetimeRange(const std::pair<double, double>& range);
+
+ /// Get the range of lifetimes of emitted particles.
+ /// \return The shortest and longest lifetimes.
+ std::pair<double, double> getLifetimeRange() const;
+
+ /// Set the range of velocities of the emitted particles.
+ /// Each produced particle will have a randomly chosen velocity in the supplied range.
+ /// \param range The minimum and maximum velocity.
+ void setVelocityRange(const std::pair<SurgSim::Math::Vector3d, SurgSim::Math::Vector3d>& range);
+
+ /// Get the range of velocities of the emitted particles.
+ const std::pair<SurgSim::Math::Vector3d, SurgSim::Math::Vector3d>& getVelocityRange() const;
+
+ /// Set the pose of the Emitter with respect to the Scene Element
+ /// \param pose The pose to set the Emitter to
+ virtual void setLocalPose(const SurgSim::Math::RigidTransform3d& pose);
+
+ /// Get the pose of the Emitter with respect to the Scene Element
+ /// \return The pose of this Emitter
+ virtual SurgSim::Math::RigidTransform3d getLocalPose() const;
+
+ /// Get the pose of the Emitter in world coordinates
+ /// \return The pose of this Emitter
+ virtual SurgSim::Math::RigidTransform3d getPose() const;
+
+private:
+ bool doInitialize() override;
+ bool doWakeUp() override;
+
+ /// PointGenerator for generating random points within or on the emitter shape.
+ RandomPointGenerator m_pointGenerator;
+
+ /// The emit mode of this emitter.
+ int m_mode;
+
+ /// The emit rate of this emitter.
+ double m_rate;
+
+ /// The range of lifetimes of emitted particles.
+ std::pair<double, double> m_lifetimeRange;
+
+ /// The range of velocities of the emitted particles.
+ std::pair<SurgSim::Math::Vector3d, SurgSim::Math::Vector3d> m_velocityRange;
+
+ /// Number of particles not added during last update.
+ double m_particlesNotAdded;
+
+ ///@{
+ /// Random number generator and distribution used to assign random lifetimes and velocities
+ std::mt19937 m_generator;
+ std::uniform_real_distribution<double> m_zeroOneDistribution;
+ ///@}
+
+ /// Shape of emitter.
+ std::shared_ptr<SurgSim::Math::Shape> m_shape;
+
+ /// Representation to emit to.
+ std::shared_ptr<SurgSim::Particles::Representation> m_target;
+
+ /// Local Pose of the Representation with respect to the SceneElement
+ SurgSim::Math::RigidTransform3d m_localPose;
+
+ /// Logger used by the Emitter
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+};
+
+}; // namespace Particles
+}; // namespace SurgSim
+
+#endif // SURGSIM_PARTICLES_EMITTER_H
diff --git a/SurgSim/Particles/Particles.h b/SurgSim/Particles/Particles.h
new file mode 100644
index 0000000..75758de
--- /dev/null
+++ b/SurgSim/Particles/Particles.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_PARTICLES_PARTICLES_H
+#define SURGSIM_PARTICLES_PARTICLES_H
+
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Math/Vector.h"
+
+
+namespace SurgSim
+{
+namespace Particles
+{
+
+struct ParticleData
+{
+ /// The lifetime of the particle [s]
+ double lifetime;
+
+ /// The velocity of the particle [m/s]
+ Math::Vector3d velocity;
+
+ /// Equality operator.
+ /// \param rhs The right hand side.
+ /// \return true if the parameters are considered equivalent.
+ bool operator==(const ParticleData& rhs) const
+ {
+ return lifetime == rhs.lifetime && velocity.isApprox(rhs.velocity);
+ }
+
+ /// Inequality operator.
+ /// \param rhs The right hand side.
+ /// \return true if the parameters are not considered equivalent.
+ bool operator!=(const ParticleData& rhs) const
+ {
+ return !((*this) == rhs);
+ }
+};
+
+typedef DataStructures::Vertices<ParticleData> Particles;
+typedef Particles::VertexType Particle;
+
+};
+};
+#endif //SURGSIM_PARTICLES_PARTICLES_H
+
+
diff --git a/SurgSim/Particles/ParticlesCollisionRepresentation.cpp b/SurgSim/Particles/ParticlesCollisionRepresentation.cpp
new file mode 100644
index 0000000..539b7fe
--- /dev/null
+++ b/SurgSim/Particles/ParticlesCollisionRepresentation.cpp
@@ -0,0 +1,109 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Particles/ParticlesCollisionRepresentation.h"
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/ParticlesShape.h"
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Particles/Representation.h"
+
+
+namespace SurgSim
+{
+namespace Particles
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Particles::ParticlesCollisionRepresentation,
+ ParticlesCollisionRepresentation);
+
+ParticlesCollisionRepresentation::ParticlesCollisionRepresentation(const std::string& name) :
+ SurgSim::Collision::Representation(name),
+ m_shape(std::make_shared<SurgSim::Math::ParticlesShape>())
+{
+ m_shape->setRadius(0.01);
+}
+
+ParticlesCollisionRepresentation::~ParticlesCollisionRepresentation()
+{
+}
+
+bool ParticlesCollisionRepresentation::doInitialize()
+{
+ return true;
+}
+
+bool ParticlesCollisionRepresentation::doWakeUp()
+{
+ auto particleRepresentation = m_particleRepresentation.lock();
+ if (particleRepresentation == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << getName()
+ << ": does not have a Particle Representation.";
+ return false;
+ }
+
+ m_shape->getVertices().reserve(particleRepresentation->getMaxParticles());
+
+ update(0.0);
+ return true;
+}
+
+int ParticlesCollisionRepresentation::getShapeType() const
+{
+ return m_shape->getType();
+}
+
+const std::shared_ptr<SurgSim::Math::Shape> ParticlesCollisionRepresentation::getShape() const
+{
+ return m_shape;
+}
+
+void ParticlesCollisionRepresentation::setParticleRepresentation(
+ std::shared_ptr<SurgSim::Particles::Representation> representation)
+{
+ m_particleRepresentation = representation;
+}
+
+const std::shared_ptr<SurgSim::Particles::Representation> ParticlesCollisionRepresentation::getParticleRepresentation()
+ const
+{
+ auto particleRepresentation = m_particleRepresentation.lock();
+ SURGSIM_ASSERT(particleRepresentation != nullptr) <<
+ "Failed to get the Particle Representation. The ParticlesCollisionRepresentation either was not "
+ "attached to a Particle Representation or the Particle Representation has expired.";
+
+ return particleRepresentation;
+}
+
+void ParticlesCollisionRepresentation::setParticleRadius(double radius)
+{
+ m_shape->setRadius(radius);
+}
+
+double ParticlesCollisionRepresentation::getParticleRadius() const
+{
+ return m_shape->getRadius();
+}
+
+void ParticlesCollisionRepresentation::updateShapeData()
+{
+ *m_shape = getParticleRepresentation()->getParticles().unsafeGet();
+ invalidatePosedShapeMotion();
+}
+
+};
+};
+
diff --git a/SurgSim/Particles/ParticlesCollisionRepresentation.h b/SurgSim/Particles/ParticlesCollisionRepresentation.h
new file mode 100644
index 0000000..b8f335a
--- /dev/null
+++ b/SurgSim/Particles/ParticlesCollisionRepresentation.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_PARTICLES_PARTICLESCOLLISIONREPRESENTATION_H
+#define SURGSIM_PARTICLES_PARTICLESCOLLISIONREPRESENTATION_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+
+
+namespace SurgSim
+{
+
+namespace Math
+{
+class Shape;
+class ParticlesShape;
+};
+
+namespace Particles
+{
+
+class Representation;
+
+SURGSIM_STATIC_REGISTRATION(ParticlesCollisionRepresentation);
+
+/// A Collision Representation that can be attached to a Particle Representation
+class ParticlesCollisionRepresentation : public SurgSim::Collision::Representation
+{
+public:
+
+ /// Constructor
+ /// \param name Name of the Representation
+ explicit ParticlesCollisionRepresentation(const std::string& name);
+
+ /// Destructor
+ virtual ~ParticlesCollisionRepresentation();
+
+ SURGSIM_CLASSNAME(SurgSim::Particles::ParticlesCollisionRepresentation);
+
+ const std::shared_ptr<SurgSim::Math::Shape> getShape() const override;
+
+ int getShapeType() const override;
+
+ void updateShapeData() override;
+
+ /// Sets the particle representation this collision representation is connected
+ /// \param representation The paticle representation
+ void setParticleRepresentation(std::shared_ptr<SurgSim::Particles::Representation> representation);
+
+ /// Gets the particle representation this collision representation is connected
+ /// \return The particle representation
+ const std::shared_ptr<SurgSim::Particles::Representation> getParticleRepresentation() const;
+
+ /// Set the particles' radius
+ /// \param radius the radius being set to all particles
+ void setParticleRadius(double radius);
+
+ /// Get the radius of the particles
+ /// \return The particles' radius
+ double getParticleRadius() const;
+
+private:
+ bool doInitialize() override;
+
+ bool doWakeUp() override;
+
+ /// Shape used for collision detection
+ std::shared_ptr<SurgSim::Math::ParticlesShape> m_shape;
+
+ /// Reference to the particle representation driving changes to this collision representation
+ std::weak_ptr<SurgSim::Particles::Representation> m_particleRepresentation;
+};
+
+};
+};
+
+#endif // SURGSIM_PARTICLES_PARTICLESCOLLISIONREPRESENTATION_H
+
diff --git a/SurgSim/Particles/PointGenerator.cpp b/SurgSim/Particles/PointGenerator.cpp
new file mode 100644
index 0000000..0eb4dcd
--- /dev/null
+++ b/SurgSim/Particles/PointGenerator.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/Particles/PointGenerator.h"
+
+#include <boost/math/special_functions/next.hpp> // For boost::math::float_next()
+
+namespace SurgSim
+{
+
+namespace Particles
+{
+PointGenerator::PointGenerator() :
+ m_openOneOneDistribution(boost::math::float_next(-1.0), 1.0),
+ m_closedOneOneDistribution(-1.0, boost::math::float_next(1.0)),
+ m_closedZeroOneDistribution(0.0, boost::math::float_next(1.0)),
+ m_closedZeroOpenOneDistribution(0.0, 1.0)
+{
+ std::random_device randomDevice;
+ m_generator.seed(randomDevice());
+}
+
+PointGenerator::~PointGenerator()
+{
+}
+
+}; // namespace Particles
+}; // namespace SurgSim
diff --git a/SurgSim/Particles/PointGenerator.h b/SurgSim/Particles/PointGenerator.h
new file mode 100644
index 0000000..4052180
--- /dev/null
+++ b/SurgSim/Particles/PointGenerator.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_PARTICLES_POINTGENERATOR_H
+#define SURGSIM_PARTICLES_POINTGENERATOR_H
+
+#include <memory>
+#include <random>
+
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+class Shape;
+}
+
+namespace Particles
+{
+
+/// PointGenerator is used to generate points inside or on the surface of a given shape.
+/// Derived classes need to implement pointInShape() and pointOnShape().
+class PointGenerator
+{
+public:
+ /// Constructor
+ PointGenerator();
+
+ /// Destructor
+ virtual ~PointGenerator();
+
+ /// Generates one point inside the given shape.
+ /// \param shape The shape inside which a point will be generated.
+ /// \return A point inside the shape, shape is assumed to be located at the origin.
+ virtual SurgSim::Math::Vector3d pointInShape(std::shared_ptr<SurgSim::Math::Shape> shape) = 0;
+
+ /// Generates one point on the surface of the given shape.
+ /// \param shape The shape on which a point will be generated.
+ /// \return A point on the surface of the shape, shape is assumed to be located at the origin.
+ virtual SurgSim::Math::Vector3d pointOnShape(std::shared_ptr<SurgSim::Math::Shape> shape) = 0;
+
+protected:
+ ///@{
+ /// Random number generator and some predefined distributions to be used by different shape point generators.
+ std::mt19937 m_generator;
+
+ std::uniform_real_distribution<double> m_openOneOneDistribution; // <-- (-1.0, 1.0)
+ std::uniform_real_distribution<double> m_closedOneOneDistribution; // <-- [-1.0, 1.0]
+ std::uniform_real_distribution<double> m_closedZeroOneDistribution; // <-- [0.0, 1.0]
+ std::uniform_real_distribution<double> m_closedZeroOpenOneDistribution; // <-- [0.0, 1.0)
+ ///@}
+};
+
+}; // namespace Particles
+}; // namespace SurgSim
+
+#endif // SURGSIM_PARTICLES_POINTGENERATOR_H
diff --git a/SurgSim/Particles/RandomBoxPointGenerator.cpp b/SurgSim/Particles/RandomBoxPointGenerator.cpp
new file mode 100644
index 0000000..39170d9
--- /dev/null
+++ b/SurgSim/Particles/RandomBoxPointGenerator.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/Particles/RandomBoxPointGenerator.h"
+
+#include "SurgSim/Math/BoxShape.h"
+
+namespace SurgSim
+{
+namespace Particles
+{
+using SurgSim::Math::Vector3d;
+
+RandomBoxPointGenerator::~RandomBoxPointGenerator()
+{
+}
+
+Vector3d RandomBoxPointGenerator::pointInShape(std::shared_ptr<SurgSim::Math::Shape> shape)
+{
+ auto box = std::static_pointer_cast<SurgSim::Math::BoxShape>(shape);
+ auto halfSize = box->getSize() * 0.5;
+
+ Vector3d random = Vector3d::NullaryExpr([&](int index){return m_openOneOneDistribution(m_generator);});
+
+ return random.array() * halfSize.array();
+}
+
+Vector3d RandomBoxPointGenerator::pointOnShape(std::shared_ptr<SurgSim::Math::Shape> shape)
+{
+ auto box = std::static_pointer_cast<SurgSim::Math::BoxShape>(shape);
+ Vector3d halfSize = box->getSize() * 0.5;
+
+ std::uniform_int_distribution<int> axisDirectionSelector(0, 2); // 0: X-Axis, 1: Y-Axis, 2: Z-Axis.
+ std::uniform_int_distribution<int> valueSelector(0, 1); // 0: negative size value, 1: positive size value.
+
+ Vector3d result;
+ // Choose one axis to be fixed.
+ int axis = axisDirectionSelector(m_generator);
+ result[axis] = valueSelector(m_generator) == 0 ? -halfSize[axis] : halfSize[axis];
+
+ // Then generate coordinates for the other two axes.
+ for (size_t t = 0; t < 2; ++t)
+ {
+ ++axis;
+ axis %= 3;
+ result[axis] = m_closedOneOneDistribution(m_generator) * halfSize[axis];
+ }
+
+ return result;
+}
+
+}; // namespace Particles
+}; // namespace SurgSim
diff --git a/SurgSim/Particles/RandomBoxPointGenerator.h b/SurgSim/Particles/RandomBoxPointGenerator.h
new file mode 100644
index 0000000..38a8274
--- /dev/null
+++ b/SurgSim/Particles/RandomBoxPointGenerator.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_PARTICLES_RANDOMBOXPOINTGENERATOR_H
+#define SURGSIM_PARTICLES_RANDOMBOXPOINTGENERATOR_H
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/PointGenerator.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+class Shape;
+}
+
+namespace Particles
+{
+
+/// Class to generate points inside or on the surface of a box.
+class RandomBoxPointGenerator: public PointGenerator
+{
+public:
+ /// Destructor
+ virtual ~RandomBoxPointGenerator();
+
+ SurgSim::Math::Vector3d pointInShape(std::shared_ptr<SurgSim::Math::Shape> shape) override;
+ SurgSim::Math::Vector3d pointOnShape(std::shared_ptr<SurgSim::Math::Shape> shape) override;
+};
+
+}; // namespace Particles
+}; // namespace SurgSim
+
+#endif // SURGSIM_PARTICLES_RANDOMBOXPOINTGENERATOR_H
diff --git a/SurgSim/Particles/RandomMeshPointGenerator.cpp b/SurgSim/Particles/RandomMeshPointGenerator.cpp
new file mode 100644
index 0000000..0168134
--- /dev/null
+++ b/SurgSim/Particles/RandomMeshPointGenerator.cpp
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Particles/RandomMeshPointGenerator.h"
+
+#include "SurgSim/Math/MeshShape.h"
+
+using SurgSim::Math::Vector2d;
+using SurgSim::Math::Vector3d;
+
+
+namespace SurgSim
+{
+namespace Particles
+{
+
+Math::Vector3d RandomMeshPointGenerator::pointInShape(std::shared_ptr<Math::Shape> shape)
+{
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "PointGenerator does not support generating points in shape: "<< shape->getType();
+
+ return Vector3d::Zero();
+}
+
+Math::Vector3d RandomMeshPointGenerator::pointOnShape(std::shared_ptr<Math::Shape> shape)
+{
+ Vector3d point;
+ auto mesh = std::static_pointer_cast<Math::MeshShape>(shape);
+
+ if (mesh->getNumTriangles() > 0)
+ {
+ auto& triangles = mesh->getTriangles();
+ std::uniform_int_distribution<int> triangleSelector(0, static_cast<int>(triangles.size()) - 1);
+
+ bool validTriangleFound = false;
+ size_t index;
+ while (!validTriangleFound)
+ {
+ index = triangleSelector(m_generator);
+ validTriangleFound = triangles[index].isValid;
+ }
+ auto vertices = mesh->getTrianglePositions(index);
+
+ // Find a random point on the triangle using algorithm developed by Osada et al.
+ // R. Osada, T. Funkhouser, B. Chazelle, D. Dobkin, "Shape Distributions",
+ // ACM Transactions on Graphics, vol. 21, no. 4, pp. 807–832, October 2002
+ Vector2d random = Vector2d::NullaryExpr([&](int index){return m_closedZeroOneDistribution(m_generator);});
+ random[0] = sqrt(random[0]);
+ point = (1 - random[0]) * vertices[0];
+ point += random[0] * (1 - random[1]) * vertices[1];
+ point += random[0] * random[1] * vertices[2];
+ }
+ else
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) <<
+ "Mesh does not contain any triangles, cannot generate point.";
+ point = Vector3d::Zero();
+ }
+ return point;
+}
+
+
+}; // namespace Particles
+}; // namespace SurgSim
+
diff --git a/SurgSim/Particles/RandomMeshPointGenerator.h b/SurgSim/Particles/RandomMeshPointGenerator.h
new file mode 100644
index 0000000..f6b67b7
--- /dev/null
+++ b/SurgSim/Particles/RandomMeshPointGenerator.h
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PARTICLES_RANDOMMESHPOINTGENERATOR_H
+#define SURGSIM_PARTICLES_RANDOMMESHPOINTGENERATOR_H
+
+#include "SurgSim/Particles/PointGenerator.h"
+
+
+namespace SurgSim
+{
+
+namespace Math
+{
+class Shape;
+}
+
+namespace Particles
+{
+
+/// Class to generate points on the surface of a mesh
+/// \note Each triangle in the mesh has equal weight in the distribution of
+/// points. As a result, areas with higher triangle density have a higher
+/// likely hood of generating points than areas with a lower triangle density.
+class RandomMeshPointGenerator: public PointGenerator
+{
+public:
+ Math::Vector3d pointInShape(std::shared_ptr<Math::Shape> shape) override;
+
+ Math::Vector3d pointOnShape(std::shared_ptr<Math::Shape> shape) override;
+};
+
+}; // namespace Particles
+}; // namespace SurgSim
+
+
+#endif // SURGSIM_PARTICLES_RANDOMMESHPOINTGENERATOR_H
diff --git a/SurgSim/Particles/RandomPointGenerator.cpp b/SurgSim/Particles/RandomPointGenerator.cpp
new file mode 100644
index 0000000..0691f75
--- /dev/null
+++ b/SurgSim/Particles/RandomPointGenerator.cpp
@@ -0,0 +1,65 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Particles/RandomPointGenerator.h"
+
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Particles/DefaultPointGenerator.h"
+#include "SurgSim/Particles/RandomBoxPointGenerator.h"
+#include "SurgSim/Particles/RandomMeshPointGenerator.h"
+#include "SurgSim/Particles/RandomSpherePointGenerator.h"
+
+
+namespace SurgSim
+{
+namespace Particles
+{
+
+RandomPointGenerator::RandomPointGenerator()
+{
+ for (size_t index = 0; index < static_cast<size_t>(Math::SHAPE_TYPE_COUNT); ++index)
+ {
+ m_pointGenerators[index].reset(new DefaultPointGenerator());
+ }
+
+ m_pointGenerators[Math::SHAPE_TYPE_BOX].reset(new RandomBoxPointGenerator());
+ m_pointGenerators[Math::SHAPE_TYPE_MESH].reset(new RandomMeshPointGenerator());
+ m_pointGenerators[Math::SHAPE_TYPE_SPHERE].reset(new RandomSpherePointGenerator());
+}
+
+Math::Vector3d RandomPointGenerator::pointInShape(std::shared_ptr<Math::Shape> shape)
+{
+ SURGSIM_ASSERT(shape != nullptr) << "Empty shape passed in.";
+
+ auto shapeType = shape->getType();
+ SURGSIM_ASSERT(Math::SHAPE_TYPE_NONE < shapeType && shapeType < Math::SHAPE_TYPE_COUNT) <<
+ "Unknown shape type passed in.";
+
+ return m_pointGenerators[shapeType]->pointInShape(shape);
+}
+
+Math::Vector3d RandomPointGenerator::pointOnShape(std::shared_ptr<Math::Shape> shape)
+{
+ SURGSIM_ASSERT(shape != nullptr) << "Empty shape passed in.";
+
+ auto shapeType = shape->getType();
+ SURGSIM_ASSERT(Math::SHAPE_TYPE_NONE < shapeType && shapeType < Math::SHAPE_TYPE_COUNT) <<
+ "Unknown shape type passed in.";
+
+ return m_pointGenerators[shapeType]->pointOnShape(shape);
+}
+
+}; // namespace Particles
+}; // namespace SurgSim
diff --git a/SurgSim/Particles/RandomPointGenerator.h b/SurgSim/Particles/RandomPointGenerator.h
new file mode 100644
index 0000000..d1e1f47
--- /dev/null
+++ b/SurgSim/Particles/RandomPointGenerator.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_PARTICLES_RANDOMPOINTGENERATOR_H
+#define SURGSIM_PARTICLES_RANDOMPOINTGENERATOR_H
+
+#include <array>
+#include <memory>
+
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/PointGenerator.h"
+
+namespace SurgSim
+{
+
+namespace Particles
+{
+/// RandomPointGenerator will generate points based on the shape passed.
+/// Internally, this class maintains a list of PointGenerators for each supported shape.
+/// The list gets populated when RandomPointGenerator is constructed.
+/// \sa PointGenerator
+class RandomPointGenerator : public PointGenerator
+{
+public:
+ /// Constructor
+ RandomPointGenerator();
+
+ SurgSim::Math::Vector3d pointInShape(std::shared_ptr<SurgSim::Math::Shape> shape) override;
+ SurgSim::Math::Vector3d pointOnShape(std::shared_ptr<SurgSim::Math::Shape> shape) override;
+
+private:
+ /// List of point generators.
+ /// Will be populated by constructor.
+ std::array<std::unique_ptr<PointGenerator>, SurgSim::Math::SHAPE_TYPE_COUNT> m_pointGenerators;
+};
+
+}; // namespace Particles
+}; // namespace SurgSim
+
+#endif // SURGSIM_PARTICLES_RANDOMPOINTGENERATOR_H
diff --git a/SurgSim/Particles/RandomSpherePointGenerator.cpp b/SurgSim/Particles/RandomSpherePointGenerator.cpp
new file mode 100644
index 0000000..0b6521f
--- /dev/null
+++ b/SurgSim/Particles/RandomSpherePointGenerator.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/Particles/RandomSpherePointGenerator.h"
+
+#include "SurgSim/Math/SphereShape.h"
+
+namespace SurgSim
+{
+namespace Particles
+{
+using SurgSim::Math::Vector3d;
+
+RandomSpherePointGenerator::~RandomSpherePointGenerator()
+{
+}
+
+Vector3d RandomSpherePointGenerator::pointInShape(std::shared_ptr<SurgSim::Math::Shape> shape)
+{
+ auto sphere = std::static_pointer_cast<SurgSim::Math::SphereShape>(shape);
+
+ // A point on the sphere, use it as direction and then multiple it by a number between [0,1) to move it inside.
+ Vector3d result = pointOnShape(shape) * m_closedZeroOpenOneDistribution(m_generator);
+
+ return result;
+}
+
+Vector3d RandomSpherePointGenerator::pointOnShape(std::shared_ptr<SurgSim::Math::Shape> shape)
+{
+ auto sphere = std::static_pointer_cast<SurgSim::Math::SphereShape>(shape);
+ double radius = sphere->getRadius();
+
+ // Spherical coordinate system can not produce an uniformly distributed points on the surface of the sphere.
+ // ref: http://mathworld.wolfram.com/SpherePointPicking.html
+
+ // Implementation was based on http://www.cs.cmu.edu/~mws/rpos.html
+ double z = 0.0;
+ double phi = 0.0;
+ double theta = 0.0;
+ double cosineTheta = 0.0;
+
+ Vector3d result = Vector3d::Zero();
+ // If the origin (0.0, 0.0, 0.0) is produced, regenerate.
+ while (result.isZero())
+ {
+ z = m_closedOneOneDistribution(m_generator) * radius;
+ phi = m_closedZeroOneDistribution(m_generator) * 2.0 * M_PI;
+ theta = std::asin(z / radius);
+
+ cosineTheta = std::cos(theta);
+
+ result.x() = radius * cosineTheta * std::cos(phi);
+ result.y() = radius * cosineTheta * std::sin(phi);
+ result.z() = z;
+ }
+
+ return result;
+}
+
+}; // namespace Particles
+}; // namespace SurgSim
diff --git a/SurgSim/Particles/RandomSpherePointGenerator.h b/SurgSim/Particles/RandomSpherePointGenerator.h
new file mode 100644
index 0000000..79c7eeb
--- /dev/null
+++ b/SurgSim/Particles/RandomSpherePointGenerator.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_PARTICLES_RANDOMSPHEREPOINTGENERATOR_H
+#define SURGSIM_PARTICLES_RANDOMSPHEREPOINTGENERATOR_H
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/PointGenerator.h"
+
+namespace SurgSim
+{
+
+namespace Math
+{
+class Shape;
+}
+
+namespace Particles
+{
+
+/// Class to generate points inside or on the surface of a sphere.
+class RandomSpherePointGenerator: public PointGenerator
+{
+public:
+ /// Destructor
+ virtual ~RandomSpherePointGenerator();
+
+ SurgSim::Math::Vector3d pointInShape(std::shared_ptr<SurgSim::Math::Shape> shape) override;
+ SurgSim::Math::Vector3d pointOnShape(std::shared_ptr<SurgSim::Math::Shape> shape) override;
+};
+
+}; // namespace Particles
+}; // namespace SurgSim
+
+#endif // SURGSIM_PARTICLES_RANDOMSPHEREPOINTGENERATOR_H
diff --git a/SurgSim/Particles/RenderTests/CMakeLists.txt b/SurgSim/Particles/RenderTests/CMakeLists.txt
new file mode 100644
index 0000000..200a67f
--- /dev/null
+++ b/SurgSim/Particles/RenderTests/CMakeLists.txt
@@ -0,0 +1,50 @@
+# 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
+ ${OPENTHREADS_INCLUDE_DIR}
+)
+
+set(UNIT_TEST_SOURCES
+ RenderTest.cpp
+ RenderTestSphRepresentation.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
+ SurgSimParticles
+)
+
+surgsim_add_unit_tests(SurgSimParticlesRenderTest)
+
+set_target_properties(SurgSimParticlesRenderTest PROPERTIES FOLDER "Particles")
+
+# Configure the path for the data files
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
diff --git a/SurgSim/Particles/RenderTests/RenderTest.cpp b/SurgSim/Particles/RenderTests/RenderTest.cpp
new file mode 100644
index 0000000..10d7ba8
--- /dev/null
+++ b/SurgSim/Particles/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/Particles/RenderTests/RenderTest.h"
+
+namespace SurgSim
+{
+namespace Particles
+{
+
+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 Particles
+}; // namespace SurgSim
diff --git a/SurgSim/Particles/RenderTests/RenderTest.h b/SurgSim/Particles/RenderTests/RenderTest.h
new file mode 100644
index 0000000..34be85e
--- /dev/null
+++ b/SurgSim/Particles/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_PARTICLES_RENDERTESTS_RENDERTEST_H
+#define SURGSIM_PARTICLES_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 Particles
+{
+
+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 Particles
+}; // namespace SurgSim
+
+#endif //SURGSIM_PARTICLES_RENDERTESTS_RENDERTEST_H
diff --git a/SurgSim/Particles/RenderTests/RenderTestSphRepresentation.cpp b/SurgSim/Particles/RenderTests/RenderTestSphRepresentation.cpp
new file mode 100644
index 0000000..f400f2e
--- /dev/null
+++ b/SurgSim/Particles/RenderTests/RenderTestSphRepresentation.cpp
@@ -0,0 +1,166 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+///\file RenderTestSphRepresentation.cpp render test for SphRepresentation
+
+#include <memory>
+
+#include "SurgSim/Blocks/TransferParticlesToPointCloudBehavior.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/Emitter.h"
+#include "SurgSim/Particles/ParticlesCollisionRepresentation.h"
+#include "SurgSim/Particles/RandomSpherePointGenerator.h"
+#include "SurgSim/Particles/RenderTests/RenderTest.h"
+#include "SurgSim/Particles/Sink.h"
+#include "SurgSim/Particles/SphRepresentation.h"
+
+
+namespace SurgSim
+{
+namespace Particles
+{
+
+std::shared_ptr<Framework::SceneElement> createCube()
+{
+ auto element = std::make_shared<Framework::BasicSceneElement>("cube");
+
+ auto mesh = std::make_shared<Math::MeshShape>();
+ mesh->load("Geometry/Cube.ply");
+
+ auto collision = std::make_shared<Collision::ShapeCollisionRepresentation>("collision");
+ collision->setShape(mesh);
+ element->addComponent(collision);
+
+ auto graphics = std::make_shared<Graphics::OsgMeshRepresentation>("graphics");
+ graphics->setShape(mesh);
+ graphics->setDrawAsWireFrame(true);
+ element->addComponent(graphics);
+
+ return element;
+}
+
+std::shared_ptr<Framework::SceneElement> createSink(const std::shared_ptr<Framework::SceneElement>& particles)
+{
+ auto element = std::make_shared<Framework::BasicSceneElement>("cube sink");
+
+ auto mesh = std::make_shared<Math::MeshShape>();
+ mesh->load("Geometry/Cube.ply");
+
+ auto collision = std::make_shared<Collision::ShapeCollisionRepresentation>("collision");
+ collision->setShape(mesh);
+ element->addComponent(collision);
+
+ auto graphics = std::make_shared<Graphics::OsgMeshRepresentation>("graphics");
+ graphics->setShape(mesh);
+ graphics->setDrawAsWireFrame(true);
+ element->addComponent(graphics);
+
+ auto sink = std::make_shared<Sink>("sink");
+ sink->setCollisionRepresentation(collision);
+ sink->setTarget(particles->getComponent("physics"));
+ element->addComponent(sink);
+
+ return element;
+}
+
+std::shared_ptr<Framework::SceneElement> createEmitter(const std::shared_ptr<Framework::SceneElement>& particles)
+{
+ auto element = std::make_shared<Framework::BasicSceneElement>("sphere emitter");
+
+ const double radius = 0.1;
+
+ auto emitter = std::make_shared<Emitter>("emitter");
+ emitter->setTarget(particles->getComponent("physics"));
+ emitter->setShape(std::make_shared<Math::SphereShape>(radius));
+ emitter->setMode(EMIT_MODE_SURFACE);
+ emitter->setRate(2000.0);
+ emitter->setLifetimeRange(std::make_pair(30000, 600000));
+ emitter->setVelocityRange(std::make_pair(Math::Vector3d::Zero(), Math::Vector3d::Zero()));
+ element->addComponent(emitter);
+
+ auto graphics = std::make_shared<Graphics::OsgSphereRepresentation>("graphics");
+ graphics->setRadius(radius);
+ graphics->setDrawAsWireFrame(true);
+ element->addComponent(graphics);
+
+ return element;
+}
+
+std::shared_ptr<Framework::SceneElement> createParticleSystem()
+{
+ auto element = std::make_shared<Framework::BasicSceneElement>("particles");
+
+ // c.f. "Lagrangian Fluid Dynamics Using Smoothed Particle Hydrodynamics", Micky Kelager, January 9th 2006.
+ // for input data to simulate water.
+ auto particles = std::make_shared<SphRepresentation>("physics");
+ particles->setMaxParticles(2000);
+ particles->setMassPerParticle(0.02);
+ particles->setDensity(998.29);
+ particles->setGasStiffness(3.0);
+ particles->setKernelSupport(0.0457);
+ particles->setSurfaceTension(0.0728);
+ particles->setViscosity(3.5);
+ particles->setStiffness(1000.0);
+ particles->setDamping(4.0);
+ particles->setFriction(0.1);
+ element->addComponent(particles);
+
+ auto particleCollision = std::make_shared<ParticlesCollisionRepresentation>("collision");
+ particles->setCollisionRepresentation(particleCollision);
+ element->addComponent(particleCollision);
+
+ auto particleGraphics = std::make_shared<Graphics::OsgPointCloudRepresentation>("graphics");
+ particleGraphics->setColor(Math::Vector4d::Ones());
+ particleGraphics->setPointSize(3.0f);
+ element->addComponent(particleGraphics);
+
+ auto graphicsUpdater = std::make_shared<Blocks::TransferParticlesToPointCloudBehavior>("particles to graphics");
+ graphicsUpdater->setSource(particles);
+ graphicsUpdater->setTarget(particleGraphics);
+ element->addComponent(graphicsUpdater);
+
+ return element;
+}
+
+TEST_F(RenderTests, SphRenderTest)
+{
+ auto particles = createParticleSystem();
+ scene->addSceneElement(particles);
+
+ auto cube = createCube();
+ cube->setPose(Math::makeRigidTranslation(Math::Vector3d(1.0, -1.5, 0.0)));
+ scene->addSceneElement(cube);
+
+ auto emitter = createEmitter(particles);
+ emitter->setPose(Math::makeRigidTranslation(Math::Vector3d(0.0, 0.1, 0.0)));
+ scene->addSceneElement(emitter);
+
+ auto sink = createSink(particles);
+ sink->setPose(Math::makeRigidTranslation(Math::Vector3d(-1.0, -1.5, 0.0)));
+ scene->addSceneElement(sink);
+
+ physicsManager->setRate(500.0);
+ runTest(Math::Vector3d(0.0, 0.0, 8.5), Math::Vector3d::Zero(), 20000.0);
+}
+
+};
+};
diff --git a/SurgSim/Particles/RenderTests/config.txt.in b/SurgSim/Particles/RenderTests/config.txt.in
new file mode 100644
index 0000000..76e073b
--- /dev/null
+++ b/SurgSim/Particles/RenderTests/config.txt.in
@@ -0,0 +1,2 @@
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data/
+
diff --git a/SurgSim/Particles/Representation.cpp b/SurgSim/Particles/Representation.cpp
new file mode 100644
index 0000000..1f31183
--- /dev/null
+++ b/SurgSim/Particles/Representation.cpp
@@ -0,0 +1,160 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Particles/Representation.h"
+
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/ParticlesCollisionRepresentation.h"
+
+
+namespace SurgSim
+{
+namespace Particles
+{
+
+Representation::Representation(const std::string& name) :
+ SurgSim::Framework::Representation(name),
+ m_maxParticles(0u),
+ m_logger(SurgSim::Framework::Logger::getLogger("Particles"))
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, size_t, MaxParticles, getMaxParticles,
+ setMaxParticles);
+}
+
+Representation::~Representation()
+{
+}
+
+bool Representation::doInitialize()
+{
+ m_particles.publish();
+ return true;
+}
+
+void Representation::setMaxParticles(size_t maxParticles)
+{
+ m_particles.unsafeGet().getVertices().reserve(maxParticles);
+ m_maxParticles = maxParticles;
+}
+
+size_t Representation::getMaxParticles() const
+{
+ return m_maxParticles;
+}
+
+SurgSim::DataStructures::BufferedValue<Particles>& Representation::getParticles()
+{
+ return m_particles;
+}
+
+bool Representation::addParticle(const Particle& particle)
+{
+ bool result;
+ auto& particles = m_particles.unsafeGet().getVertices();
+ if (particles.size() < m_maxParticles)
+ {
+ particles.push_back(particle);
+ result = true;
+ }
+ else
+ {
+ SURGSIM_LOG_DEBUG(m_logger) << "Unable to add another particle, maximum has been reached ("
+ << m_maxParticles << ").";
+ result = false;
+ }
+ return result;
+}
+
+bool Representation::addParticle(const Math::Vector3d& position, const Math::Vector3d& velocity,
+ double lifetime)
+{
+ ParticleData data = {lifetime, velocity};
+ return addParticle(Particle(position, data));
+}
+
+void Representation::removeParticle(size_t index)
+{
+ auto& particles = m_particles.unsafeGet().getVertices();
+ particles.at(index).data.lifetime = 0.0;
+}
+
+void Representation::update(double dt)
+{
+ auto& particles = m_particles.unsafeGet().getVertices();
+ auto particle = particles.begin();
+ auto newEnd = particles.end();
+ while (particle != newEnd)
+ {
+ particle->data.lifetime -= dt;
+ if (particle->data.lifetime <= 0.0)
+ {
+ --newEnd;
+ std::swap(*particle, *newEnd);
+ }
+ else
+ {
+ ++particle;
+ }
+ }
+ particles.erase(newEnd, particles.end());
+
+ if (!doUpdate(dt))
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Particle System " << getName() << " failed to update.";
+ }
+ m_particles.publish();
+}
+
+void Representation::handleCollisions(double dt)
+{
+ auto collisionRepresentation = m_collisionRepresentation;
+ if (collisionRepresentation != nullptr)
+ {
+ if (!doHandleCollisions(dt, collisionRepresentation->getCollisions().unsafeGet()))
+ {
+ SURGSIM_LOG_WARNING(m_logger) << "Particle System " << getName() << " failed to handle collisions.";
+ }
+ }
+}
+
+std::shared_ptr<SurgSim::Collision::Representation> Representation::getCollisionRepresentation() const
+{
+ return m_collisionRepresentation;
+}
+
+void Representation::setCollisionRepresentation(std::shared_ptr<SurgSim::Collision::Representation> representation)
+{
+ if (m_collisionRepresentation != representation)
+ {
+ auto oldCollisionRep = std::dynamic_pointer_cast<ParticlesCollisionRepresentation>(m_collisionRepresentation);
+ if (oldCollisionRep != nullptr)
+ {
+ oldCollisionRep->setParticleRepresentation(nullptr);
+ }
+ m_collisionRepresentation = representation;
+
+ auto newCollisionRep = std::dynamic_pointer_cast<ParticlesCollisionRepresentation>(representation);
+ if (newCollisionRep != nullptr)
+ {
+ newCollisionRep->setParticleRepresentation(
+ std::static_pointer_cast<SurgSim::Particles::Representation>(getSharedPtr()));
+ }
+ }
+}
+
+
+}; // namespace Particles
+}; // namespace SurgSim
diff --git a/SurgSim/Particles/Representation.h b/SurgSim/Particles/Representation.h
new file mode 100644
index 0000000..6ee99a1
--- /dev/null
+++ b/SurgSim/Particles/Representation.h
@@ -0,0 +1,122 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PARTICLES_REPRESENTATION_H
+#define SURGSIM_PARTICLES_REPRESENTATION_H
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/Representation.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/Particles.h"
+
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+class Logger;
+};
+
+namespace Particles
+{
+
+/// The Representation class defines the base class for all Particle System.
+class Representation : public SurgSim::Framework::Representation
+{
+public:
+ /// Constructor
+ /// \param name The representation's name
+ explicit Representation(const std::string& name);
+
+ /// Destructor
+ virtual ~Representation();
+
+ /// Set the maximum number of particles of this system.
+ /// \note Once initialized, it can't be changed.
+ /// \param maxParticles The maximum number of particles in this system.
+ void setMaxParticles(size_t maxParticles);
+
+ /// \return The number of particles allowed in this system.
+ size_t getMaxParticles() const;
+
+ /// Add a particle
+ /// \param particle The new particle
+ /// \return True if the particle was successfully added, false otherwise
+ bool addParticle(const Particle& particle);
+
+ /// Add a particle
+ /// \param position The position of the new particle
+ /// \param velocity The velocity of the new particle
+ /// \param lifetime The lenght of time the particle will exist
+ /// \return True if the particle was successfully added, false otherwise
+ bool addParticle(const Math::Vector3d& position, const Math::Vector3d& velocity, double lifetime);
+
+ /// Remove a particle
+ /// \note The particle will be removed during the next update
+ /// \param index of the particle
+ void removeParticle(size_t index);
+
+ /// Get the particles
+ /// \return The particles in a BufferedValue
+ SurgSim::DataStructures::BufferedValue<Particles>& getParticles();
+
+ /// Update the particle system
+ /// \param dt The time step.
+ void update(double dt);
+
+ /// Handle collisions with particle system
+ /// \param dt The time step.
+ void handleCollisions(double dt);
+
+ /// Set the collision representation for this Particle Representation
+ /// \param representation The collision representation to be set
+ void setCollisionRepresentation(std::shared_ptr<SurgSim::Collision::Representation> representation);
+
+ /// Get the collision representation for this Particle Representation
+ /// \return the collision representation
+ std::shared_ptr<SurgSim::Collision::Representation> getCollisionRepresentation() const;
+
+protected:
+ /// Implementation of the specific behavior of the particle system
+ /// \return True if update succeeded, False otherwise.
+ virtual bool doUpdate(double dt) = 0;
+
+ /// Implementation of the specific collision handling of the particle system
+ /// \return True if succeeded, False otherwise.
+ virtual bool doHandleCollisions(double dt, const SurgSim::Collision::ContactMapType& collisions) = 0;
+
+ bool doInitialize() override;
+
+ /// Maximum amount of particles allowed in this particle system.
+ size_t m_maxParticles;
+
+ /// BufferedValue of particles.
+ SurgSim::DataStructures::BufferedValue<Particles> m_particles;
+
+ /// Logger used by the particle system.
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+
+ /// This entity's collision representation
+ std::shared_ptr<SurgSim::Collision::Representation> m_collisionRepresentation;
+};
+
+}; // namespace Particles
+}; // namespace SurgSim
+
+#endif // SURGSIM_PARTICLES_REPRESENTATION_H
diff --git a/SurgSim/Particles/Sink.cpp b/SurgSim/Particles/Sink.cpp
new file mode 100644
index 0000000..3a28045
--- /dev/null
+++ b/SurgSim/Particles/Sink.cpp
@@ -0,0 +1,107 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Particles/Sink.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/Component.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Particles/Representation.h"
+
+
+namespace SurgSim
+{
+
+namespace Particles
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Particles::Sink, Sink);
+
+Sink::Sink(const std::string& name) :
+ Framework::Behavior(name),
+ m_logger(Framework::Logger::getLogger("Particles"))
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Sink, std::shared_ptr<SurgSim::Framework::Component>, Target, getTarget,
+ setTarget);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Sink, std::shared_ptr<SurgSim::Framework::Component>, CollisionRepresentation,
+ getCollisionRepresentation, setCollisionRepresentation);
+}
+
+void Sink::setTarget(const std::shared_ptr<SurgSim::Framework::Component>& target)
+{
+ m_target = Framework::checkAndConvert<SurgSim::Particles::Representation>(target,
+ "SurgSim::Particles::Representation");
+}
+
+std::shared_ptr<SurgSim::Framework::Component> Sink::getTarget()
+{
+ return m_target;
+}
+
+void Sink::setCollisionRepresentation(const std::shared_ptr<Framework::Component>& representation)
+{
+ m_collisionRepresentation = Framework::checkAndConvert<SurgSim::Collision::Representation>(representation,
+ "SurgSim::Collision::Representation");
+}
+
+std::shared_ptr<SurgSim::Framework::Component> Sink::getCollisionRepresentation()
+{
+ return m_collisionRepresentation;
+}
+
+bool Sink::doInitialize()
+{
+ return true;
+}
+
+bool Sink::doWakeUp()
+{
+ if (m_collisionRepresentation == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Sinks need a Collision Representation.";
+ return false;
+ }
+ if (m_target == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Sinks need a Particle Representation to remove from.";
+ return false;
+ }
+ return true;
+}
+
+void Sink::update(double dt)
+{
+ auto collisions = m_collisionRepresentation->getCollisions().safeGet();
+ auto found = collisions->find(m_target->getCollisionRepresentation());
+ if (found != collisions->end())
+ {
+ for (auto& contact : found->second)
+ {
+ m_target->removeParticle(contact->penetrationPoints.second.index.getValue());
+ }
+ }
+}
+
+int Sink::getTargetManagerType() const
+{
+ return Framework::MANAGER_TYPE_PHYSICS;
+}
+
+
+};
+};
diff --git a/SurgSim/Particles/Sink.h b/SurgSim/Particles/Sink.h
new file mode 100644
index 0000000..0759166
--- /dev/null
+++ b/SurgSim/Particles/Sink.h
@@ -0,0 +1,94 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PARTICLES_SINK_H
+#define SURGSIM_PARTICLES_SINK_H
+
+#include <memory>
+
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+
+
+namespace SurgSim
+{
+
+namespace Collision
+{
+class Representation;
+};
+
+namespace Framework
+{
+class Component;
+class Logger;
+};
+
+namespace Particles
+{
+
+class Representation;
+
+SURGSIM_STATIC_REGISTRATION(Sink);
+
+/// Sink removes particles from a ParticleSystem
+class Sink : public SurgSim::Framework::Behavior
+{
+public:
+ /// Constructor
+ /// \param name The Sink's name
+ explicit Sink(const std::string& name);
+
+ SURGSIM_CLASSNAME(SurgSim::Particles::Sink);
+
+ /// Set the target to remove particles from
+ /// \param target The ParticleSystem to remove from.
+ void setTarget(const std::shared_ptr<SurgSim::Framework::Component>& target);
+
+ /// Get the target removing particles from
+ /// \return The ParticleSystem to remove from.
+ std::shared_ptr<SurgSim::Framework::Component> getTarget();
+
+ /// Set the collision representation for this Sink
+ /// Particles that collide with this collision representation will be removed
+ /// \param representation The collision representation
+ void setCollisionRepresentation(const std::shared_ptr<SurgSim::Framework::Component>& representation);
+
+ /// Get the collision representation for this Sink
+ /// Particles that collide with this collision representation will be removed
+ /// \return the collision representation for this Sink
+ std::shared_ptr<SurgSim::Framework::Component> getCollisionRepresentation();
+
+ void update(double dt) override;
+
+ int getTargetManagerType() const override;
+
+private:
+ bool doInitialize() override;
+
+ bool doWakeUp() override;
+
+ std::shared_ptr<SurgSim::Collision::Representation> m_collisionRepresentation;
+
+ std::shared_ptr<SurgSim::Particles::Representation> m_target;
+
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+};
+
+};
+};
+
+#endif // SURGSIM_PARTICLES_SINK_H
+
diff --git a/SurgSim/Particles/SphRepresentation.cpp b/SurgSim/Particles/SphRepresentation.cpp
new file mode 100644
index 0000000..c0caaa8
--- /dev/null
+++ b/SurgSim/Particles/SphRepresentation.cpp
@@ -0,0 +1,367 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Particles/SphRepresentation.h"
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/DataStructures/Grid.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/Vector.h"
+
+
+namespace SurgSim
+{
+namespace Particles
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Particles::SphRepresentation, SphRepresentation);
+
+SphRepresentation::SphRepresentation(const std::string& name) :
+ Representation(name),
+ m_mass(0.0),
+ m_densityReference(0.0),
+ m_gasStiffness(0.0),
+ m_surfaceTension(0.0),
+ m_stiffness(0.0),
+ m_damping(0.0),
+ m_friction(0.0),
+ m_gravity(Math::Vector3d(0.0, -9.81, 0.0)),
+ m_viscosity(0.0),
+ m_h(0.0)
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SphRepresentation, double, MassPerParticle, getMassPerParticle,
+ setMassPerParticle);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SphRepresentation, double, Density, getDensity, setDensity);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SphRepresentation, double, GasStiffness, getGasStiffness, setGasStiffness);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SphRepresentation, double, SurfaceTension, getSurfaceTension, setSurfaceTension);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SphRepresentation, double, Viscosity, getViscosity, setViscosity);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SphRepresentation, double, Stiffness, getStiffness, setStiffness);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SphRepresentation, double, Damping, getDamping, setDamping);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SphRepresentation, double, Friction, getFriction, setFriction);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SphRepresentation, double, KernelSupport, getKernelSupport, setKernelSupport);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(SphRepresentation, Math::Vector3d, Gravity, getGravity, setGravity);
+}
+
+SphRepresentation::~SphRepresentation()
+{
+}
+
+void SphRepresentation::setMassPerParticle(double particleMass)
+{
+ SURGSIM_ASSERT(particleMass > 0.0) <<
+ "The mass per particle needs to be a valid positive non null value." << particleMass << " was provided.";
+ m_mass = particleMass;
+}
+
+double SphRepresentation::getMassPerParticle() const
+{
+ return m_mass;
+}
+
+void SphRepresentation::setDensity(double density)
+{
+ SURGSIM_ASSERT(density > 0.0) <<
+ "The density needs to be a valid positive non null value." << density << " was provided.";
+ m_densityReference = density;
+}
+
+double SphRepresentation::getDensity() const
+{
+ return m_densityReference;
+}
+
+void SphRepresentation::setGasStiffness(double stiffness)
+{
+ SURGSIM_ASSERT(stiffness > 0.0) <<
+ "The gas stiffness needs to be a valid positive non null value." << stiffness << " was provided.";
+ m_gasStiffness = stiffness;
+}
+
+double SphRepresentation::getGasStiffness() const
+{
+ return m_gasStiffness;
+}
+
+void SphRepresentation::setSurfaceTension(double surfaceTension)
+{
+ SURGSIM_ASSERT(surfaceTension >= 0.0) <<
+ "The surface tension needs to be a valid positive or null value." <<
+ surfaceTension << " was provided.";
+ m_surfaceTension = surfaceTension;
+}
+
+double SphRepresentation::getSurfaceTension() const
+{
+ return m_surfaceTension;
+}
+
+void SphRepresentation::setGravity(const SurgSim::Math::Vector3d& gravity)
+{
+ m_gravity = gravity;
+}
+
+SurgSim::Math::Vector3d SphRepresentation::getGravity() const
+{
+ return m_gravity;
+}
+
+void SphRepresentation::setViscosity(double viscosity)
+{
+ SURGSIM_ASSERT(viscosity >= 0.0) <<
+ "The viscosity needs to be a valid positive or null value." << viscosity << " was provided.";
+ m_viscosity = viscosity;
+}
+
+double SphRepresentation::getViscosity() const
+{
+ return m_viscosity;
+}
+
+void SphRepresentation::setKernelSupport(double support)
+{
+ SURGSIM_ASSERT(support > 0.0) <<
+ "The kernel support needs to be a valid positive non-null value." << support << " was provided.";
+
+ m_h = support;
+ m_hSquared = m_h * m_h;
+ m_kernelPoly6 = 315.0 / (64.0 * M_PI * std::pow(m_h, 9));
+ m_kernelPoly6Gradient = -945.0 / (32.0 * M_PI * std::pow(m_h, 9));
+ m_kernelSpikyGradient = -45.0 / (M_PI * std::pow(m_h, 6));
+ m_kernelViscosityLaplacian = 45.0 / (M_PI * std::pow(m_h, 5));
+ m_kernelPoly6Laplacian = 945.0 / (8.0 * M_PI * std::pow(m_h, 9));
+}
+
+double SphRepresentation::getKernelSupport() const
+{
+ return m_h;
+}
+
+void SphRepresentation::setStiffness(double stiffness)
+{
+ m_stiffness = stiffness;
+}
+
+double SphRepresentation::getStiffness() const
+{
+ return m_stiffness;
+}
+
+void SphRepresentation::setDamping(double damping)
+{
+ m_damping = damping;
+}
+
+double SphRepresentation::getDamping() const
+{
+ return m_damping;
+}
+
+void SphRepresentation::setFriction(double friction)
+{
+ m_friction = friction;
+}
+
+double SphRepresentation::getFriction() const
+{
+ return m_friction;
+}
+
+bool SphRepresentation::doInitialize()
+{
+ if (!Representation::doInitialize())
+ {
+ return false;
+ }
+
+ SURGSIM_ASSERT(m_mass > 0.0) <<
+ "The mass per particle needs to be set prior to adding the component in the SceneElement";
+ SURGSIM_ASSERT(m_densityReference > 0.0) <<
+ "The reference density needs to be set prior to adding the component in the SceneElement";
+ SURGSIM_ASSERT(m_gasStiffness > 0.0) <<
+ "The gas stiffness needs to be set prior to adding the component in the SceneElement";
+ SURGSIM_ASSERT(m_h > 0.0) <<
+ "The kernel support needs to be set prior to adding the component in the SceneElement";
+
+ m_normal.resize(m_maxParticles, 3);
+ m_acceleration.resize(m_maxParticles, 3);
+ m_density.resize(m_maxParticles);
+ m_pressure.resize(m_maxParticles);
+
+ // The 3d grid is composed of 2^10 cubic cells on each dimension of size m_h each.
+ // This covers a volume of (m_h * 2^10)^3 cubic meter centered on the origin.
+ Math::Vector3d aabbSize(1024 * m_h, 1024 * m_h, 1024 * m_h);
+ Eigen::AlignedBox<double, 3> aabb;
+ aabb.min() = -aabbSize / 2.0;
+ aabb.max() = aabbSize / 2.0;
+ Math::Vector3d cellSize = Math::Vector3d::Constant(m_h);
+ m_grid = std::make_shared<DataStructures::Grid<size_t, 3>>(cellSize, aabb);
+
+ return true;
+}
+
+bool SphRepresentation::doUpdate(double dt)
+{
+ // Compute acceleration
+ computeNeighbors();
+ computeDensityAndPressureField();
+ computeNormalField();
+ computeAccelerations();
+
+ // Integrate ODE to determine new velocity and position
+ computeVelocityAndPosition(dt);
+
+ return true;
+}
+
+void SphRepresentation::computeVelocityAndPosition(double dt)
+{
+ auto& particles = m_particles.unsafeGet().getVertices();
+ for (size_t i = 0; i < particles.size(); i++)
+ {
+ particles[i].data.velocity += dt * m_acceleration.row(i);
+ particles[i].position += dt * particles[i].data.velocity;
+ }
+}
+
+void SphRepresentation::computeNeighbors()
+{
+ m_grid->reset();
+
+ auto& particles = m_particles.unsafeGet().getVertices();
+ for (size_t i = 0; i < particles.size(); i++)
+ {
+ m_grid->addElement(i, particles[i].position);
+ }
+}
+
+void SphRepresentation::computeDensityAndPressureField()
+{
+ auto& particles = m_particles.unsafeGet().getVertices();
+ m_density.head(particles.size()).setZero();
+ for (size_t i = 0; i < particles.size(); i++)
+ {
+ for (auto j : m_grid->getNeighbors(i))
+ {
+ const Math::Vector3d r = particles[i].position - particles[j].position;
+ const double rSquaredNorm = r.squaredNorm();
+ if (rSquaredNorm < m_hSquared)
+ {
+ m_density[i] += (m_hSquared - rSquaredNorm) * (m_hSquared - rSquaredNorm) * (m_hSquared - rSquaredNorm);
+ }
+ }
+ }
+ m_density *= m_mass * m_kernelPoly6;
+ m_pressure = m_gasStiffness * (m_density.array() - m_densityReference);
+}
+
+void SphRepresentation::computeNormalField()
+{
+ auto& particles = m_particles.unsafeGet().getVertices();
+ m_normal.topRows(particles.size()).setZero();
+ for (size_t i = 0; i < particles.size(); i++)
+ {
+ for (auto j : m_grid->getNeighbors(i))
+ {
+ const Math::Vector3d r = particles[i].position - particles[j].position;
+ const double rSquaredNorm = r.squaredNorm();
+ if (rSquaredNorm < m_hSquared)
+ {
+ m_normal.row(i) += (m_hSquared - rSquaredNorm) * (m_hSquared - rSquaredNorm)/ m_density[j] * r;
+ }
+ }
+ }
+ m_normal *= m_kernelPoly6Gradient * m_mass;
+}
+
+void SphRepresentation::computeAccelerations()
+{
+ auto& particles = m_particles.unsafeGet().getVertices();
+ m_acceleration.topRows(particles.size()).setZero();
+ for (size_t i = 0; i < particles.size(); i++)
+ {
+ for (auto j : m_grid->getNeighbors(i))
+ {
+ // Consider symmetry here
+ if (j <= i)
+ {
+ continue;
+ }
+
+ const Math::Vector3d r = particles[i].position - particles[j].position;
+ const double rSquaredNorm = r.squaredNorm();
+ if (rSquaredNorm < m_hSquared)
+ {
+ // Pressure force
+ const double rNorm = std::max(std::sqrt(rSquaredNorm), 0.0001);
+ const Math::Vector3d gradient = r * m_kernelSpikyGradient * (m_h - rNorm) * (m_h -rNorm) / rNorm;
+ Math::Vector3d f = (-(m_pressure[i] + m_pressure[j]) / (2.0 * m_density[i])) * gradient;
+
+ // Viscosity force
+ const Math::Vector3d v = particles[i].data.velocity - particles[j].data.velocity;
+ const double laplacian = m_kernelViscosityLaplacian * (1.0 - rNorm / m_h);
+ f += -(m_viscosity * v / m_density[j]) * laplacian;
+
+ // Surface tension force
+ const double normalNorm = m_normal.row(j).norm();
+ if (normalNorm > 20.0)
+ {
+ const Math::Vector3d unitNormal = m_normal.row(j) / normalNorm;
+ double laplacianPoly6 = m_kernelPoly6Laplacian * (m_hSquared - rSquaredNorm);
+ laplacianPoly6 *= (rSquaredNorm - 3.0 / 4.0 * (m_hSquared - rSquaredNorm));
+ f += -m_surfaceTension / m_density[j] * laplacianPoly6 * unitNormal;
+ }
+
+ // Action/reaction application on the pair of particles
+ m_acceleration.row(i) += f;
+ m_acceleration.row(j) -= f;
+ }
+ }
+ }
+ m_acceleration.array().colwise() *= m_mass / m_density.array();
+
+ const Math::Vector3d localGravity = getPose().linear().inverse() * m_gravity;
+ m_acceleration.rowwise() += localGravity.transpose();
+}
+
+bool SphRepresentation::doHandleCollisions(double dt, const SurgSim::Collision::ContactMapType& collisions)
+{
+ const Math::RigidTransform3d inversePose = getPose().inverse();
+ auto& particles = m_particles.unsafeGet().getVertices();
+
+ for (auto& collision : collisions)
+ {
+ for (auto& contact : collision.second)
+ {
+ Math::Vector3d normal = inversePose.linear() * contact->normal;
+ size_t index = contact->penetrationPoints.first.index.getValue();
+ auto& particle = particles[index];
+
+ double velocityAlongNormal = particle.data.velocity.dot(normal);
+ double forceIntensity = m_stiffness * contact->depth - m_damping * velocityAlongNormal;
+
+ Math::Vector3d tangentVelocity = particle.data.velocity - velocityAlongNormal * normal;
+ Math::Vector3d forceDirection = normal - m_friction * tangentVelocity.normalized();
+ Math::Vector3d accelerationCorrection = (forceIntensity / m_mass) * forceDirection;
+
+ particle.data.velocity += dt * accelerationCorrection;
+ particle.position += dt * dt * accelerationCorrection;
+ }
+ }
+ return true;
+}
+
+}; // namespace Particles
+}; // namespace SurgSim
diff --git a/SurgSim/Particles/SphRepresentation.h b/SurgSim/Particles/SphRepresentation.h
new file mode 100644
index 0000000..4274e74
--- /dev/null
+++ b/SurgSim/Particles/SphRepresentation.h
@@ -0,0 +1,205 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SURGSIM_PARTICLES_SPHREPRESENTATION_H
+#define SURGSIM_PARTICLES_SPHREPRESENTATION_H
+
+#include <Eigen/Core>
+#include <vector>
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/Representation.h"
+
+
+namespace SurgSim
+{
+
+namespace DataStructures
+{
+template <class T, size_t N>
+class Grid;
+}; // namespace DataStructures
+
+namespace Particles
+{
+
+SURGSIM_STATIC_REGISTRATION(SphRepresentation);
+
+/// SphRepresentation is a Representation dedicated to Smoothed-Particles Hydrodynamics (SPH).
+/// This class is mostly based on these papers:
+/// "Particle-Based Fluid Simulation for Interactive Applications", M. Muller, D. Charypar, M. Gross.
+/// In Proceedings of ACM SIGGRAPH Symposium on Computer Animation (SCA) 2003, pp 154-159.
+/// "Interactive Blood Simulation for Virtual Surgery Based on Smoothed Particle Hydrodynamics", M. Muller,
+/// S. Schirm, M. Teschner. Journal of Technology and Health Care, ISSN 0928-7329, IOS Press, Amsterdam.
+class SphRepresentation : public Representation
+{
+public:
+ SURGSIM_CLASSNAME(SurgSim::Particles::SphRepresentation);
+
+ /// Constructor
+ /// \param name The representation's name
+ explicit SphRepresentation(const std::string& name);
+
+ /// Destructor
+ virtual ~SphRepresentation();
+
+ /// Set the mass for each particle
+ /// \param particleMass The mass that will be used for all particles [Kg]
+ /// \throws An exception SurgSim::Framework::AssertionFailure if the value is negative or null
+ /// \note In the SPH model, a particle has a constant mass, but its volume and density vary
+ /// \note (mass = volume * density). <br>
+ /// \note Example: If we want to simulate 1 liter of water (0.001 m3 at 1000kg.m-3)
+ /// \note with 50 particles, we need each particle to have a mass of 1.0/50 = 0.02kg
+ void setMassPerParticle(double particleMass);
+
+ /// Get the mass for each particle
+ /// \return The mass that is used for all particle [Kg]
+ double getMassPerParticle() const;
+
+ /// Set the density of the fluid
+ /// \param density of the fluid [Kg.m-3]
+ /// \throws An exception SurgSim::Framework::AssertionFailure if the value is negative or null
+ void setDensity(double density);
+
+ /// Get the density of the fluid
+ /// \return The density of the fluid [Kg.m-3]
+ double getDensity() const;
+
+ /// Set the gas stiffness coefficient
+ /// \param stiffness coefficient of the gas [N.m.Kg-1]
+ /// \throws An exception SurgSim::Framework::AssertionFailure if the value is negative or null
+ void setGasStiffness(double stiffness);
+
+ /// Get the gas stiffness coefficient
+ /// \return The stiffness coefficient of the gas [N.m.Kg-1]
+ double getGasStiffness() const;
+
+ /// Set the surface tension
+ /// \param surfaceTension The surface tension [N.m-1]
+ /// \throws An exception SurgSim::Framework::AssertionFailure if the value is negative
+ void setSurfaceTension(double surfaceTension);
+
+ /// Get the surface tension
+ /// \return The surface tension [N.m-1]
+ double getSurfaceTension() const;
+
+ /// Set the gravity vector
+ /// \param gravity The 3d gravity vector [m]
+ void setGravity(const SurgSim::Math::Vector3d& gravity);
+
+ /// Get the gravity vector (default is (0 -9.81 0))
+ /// \return The 3d gravity vector [m]
+ SurgSim::Math::Vector3d getGravity() const;
+
+ /// Set the viscosity coefficient
+ /// \param viscosity coefficient [N.s.m-2]
+ /// \throws An exception SurgSim::Framework::AssertionFailure if the value is negative
+ void setViscosity(double viscosity);
+
+ /// Get the viscosity coefficient (default is 0.0)
+ /// \return The viscosity coefficient [N.s.m-2]
+ double getViscosity() const;
+
+ /// Set the kernel function support
+ /// \param support The length of the kernel support [m]
+ /// \throws An exception SurgSim::Framework::AssertionFailure if the value is negative or null
+ void setKernelSupport(double support);
+
+ /// Get the kernel function support
+ /// \return The length of the kernel support [m]
+ double getKernelSupport() const;
+
+ /// Set the particles stiffness when colliding
+ /// \param stiffness The stiffness [N/m]
+ void setStiffness(double stiffness);
+
+ /// Get the particles stiffness when colliding
+ /// \return The stiffness [N/m]
+ double getStiffness() const;
+
+ /// Set the particles damping when colliding
+ /// \param damping The damping [Ns/m]
+ void setDamping(double damping);
+
+ /// Get the particles damping when colliding
+ /// \return The damping [Ns/m]
+ double getDamping() const;
+
+ /// Set the sliding coefficient of friction for the particles during collisions
+ /// \param friction The sliding coefficient of friction
+ void setFriction(double friction);
+
+ /// Get the sliding coefficient of friction for the particles during collisions
+ /// \return The sliding coefficient of friction
+ double getFriction() const;
+
+protected:
+ bool doInitialize() override;
+
+ bool doUpdate(double dt) override;
+
+ bool doHandleCollisions(double dt, const SurgSim::Collision::ContactMapType& collisions) override;
+
+ /// Compute the particles' velocity and position given a time step dt
+ /// \param dt The time step to advance the simulation too
+ /// \note This method integrates the ODE equation of the SPH, computing velocities and positions from the
+ /// \note accelerations and storing them in the state. Therefore computeAcceleration(dt) should be called before.
+ void computeVelocityAndPosition(double dt);
+
+ Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> m_normal; ///< Particles' normal
+ Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> m_acceleration; ///< Particles' acceleration
+ Math::Vector m_density; ///< Particles' density
+ Math::Vector m_pressure; ///< Particles' pressure
+ double m_mass; ///< Mass per particle (determine the density of particle per m3)
+ double m_densityReference; ///< Density of the reference gas
+ double m_gasStiffness; ///< Stiffness of the gas considered
+ double m_surfaceTension; ///< Surface tension
+ double m_stiffness; ///< Collision stiffness
+ double m_damping; ///< Collision damping
+ double m_friction; ///< Collision sliding friction coefficient
+ SurgSim::Math::Vector3d m_gravity; ///< 3D Gravity vector
+ double m_viscosity; ///< Viscosity coefficient
+
+ /// Kernels parameter (support length and its powers)
+ double m_h;
+ double m_hSquared;
+ double m_kernelPoly6;
+ double m_kernelPoly6Gradient;
+ double m_kernelSpikyGradient;
+ double m_kernelViscosityLaplacian;
+ double m_kernelPoly6Laplacian;
+
+ /// Grid acceleration to evaluate the kernels locally (storing the particles' index)
+ std::shared_ptr<SurgSim::DataStructures::Grid<size_t, 3>> m_grid;
+
+private:
+ /// Compute the neighbors
+ void computeNeighbors();
+
+ /// Compute the density and pressure field
+ void computeDensityAndPressureField();
+
+ /// Compute the normal field
+ void computeNormalField();
+
+ /// Compute the Sph accelerations
+ void computeAccelerations();
+
+};
+
+}; // namespace Particles
+}; // namespace SurgSim
+
+#endif // SURGSIM_PARTICLES_SPHREPRESENTATION_H
diff --git a/SurgSim/Particles/UnitTests/CMakeLists.txt b/SurgSim/Particles/UnitTests/CMakeLists.txt
new file mode 100644
index 0000000..4aa0fff
--- /dev/null
+++ b/SurgSim/Particles/UnitTests/CMakeLists.txt
@@ -0,0 +1,50 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2015, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES 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
+ EmitterTests.cpp
+ MockObjects.cpp
+ ParticlesCollisionRepresentationTests.cpp
+ PointGeneratorTests.cpp
+ RandomPointGeneratorTests.cpp
+ RepresentationTests.cpp
+ SinkTests.cpp
+ SphRepresentationTests.cpp
+)
+
+set(UNIT_TEST_HEADERS
+ MockObjects.h
+)
+
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/config.txt.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/config.txt"
+)
+
+set(LIBS
+ SurgSimCollision
+ SurgSimFramework
+ SurgSimMath
+ SurgSimParticles
+ SurgSimPhysics
+)
+
+surgsim_add_unit_tests(SurgSimParticlesTest)
+
+set_target_properties(SurgSimParticlesTest PROPERTIES FOLDER "Particles")
diff --git a/SurgSim/Particles/UnitTests/EmitterTests.cpp b/SurgSim/Particles/UnitTests/EmitterTests.cpp
new file mode 100644
index 0000000..f5b01f6
--- /dev/null
+++ b/SurgSim/Particles/UnitTests/EmitterTests.cpp
@@ -0,0 +1,296 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 <Eigen/Geometry>
+#include <memory>
+#include <vector>
+#include <yaml-cpp/yaml.h>
+
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/Emitter.h"
+#include "SurgSim/Particles/UnitTests/MockObjects.h"
+
+using SurgSim::Framework::Component;
+using SurgSim::Math::Shape;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::makeRigidTransform;
+
+namespace
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, MockParticleSystem, MockParticleSystem);
+}
+
+namespace SurgSim
+{
+namespace Particles
+{
+
+TEST(EmitterTest, Constructor)
+{
+ std::shared_ptr<Emitter> emitter;
+ ASSERT_NO_THROW(emitter = std::make_shared<Emitter>("Emitter"));
+
+ EXPECT_EQ(EMIT_MODE_VOLUME, emitter->getMode());
+ EXPECT_EQ(0.0, emitter->getRate());
+ EXPECT_EQ(0.0, emitter->getLifetimeRange().first);
+ EXPECT_EQ(0.0, emitter->getLifetimeRange().second);
+ EXPECT_TRUE(emitter->getVelocityRange().first.isZero());
+ EXPECT_TRUE(emitter->getVelocityRange().second.isZero());
+ EXPECT_EQ(nullptr, emitter->getShape());
+}
+
+TEST(EmitterTest, SetGetShape)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ auto sphere = std::make_shared<SurgSim::Math::SphereShape>(0.1);
+ auto particleSystem = std::make_shared<MockParticleSystem>("ParticleSystem");
+
+ {
+ auto emitter = std::make_shared<Emitter>("Emitter");
+ emitter->setTarget(particleSystem);
+ EXPECT_TRUE(emitter->initialize(runtime));
+ EXPECT_FALSE(emitter->wakeUp()) << "Without a shape, the emitter should not wakup";
+ }
+ {
+ auto emitter = std::make_shared<Emitter>("Emitter");
+ emitter->setTarget(particleSystem);
+ emitter->setShape(sphere);
+ EXPECT_EQ(sphere, emitter->getShape());
+ EXPECT_TRUE(emitter->initialize(runtime));
+ EXPECT_TRUE(emitter->wakeUp()) << "With a shape, the emitter should wakeup";
+ }
+}
+
+TEST(EmitterTest, SetGetTarget)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ auto sphere = std::make_shared<SurgSim::Math::SphereShape>(0.1);
+ auto particleSystem = std::make_shared<MockParticleSystem>("ParticleSystem");
+
+ {
+ auto emitter = std::make_shared<Emitter>("Emitter");
+ auto notParticleSystem = std::make_shared<Emitter>("Not a ParticleSystem");
+ emitter->setShape(sphere);
+ EXPECT_THROW(emitter->setTarget(notParticleSystem), SurgSim::Framework::AssertionFailure);
+ EXPECT_TRUE(emitter->initialize(runtime));
+ EXPECT_FALSE(emitter->wakeUp()) << "Without a target, the emitter should not wakup";
+ }
+ {
+ auto emitter = std::make_shared<Emitter>("Emitter");
+ emitter->setShape(sphere);
+ EXPECT_NO_THROW(emitter->setTarget(particleSystem));
+ EXPECT_EQ(particleSystem, emitter->getTarget());
+ EXPECT_TRUE(emitter->initialize(runtime));
+ EXPECT_TRUE(emitter->wakeUp()) << "With a target, the emitter should wakeup";
+ }
+}
+
+TEST(EmitterTest, SetGetMode)
+{
+ auto emitter = std::make_shared<Emitter>("Emitter");
+ EXPECT_THROW(emitter->setMode(EMIT_MODE_COUNT), SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(emitter->setMode(EMIT_MODE_SURFACE));
+ EXPECT_EQ(EMIT_MODE_SURFACE, emitter->getMode());
+}
+
+TEST(EmitterTest, GetSetRate)
+{
+ auto emitter = std::make_shared<Emitter>("Emitter");
+ EXPECT_THROW(emitter->setRate(-10.0), SurgSim::Framework::AssertionFailure);
+ EXPECT_NO_THROW(emitter->setRate(2.0));
+ EXPECT_EQ(2.0, emitter->getRate());
+}
+
+TEST(EmitterTest, GetSetLifetimeRange)
+{
+ auto emitter = std::make_shared<Emitter>("Emitter");
+
+ EXPECT_THROW(emitter->setLifetimeRange(std::make_pair(-1.0, 1.0)), SurgSim::Framework::AssertionFailure)
+ << "Negative lifetimes should not be allowed";
+
+ EXPECT_THROW(emitter->setLifetimeRange(std::make_pair(10.0, 1.0)), SurgSim::Framework::AssertionFailure)
+ << "Should not allow lower bound to be greater than upper bound";
+
+ EXPECT_NO_THROW(emitter->setLifetimeRange(std::make_pair(1.0, 10.0)));
+ EXPECT_EQ(1.0, emitter->getLifetimeRange().first);
+ EXPECT_EQ(10.0, emitter->getLifetimeRange().second);
+}
+
+TEST(EmitterTest, GetSetVelocityRange)
+{
+ auto emitter = std::make_shared<Emitter>("Emitter");
+
+ EXPECT_THROW(emitter->setVelocityRange(std::make_pair(Vector3d::Constant(20.0), Vector3d::Ones())),
+ SurgSim::Framework::AssertionFailure) << "Minimum velocity must be less than maximum";
+
+ EXPECT_NO_THROW(emitter->setVelocityRange(std::make_pair(Vector3d::Ones(), Vector3d::Constant(3.0))));
+ EXPECT_NO_THROW(emitter->setVelocityRange(std::make_pair(Vector3d::Constant(-2.0), Vector3d::Ones())));
+
+ EXPECT_TRUE(emitter->getVelocityRange().first.isApprox(Vector3d::Constant(-2.0)));
+ EXPECT_TRUE(emitter->getVelocityRange().second.isApprox(Vector3d::Ones()));
+}
+
+TEST(EmitterTest, Update)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ auto sphere = std::make_shared<SurgSim::Math::SphereShape>(0.1);
+
+ auto particleSystem = std::make_shared<MockParticleSystem>("ParticleSystem");
+ particleSystem->setMaxParticles(10);
+ particleSystem->initialize(runtime);
+
+ auto emitter = std::make_shared<Emitter>("Emitter");
+ emitter->setShape(sphere);
+ emitter->setTarget(particleSystem);
+ emitter->setMode(EMIT_MODE_SURFACE);
+ emitter->setRate(10.0);
+ emitter->setLifetimeRange(std::make_pair(5.0, 10.0));
+ emitter->setVelocityRange(std::make_pair(Vector3d::Ones(), Vector3d::Constant(2.0)));
+ emitter->initialize(runtime);
+
+ ASSERT_TRUE(emitter->wakeUp());
+ ASSERT_TRUE(particleSystem->wakeUp());
+
+ emitter->update(0.1);
+ EXPECT_EQ(1, particleSystem->getParticles().unsafeGet().getNumVertices());
+
+ emitter->update(0.05);
+ EXPECT_EQ(1, particleSystem->getParticles().unsafeGet().getNumVertices());
+ emitter->update(0.05);
+ EXPECT_EQ(2, particleSystem->getParticles().unsafeGet().getNumVertices());
+
+ emitter->update(0.9);
+ EXPECT_EQ(10, particleSystem->getParticles().unsafeGet().getNumVertices());
+ emitter->update(0.2);
+ EXPECT_EQ(10, particleSystem->getParticles().unsafeGet().getNumVertices())
+ << "Particles should not have been added, the particle system should have reached its maximum.";
+
+ particleSystem->update(1.0);
+ auto particles = particleSystem->getParticles().unsafeGet().getVertices();
+ ASSERT_EQ(10, particles.size());
+ for (auto particle : particles)
+ {
+ EXPECT_NEAR(sphere->getRadius(), particle.position.norm(), 1e-9);
+ EXPECT_LT(4.0, particle.data.lifetime);
+ EXPECT_GT(9.0, particle.data.lifetime);
+ EXPECT_TRUE((Vector3d::Ones().array() <= particle.data.velocity.array()).all());
+ EXPECT_TRUE((Vector3d::Constant(2.0).array() >= particle.data.velocity.array()).all());
+ }
+}
+
+TEST(EmitterTest, PosedUpdate)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ auto sphere = std::make_shared<SurgSim::Math::SphereShape>(0.1);
+
+ auto particleSystem = std::make_shared<MockParticleSystem>("ParticleSystem");
+ particleSystem->setMaxParticles(10);
+
+ auto emitter = std::make_shared<Emitter>("Emitter");
+ emitter->setShape(sphere);
+ emitter->setTarget(particleSystem);
+ emitter->setMode(EMIT_MODE_VOLUME);
+ emitter->setRate(1.0);
+ emitter->setLifetimeRange(std::make_pair(5.0, 10.0));
+ emitter->setVelocityRange(std::make_pair(Vector3d::Ones(), Vector3d::Constant(2.0)));
+
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("Element");
+ element->addComponent(particleSystem);
+ element->addComponent(emitter);
+ runtime->getScene()->addSceneElement(element);
+
+ ASSERT_TRUE(emitter->wakeUp());
+ ASSERT_TRUE(particleSystem->wakeUp());
+
+ auto pose1 = makeRigidTransform(Eigen::AngleAxisd(0.25 * M_PI, Vector3d::UnitX()).matrix(),
+ Vector3d(1.0, 2.0, 3.0));
+ auto pose2 = makeRigidTransform(Eigen::AngleAxisd(0.25 * M_PI, Vector3d::UnitZ()).matrix(),
+ Vector3d(2.0, -2.0, 2.0));
+
+ emitter->update(1.0);
+ EXPECT_TRUE(emitter->getPose().isApprox(Math::RigidTransform3d::Identity()));
+ ASSERT_EQ(1, particleSystem->getParticles().unsafeGet().getNumVertices());
+
+ emitter->setLocalPose(pose1);
+ emitter->update(1.0);
+ EXPECT_TRUE(emitter->getPose().isApprox(pose1));
+ ASSERT_EQ(2, particleSystem->getParticles().unsafeGet().getNumVertices());
+
+ element->setPose(pose2);
+ emitter->update(1.0);
+ EXPECT_TRUE(emitter->getPose().isApprox(pose2 * pose1));
+ ASSERT_EQ(3, particleSystem->getParticles().unsafeGet().getNumVertices());
+
+ auto particles = particleSystem->getParticles().unsafeGet().getVertices();
+ EXPECT_GT(sphere->getRadius(), particles[0].position.norm());
+ EXPECT_GT(sphere->getRadius(), (pose1.inverse() * particles[1].position).norm());
+ EXPECT_GT(sphere->getRadius(), ((pose2 * pose1).inverse() * particles[2].position).norm());
+}
+
+TEST(EmitterTest, Serialization)
+{
+ auto emitter = std::make_shared<Emitter>("Emitter");
+ EXPECT_EQ("SurgSim::Particles::Emitter", emitter->getClassName());
+
+ const std::pair<double, double> expectedLifetimeRange(1.0, 10.0);
+ const int expectedMode = 1;
+ const double expectedRate = 10.0;
+ std::shared_ptr<Shape> expectedShape = std::make_shared<SurgSim::Math::SphereShape>(0.1);
+ std::shared_ptr<Component> expectedTarget = std::make_shared<MockParticleSystem>("ParticleSystem");
+ const std::pair<Vector3d, Vector3d> expectedVelocityRange(Vector3d::Ones(), Vector3d::Constant(2.0));
+ Math::RigidTransform3d expectedPose = makeRigidTransform(Eigen::AngleAxisd(0.25 * M_PI, Vector3d::UnitX()).matrix(),
+ Vector3d(1.0, 2.0, 3.0));
+
+ EXPECT_NO_THROW(emitter->setValue("LifetimeRange", expectedLifetimeRange));
+ EXPECT_NO_THROW(emitter->setValue("Mode", expectedMode));
+ EXPECT_NO_THROW(emitter->setValue("Rate", expectedRate));
+ EXPECT_NO_THROW(emitter->setValue("Shape", expectedShape));
+ EXPECT_NO_THROW(emitter->setValue("Target", expectedTarget));
+ EXPECT_NO_THROW(emitter->setValue("VelocityRange", expectedVelocityRange));
+ EXPECT_NO_THROW(emitter->setValue("LocalPose", expectedPose));
+
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<Component>::encode(*emitter));
+ EXPECT_TRUE(node.IsMap());
+ EXPECT_EQ(10, node[emitter->getClassName()].size());
+
+ std::shared_ptr<Emitter> decodedEmitter;
+ ASSERT_NO_THROW(decodedEmitter = std::dynamic_pointer_cast<Emitter>(
+ node.as<std::shared_ptr<Component>>()));
+
+ auto lifetimeRange = decodedEmitter->getValue<std::pair<double, double>>("LifetimeRange");
+ EXPECT_EQ(expectedLifetimeRange, lifetimeRange);
+ EXPECT_EQ(expectedMode, decodedEmitter->getValue<int>("Mode"));
+ EXPECT_EQ(expectedRate, decodedEmitter->getValue<double>("Rate"));
+ EXPECT_EQ(expectedShape->getType(), decodedEmitter->getValue<std::shared_ptr<Shape>>("Shape")->getType());
+ EXPECT_EQ("ParticleSystem", decodedEmitter->getValue<std::shared_ptr<Component>>("Target")->getName());
+ auto velocityRange = decodedEmitter->getValue<std::pair<Vector3d, Vector3d>>("VelocityRange");
+ EXPECT_TRUE(expectedVelocityRange.first.isApprox(velocityRange.first));
+ EXPECT_TRUE(expectedVelocityRange.second.isApprox(velocityRange.second));
+ EXPECT_TRUE(expectedPose.isApprox(decodedEmitter->getValue<Math::RigidTransform3d>("LocalPose")));
+}
+
+}; // namespace Particles
+}; // namespace SurgSim
diff --git a/SurgSim/Particles/UnitTests/MockObjects.cpp b/SurgSim/Particles/UnitTests/MockObjects.cpp
new file mode 100644
index 0000000..da7adca
--- /dev/null
+++ b/SurgSim/Particles/UnitTests/MockObjects.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/Particles/UnitTests/MockObjects.h"
+
+
+MockParticleSystem::MockParticleSystem(const std::string& name) :
+ SurgSim::Particles::Representation(name),
+ updateCount(0)
+{
+}
+
+bool MockParticleSystem::doUpdate(double dt)
+{
+ updateCount++;
+ return true;
+}
+
+bool MockParticleSystem::doHandleCollisions(double dt, const SurgSim::Collision::ContactMapType& collisions)
+{
+ return true;
+}
+
+MockEmitter::MockEmitter(const std::string& name) :
+ SurgSim::Particles::Emitter(name),
+ updateCount(0)
+{
+}
+
+void MockEmitter::update(double dt)
+{
+ SurgSim::Particles::Emitter::update(dt);
+ updateCount++;
+}
+
diff --git a/SurgSim/Particles/UnitTests/MockObjects.h b/SurgSim/Particles/UnitTests/MockObjects.h
new file mode 100644
index 0000000..6204da2
--- /dev/null
+++ b/SurgSim/Particles/UnitTests/MockObjects.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_PARTICLES_UNITTESTS_MOCKOBJECTS_H
+#define SURGSIM_PARTICLES_UNITTESTS_MOCKOBJECTS_H
+
+#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Particles/Emitter.h"
+#include "SurgSim/Particles/Representation.h"
+
+
+class MockParticleSystem : public SurgSim::Particles::Representation
+{
+public:
+ explicit MockParticleSystem(const std::string& name);
+ SURGSIM_CLASSNAME(MockParticleSystem);
+ int updateCount;
+
+private:
+ bool doUpdate(double dt) override;
+ bool doHandleCollisions(double dt, const SurgSim::Collision::ContactMapType& collisions) override;
+};
+
+class MockEmitter : public SurgSim::Particles::Emitter
+{
+public:
+ explicit MockEmitter(const std::string& name);
+ void update(double dt) override;
+ int updateCount;
+};
+
+#endif //SURGSIM_PARTICLES_UNITTESTS_MOCKOBJECTS_H
+
+
diff --git a/SurgSim/Particles/UnitTests/ParticlesCollisionRepresentationTests.cpp b/SurgSim/Particles/UnitTests/ParticlesCollisionRepresentationTests.cpp
new file mode 100644
index 0000000..32b24ff
--- /dev/null
+++ b/SurgSim/Particles/UnitTests/ParticlesCollisionRepresentationTests.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 <memory>
+
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/ParticlesShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/ParticlesCollisionRepresentation.h"
+#include "SurgSim/Particles/Representation.h"
+#include "SurgSim/Particles/UnitTests/MockObjects.h"
+
+namespace
+{
+const double epsilon = 1e-10;
+}
+
+namespace SurgSim
+{
+namespace Particles
+{
+
+struct ParticlesCollisionRepresentationTest : public ::testing::Test
+{
+ void SetUp()
+ {
+ m_runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ m_particleRepresentation = std::make_shared<MockParticleSystem>("ParticleRepresentation");
+ m_particleCollisionRepresentation =
+ std::make_shared<ParticlesCollisionRepresentation>("ParticlesCollisionRepresentation");
+ }
+
+ std::shared_ptr<SurgSim::Framework::Runtime> m_runtime;
+ std::shared_ptr<SurgSim::Particles::Representation> m_particleRepresentation;
+ std::shared_ptr<SurgSim::Particles::ParticlesCollisionRepresentation> m_particleCollisionRepresentation;
+};
+
+TEST_F(ParticlesCollisionRepresentationTest, InitTest)
+{
+ EXPECT_NO_THROW(ParticlesCollisionRepresentation("TestParticlesCollisionRepresentation"));
+}
+
+TEST_F(ParticlesCollisionRepresentationTest, SetGetParticlesRepresentationTest)
+{
+ EXPECT_THROW(m_particleCollisionRepresentation->getParticleRepresentation(), SurgSim::Framework::AssertionFailure);
+ ASSERT_NO_THROW(m_particleCollisionRepresentation->setParticleRepresentation(m_particleRepresentation));
+ EXPECT_EQ(m_particleRepresentation, m_particleCollisionRepresentation->getParticleRepresentation());
+}
+
+TEST_F(ParticlesCollisionRepresentationTest, SetGetParticleRadius)
+{
+ ASSERT_NO_THROW(m_particleCollisionRepresentation->setParticleRadius(4.0));
+ EXPECT_EQ(4.0, m_particleCollisionRepresentation->getParticleRadius());
+}
+
+TEST_F(ParticlesCollisionRepresentationTest, ShapeTypeTest)
+{
+ EXPECT_EQ(SurgSim::Math::SHAPE_TYPE_PARTICLES, m_particleCollisionRepresentation->getShapeType());
+}
+
+TEST_F(ParticlesCollisionRepresentationTest, SerializationTest)
+{
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*m_particleCollisionRepresentation));
+
+ std::shared_ptr<SurgSim::Particles::ParticlesCollisionRepresentation> newParticlesCollisionRepresentation;
+ ASSERT_NO_THROW(newParticlesCollisionRepresentation =
+ std::dynamic_pointer_cast<SurgSim::Particles::ParticlesCollisionRepresentation>
+ (node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+}
+
+TEST_F(ParticlesCollisionRepresentationTest, UpdateAndInitializationTest)
+{
+ m_particleRepresentation->setCollisionRepresentation(m_particleCollisionRepresentation);
+ m_particleRepresentation->setMaxParticles(100);
+ m_particleRepresentation->addParticle(Math::Vector3d::Zero(), Math::Vector3d::Zero(), 1.5);
+ ASSERT_TRUE(m_particleRepresentation->initialize(m_runtime));
+ ASSERT_TRUE(m_particleRepresentation->wakeUp());
+
+ auto shape = std::dynamic_pointer_cast<Math::ParticlesShape>(m_particleCollisionRepresentation->getShape());
+ EXPECT_EQ(0u, shape->getNumVertices());
+
+ EXPECT_TRUE(m_particleCollisionRepresentation->initialize(m_runtime));
+ EXPECT_TRUE(m_particleCollisionRepresentation->wakeUp());
+
+ EXPECT_NO_THROW(m_particleRepresentation->update(1.0));
+ EXPECT_NO_THROW(m_particleCollisionRepresentation->updateShapeData());
+ EXPECT_EQ(1u, shape->getNumVertices());
+
+ EXPECT_NO_THROW(m_particleRepresentation->update(1.0));
+ EXPECT_NO_THROW(m_particleCollisionRepresentation->updateShapeData());
+ EXPECT_EQ(0u, shape->getNumVertices());
+}
+
+};
+};
diff --git a/SurgSim/Particles/UnitTests/PointGeneratorTests.cpp b/SurgSim/Particles/UnitTests/PointGeneratorTests.cpp
new file mode 100644
index 0000000..d181adf
--- /dev/null
+++ b/SurgSim/Particles/UnitTests/PointGeneratorTests.cpp
@@ -0,0 +1,105 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Aabb.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/RandomBoxPointGenerator.h"
+#include "SurgSim/Particles/RandomMeshPointGenerator.h"
+#include "SurgSim/Particles/RandomSpherePointGenerator.h"
+
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::Geometry::DistanceEpsilon;
+using SurgSim::Math::MeshShape;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+using SurgSim::Particles::RandomBoxPointGenerator;
+using SurgSim::Particles::RandomMeshPointGenerator;
+using SurgSim::Particles::RandomSpherePointGenerator;
+
+
+TEST(PointGeneratorTest, ConstructorTest)
+{
+ ASSERT_NO_THROW(RandomBoxPointGenerator());
+ ASSERT_NO_THROW(RandomMeshPointGenerator());
+ ASSERT_NO_THROW(RandomSpherePointGenerator());
+}
+
+TEST(PointGeneratorTest, BoxPointGeneratorTest)
+{
+ auto boxShape = std::make_shared<BoxShape>(2.0, 4.0, 6.0);
+ auto aabb = SurgSim::Math::Aabbd(Vector3d(-1.0, -2.0, -3.0), Vector3d(1.0, 2.0, 3.0));
+ auto boxPointGenerator = std::make_shared<RandomBoxPointGenerator>();
+
+ auto pointInsideBox = boxPointGenerator->pointInShape(boxShape);
+ bool isInOrOnBox = ((pointInsideBox - aabb.min()).array() > -SurgSim::Math::Geometry::ScalarEpsilon).all() &&
+ ((pointInsideBox - aabb.max()).array() < SurgSim::Math::Geometry::ScalarEpsilon).all();
+ EXPECT_TRUE(isInOrOnBox) << "Point should be in or on the box " << pointInsideBox;
+ bool intersection = ((aabb.min() - pointInsideBox).array().abs() < SurgSim::Math::Geometry::ScalarEpsilon).any() ||
+ ((aabb.max() - pointInsideBox).array().abs() < SurgSim::Math::Geometry::ScalarEpsilon).any();
+ EXPECT_FALSE(intersection) << "Point should not be on surface of box " << pointInsideBox;
+
+ auto pointOnBox = boxPointGenerator->pointOnShape(boxShape);
+ isInOrOnBox = ((pointOnBox - aabb.min()).array() >= -SurgSim::Math::Geometry::ScalarEpsilon).all() &&
+ ((pointOnBox - aabb.max()).array() <= SurgSim::Math::Geometry::ScalarEpsilon).all();
+ EXPECT_TRUE(isInOrOnBox) << "Point should be in or on the box " << pointOnBox;
+ bool intersection2 = ((aabb.min() - pointOnBox).array().abs() < SurgSim::Math::Geometry::ScalarEpsilon).any() ||
+ ((aabb.max() - pointOnBox).array().abs() < SurgSim::Math::Geometry::ScalarEpsilon).any();
+ EXPECT_TRUE(intersection2) << "Point should be on surface of box " << pointOnBox;
+}
+
+TEST(PointGeneratorTest, MeshPointGeneratorTest)
+{
+ auto meshShape = std::make_shared<MeshShape>();
+ std::array<size_t, 3> triangleIds;
+
+ triangleIds[0] = meshShape->addVertex(MeshShape::VertexType(Vector3d(-1.0, 1.0, 1.0)));
+ triangleIds[1] = meshShape->addVertex(MeshShape::VertexType(Vector3d(-1.0, 5.0, 1.0)));
+ triangleIds[2] = meshShape->addVertex(MeshShape::VertexType(Vector3d(-1.0, 1.0, 5.0)));
+ meshShape->addTriangle(MeshShape::TriangleType(triangleIds));
+
+ triangleIds[0] = meshShape->addVertex(MeshShape::VertexType(Vector3d(1.0, 1.0, 1.0)));
+ triangleIds[1] = meshShape->addVertex(MeshShape::VertexType(Vector3d(1.0, 5.0, 1.0)));
+ triangleIds[2] = meshShape->addVertex(MeshShape::VertexType(Vector3d(1.0, 1.0, 5.0)));
+ size_t triangleId = meshShape->addTriangle(MeshShape::TriangleType(triangleIds));
+ meshShape->getTriangle(triangleId).isValid = false;
+
+ auto meshPointGenerator = std::make_shared<RandomMeshPointGenerator>();
+ auto pointOnMesh = meshPointGenerator->pointOnShape(meshShape);
+
+ EXPECT_NEAR(-1.0, pointOnMesh[0], DistanceEpsilon)
+ << "Point is not on a triangle, or was generated with an invalid triangle.";
+ EXPECT_LE(1.0, pointOnMesh[1]);
+ EXPECT_LE(1.0, pointOnMesh[2]);
+ EXPECT_GE(6.0, pointOnMesh[1] + pointOnMesh[2]);
+}
+
+TEST(PointGeneratorTest, SpherePointGeneratorTest)
+{
+ auto sphereShape = std::make_shared<SphereShape>(2.0);
+ auto spherePointGenerator = std::make_shared<RandomSpherePointGenerator>();
+
+ auto pointInsideSphere = spherePointGenerator->pointInShape(sphereShape);
+ EXPECT_LT(pointInsideSphere.norm(), sphereShape->getRadius());
+
+ auto pointOnSphere = spherePointGenerator->pointOnShape(sphereShape);
+ EXPECT_NEAR(sphereShape->getRadius(), pointOnSphere.norm(), DistanceEpsilon);
+}
diff --git a/SurgSim/Particles/UnitTests/RandomPointGeneratorTests.cpp b/SurgSim/Particles/UnitTests/RandomPointGeneratorTests.cpp
new file mode 100644
index 0000000..67b6a47
--- /dev/null
+++ b/SurgSim/Particles/UnitTests/RandomPointGeneratorTests.cpp
@@ -0,0 +1,53 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/BoxShape.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/RandomPointGenerator.h"
+
+using SurgSim::Math::BoxShape;
+using SurgSim::Math::MeshShape;
+using SurgSim::Math::Shape;
+using SurgSim::Math::SphereShape;
+using SurgSim::Math::Vector3d;
+using SurgSim::Particles::RandomPointGenerator;
+
+TEST(RandomPointGeneratorTest, ConstructorTest)
+{
+ ASSERT_NO_THROW(RandomPointGenerator());
+}
+
+TEST(RandomPointGeneratorTest, GenerationTest)
+{
+ auto pointGenerator = std::make_shared<RandomPointGenerator>();
+
+ auto boxShape = std::make_shared<BoxShape>(1.0, 2.0, 3.0);
+ EXPECT_NO_THROW(pointGenerator->pointInShape(boxShape));
+
+ auto sphereShape = std::make_shared<SphereShape>(6.0);
+ EXPECT_NO_THROW(pointGenerator->pointOnShape(sphereShape));
+
+ auto meshShape = std::make_shared<MeshShape>();
+ EXPECT_NO_THROW(pointGenerator->pointOnShape(meshShape));
+
+ std::shared_ptr<Shape> shape;
+ EXPECT_THROW(pointGenerator->pointOnShape(shape), SurgSim::Framework::AssertionFailure);
+}
diff --git a/SurgSim/Particles/UnitTests/RepresentationTests.cpp b/SurgSim/Particles/UnitTests/RepresentationTests.cpp
new file mode 100644
index 0000000..6da6040
--- /dev/null
+++ b/SurgSim/Particles/UnitTests/RepresentationTests.cpp
@@ -0,0 +1,147 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/Emitter.h"
+#include "SurgSim/Particles/Representation.h"
+#include "SurgSim/Particles/UnitTests/MockObjects.h"
+
+using SurgSim::Math::Vector3d;
+
+
+namespace SurgSim
+{
+namespace Particles
+{
+
+TEST(RepresentationTest, ConstructorTest)
+{
+ ASSERT_NO_THROW(MockParticleSystem representation("representation"));
+
+ MockParticleSystem representation("representation");
+ EXPECT_EQ(0u, representation.getMaxParticles());
+}
+
+TEST(RepresentationTest, SetGetMaxParticles)
+{
+ auto representation = std::make_shared<MockParticleSystem>("representation");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+
+ EXPECT_NO_THROW(representation->setMaxParticles(1));
+ EXPECT_EQ(1u, representation->getMaxParticles());
+}
+
+TEST(RepresentationTest, AddParticle)
+{
+ auto representation = std::make_shared<MockParticleSystem>("representation");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ representation->setMaxParticles(1);
+ representation->initialize(runtime);
+
+ ASSERT_TRUE(representation->addParticle(Vector3d::Ones(), Vector3d::Constant(3.0), 10));
+ ASSERT_EQ(1, representation->getParticles().unsafeGet().getNumVertices());
+
+ auto& particle = representation->getParticles().unsafeGet().getVertices()[0];
+ EXPECT_TRUE(particle.position.isApprox(Vector3d::Ones()));
+ EXPECT_TRUE(particle.data.velocity.isApprox(Vector3d::Constant(3.0)));
+ EXPECT_NEAR(10.0, particle.data.lifetime, 1e-9);
+}
+
+TEST(RepresentationTest, RemoveParticle)
+{
+ auto representation = std::make_shared<MockParticleSystem>("representation");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ representation->setMaxParticles(2);
+ representation->initialize(runtime);
+
+ ASSERT_TRUE(representation->addParticle(Vector3d::Ones(), Vector3d::Constant(3.0), 10));
+ ASSERT_TRUE(representation->addParticle(Vector3d(2.0, 3.0, 4.0), Vector3d::Zero(), 10));
+ ASSERT_EQ(2, representation->getParticles().unsafeGet().getNumVertices());
+ representation->update(1.0);
+ ASSERT_EQ(2, representation->getParticles().safeGet()->getNumVertices());
+
+ representation->removeParticle(0);
+ representation->update(1.0);
+ auto& particleVertices = representation->getParticles().safeGet()->getVertices();
+ ASSERT_EQ(1, particleVertices.size());
+ EXPECT_TRUE(particleVertices[0].position.isApprox(Vector3d(2.0, 3.0, 4.0)));
+ EXPECT_TRUE(particleVertices[0].data.velocity.isApprox(Vector3d::Zero()));
+
+ representation->removeParticle(0);
+ representation->update(1.0);
+ ASSERT_EQ(0, representation->getParticles().safeGet()->getNumVertices());
+}
+
+TEST(RepresentationTest, GetParticles)
+{
+ auto representation = std::make_shared<MockParticleSystem>("representation");
+ representation->setMaxParticles(100);
+
+ Vector3d expectedPosition = Vector3d(1.0, 2.0, 3.0);
+ Vector3d expectedVelocity = Vector3d(-4.0, 5.0, -6.0);
+ double expectedLifetime = 12.34;
+ representation->addParticle(expectedPosition, expectedVelocity, expectedLifetime);
+ ASSERT_EQ(1, representation->getParticles().unsafeGet().getNumVertices());
+
+ auto& particles = representation->getParticles().unsafeGet().getVertices();
+ EXPECT_TRUE(expectedPosition.isApprox(particles[0].position));
+ EXPECT_TRUE(expectedVelocity.isApprox(particles[0].data.velocity));
+ EXPECT_NEAR(expectedLifetime, particles[0].data.lifetime, 1e-9);
+}
+
+TEST(RepresentationTest, MaxParticles)
+{
+ auto representation = std::make_shared<MockParticleSystem>("representation");
+ representation->setMaxParticles(1);
+ EXPECT_TRUE(representation->addParticle(Vector3d::Constant(1.0), Vector3d::Zero(), 10));
+ EXPECT_FALSE(representation->addParticle(Vector3d::Constant(2.0), Vector3d::Zero(), 10));
+}
+
+TEST(RepresentationTest, Aging)
+{
+ auto representation = std::make_shared<MockParticleSystem>("representation");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ representation->setMaxParticles(100);
+ representation->initialize(runtime);
+
+ representation->addParticle(Vector3d(1.0, 1.0, 1.0), Vector3d(0.0, 0.0, 0.0), 1);
+ representation->addParticle(Vector3d(2.0, 2.0, 2.0), Vector3d(0.0, 0.0, 0.0), 2);
+ representation->addParticle(Vector3d(3.0, 3.0, 3.0), Vector3d(0.0, 0.0, 0.0), 3);
+ EXPECT_EQ(3, representation->getParticles().unsafeGet().getNumVertices());
+
+ representation->update(1.0);
+ EXPECT_EQ(2, representation->getParticles().unsafeGet().getNumVertices());
+ EXPECT_EQ(2, representation->getParticles().safeGet()->getNumVertices());
+
+ representation->update(1.0);
+ EXPECT_EQ(1, representation->getParticles().unsafeGet().getNumVertices());
+ EXPECT_EQ(1, representation->getParticles().safeGet()->getNumVertices());
+
+ representation->update(1.0);
+ EXPECT_EQ(0, representation->getParticles().unsafeGet().getNumVertices());
+ EXPECT_EQ(0, representation->getParticles().safeGet()->getNumVertices());
+
+ representation->update(1.0);
+ EXPECT_EQ(0, representation->getParticles().unsafeGet().getNumVertices());
+ EXPECT_EQ(0, representation->getParticles().safeGet()->getNumVertices());
+}
+
+}; // namespace Particles
+}; // namespace SurgSim
diff --git a/SurgSim/Particles/UnitTests/SinkTests.cpp b/SurgSim/Particles/UnitTests/SinkTests.cpp
new file mode 100644
index 0000000..84b7719
--- /dev/null
+++ b/SurgSim/Particles/UnitTests/SinkTests.cpp
@@ -0,0 +1,165 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/BasicSceneElement.h"
+#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/ObjectFactory.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Particles/ParticlesCollisionRepresentation.h"
+#include "SurgSim/Particles/Sink.h"
+#include "SurgSim/Particles/UnitTests/MockObjects.h"
+#include "SurgSim/Physics/PhysicsManager.h"
+
+
+namespace
+{
+SURGSIM_REGISTER(SurgSim::Framework::Component, MockParticleSystem, MockParticleSystem);
+}
+
+namespace SurgSim
+{
+namespace Particles
+{
+
+TEST(SinkTest, Constructor)
+{
+ std::shared_ptr<Sink> sink;
+ ASSERT_NO_THROW(sink = std::make_shared<Sink>("Sink"));
+
+ EXPECT_EQ(Framework::MANAGER_TYPE_PHYSICS, sink->getTargetManagerType());
+ EXPECT_EQ(nullptr, sink->getTarget());
+ EXPECT_EQ(nullptr, sink->getCollisionRepresentation());
+}
+
+TEST(SinkTest, SetGetCollisionRepresentation)
+{
+ auto runtime = std::make_shared<Framework::Runtime>();
+ auto collision = std::make_shared<Collision::ShapeCollisionRepresentation>("Collision");
+ auto notCollision = std::make_shared<Sink>("Not a CollisionRepresentation");
+ auto particleSystem = std::make_shared<MockParticleSystem>("ParticleSystem");
+
+ {
+ auto sink = std::make_shared<Sink>("Sink");
+ sink->setTarget(particleSystem);
+ EXPECT_THROW(sink->setCollisionRepresentation(notCollision), SurgSim::Framework::AssertionFailure);
+ EXPECT_TRUE(sink->initialize(runtime));
+ EXPECT_FALSE(sink->wakeUp()) << "Without a CollisionRepresentation, the sink should not wakup";
+ }
+ {
+ auto sink = std::make_shared<Sink>("Sink");
+ sink->setTarget(particleSystem);
+ EXPECT_NO_THROW(sink->setCollisionRepresentation(collision));
+ EXPECT_EQ(collision, sink->getCollisionRepresentation());
+ EXPECT_TRUE(sink->initialize(runtime));
+ EXPECT_TRUE(sink->wakeUp()) << "With a CollisionRepresentation, the sink should wakeup";
+ }
+}
+
+TEST(SinkTest, SetGetTarget)
+{
+ auto runtime = std::make_shared<Framework::Runtime>();
+ auto collision = std::make_shared<Collision::ShapeCollisionRepresentation>("Collision");
+ auto particleSystem = std::make_shared<MockParticleSystem>("ParticleSystem");
+ auto notParticleSystem = std::make_shared<Sink>("Not a ParticleSystem");
+
+ {
+ auto sink = std::make_shared<Sink>("Sink");
+ sink->setCollisionRepresentation(collision);
+ EXPECT_THROW(sink->setTarget(notParticleSystem), SurgSim::Framework::AssertionFailure);
+ EXPECT_TRUE(sink->initialize(runtime));
+ EXPECT_FALSE(sink->wakeUp()) << "Without a target, the sink should not wakup";
+ }
+ {
+ auto sink = std::make_shared<Sink>("Sink");
+ sink->setCollisionRepresentation(collision);
+ EXPECT_NO_THROW(sink->setTarget(particleSystem));
+ EXPECT_EQ(particleSystem, sink->getTarget());
+ EXPECT_TRUE(sink->initialize(runtime));
+ EXPECT_TRUE(sink->wakeUp()) << "With a target, the sink should wakeup";
+ }
+}
+
+TEST(SinkTest, Update)
+{
+ auto runtime = std::make_shared<Framework::Runtime>("config.txt");
+ runtime->addManager(std::make_shared<Physics::PhysicsManager>());
+
+ auto element = std::make_shared<Framework::BasicSceneElement>("Element");
+
+ auto particleCollision = std::make_shared<ParticlesCollisionRepresentation>("Particle Collision");
+ particleCollision->setParticleRadius(0.01);
+ element->addComponent(particleCollision);
+
+ auto particleSystem = std::make_shared<MockParticleSystem>("ParticleSystem");
+ particleSystem->setMaxParticles(10);
+ particleSystem->setCollisionRepresentation(particleCollision);
+ element->addComponent(particleSystem);
+
+ auto mesh = std::make_shared<Math::MeshShape>();
+ mesh->load("Geometry/Cube.ply");
+
+ auto sinkCollision = std::make_shared<Collision::ShapeCollisionRepresentation>("Sink Collision");
+ sinkCollision->setShape(mesh);
+ element->addComponent(sinkCollision);
+
+ auto sink = std::make_shared<Sink>("Sink");
+ sink->setTarget(particleSystem);
+ sink->setCollisionRepresentation(sinkCollision);
+ element->addComponent(sink);
+
+ particleSystem->addParticle(Math::Vector3d(0.0, 1.009, 0.0), Math::Vector3d::Zero(), 10);
+ runtime->getScene()->addSceneElement(element);
+ EXPECT_EQ(1, particleSystem->getParticles().safeGet()->getNumVertices());
+
+ runtime->start(true);
+ runtime->step();
+ runtime->step();
+ boost::this_thread::sleep(boost::posix_time::milliseconds(50));
+ runtime->stop();
+
+ EXPECT_EQ(0, particleSystem->getParticles().safeGet()->getNumVertices());
+}
+
+TEST(SinkTest, Serialization)
+{
+ auto sink = std::make_shared<Sink>("Sink");
+ EXPECT_EQ("SurgSim::Particles::Sink", sink->getClassName());
+
+ std::shared_ptr<Framework::Component> expectedTarget = std::make_shared<MockParticleSystem>("ParticleSystem");
+ std::shared_ptr<Framework::Component> expectedCollision =
+ std::make_shared<Collision::ShapeCollisionRepresentation>("Collision");
+
+ EXPECT_NO_THROW(sink->setValue("CollisionRepresentation", expectedCollision));
+ EXPECT_NO_THROW(sink->setValue("Target", expectedTarget));
+
+ YAML::Node node;
+ EXPECT_NO_THROW(node = YAML::convert<Framework::Component>::encode(*sink));
+ EXPECT_TRUE(node.IsMap());
+
+ std::shared_ptr<Sink> decodedSink;
+ ASSERT_NO_THROW(decodedSink = std::dynamic_pointer_cast<Sink>(node.as<std::shared_ptr<Framework::Component>>()));
+
+ EXPECT_EQ("ParticleSystem", decodedSink->getValue<std::shared_ptr<Framework::Component>>("Target")->getName());
+ EXPECT_EQ("Collision",
+ decodedSink->getValue<std::shared_ptr<Framework::Component>>("CollisionRepresentation")->getName());
+}
+
+}; // namespace Particles
+}; // namespace SurgSim
diff --git a/SurgSim/Particles/UnitTests/SphRepresentationTests.cpp b/SurgSim/Particles/UnitTests/SphRepresentationTests.cpp
new file mode 100644
index 0000000..1860e59
--- /dev/null
+++ b/SurgSim/Particles/UnitTests/SphRepresentationTests.cpp
@@ -0,0 +1,327 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Particles/SphRepresentation.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Particles
+{
+
+TEST(SphRepresentationTest, ConstructorTest)
+{
+ ASSERT_NO_THROW(std::make_shared<SphRepresentation>("representation"));
+}
+
+TEST(SphRepresentationTest, SetGetTest)
+{
+ auto sph = std::make_shared<SphRepresentation>("representation");
+
+ EXPECT_DOUBLE_EQ(0.0, sph->getMassPerParticle());
+ EXPECT_THROW(sph->setMassPerParticle(0.0), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(sph->setMassPerParticle(-1.0), SurgSim::Framework::AssertionFailure);
+ sph->setMassPerParticle(0.02);
+ EXPECT_DOUBLE_EQ(0.02, sph->getMassPerParticle());
+
+ EXPECT_DOUBLE_EQ(0.0, sph->getDensity());
+ EXPECT_THROW(sph->setDensity(0.0), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(sph->setDensity(-1.0), SurgSim::Framework::AssertionFailure);
+ sph->setDensity(0.03);
+ EXPECT_DOUBLE_EQ(0.03, sph->getDensity());
+
+ EXPECT_DOUBLE_EQ(0.0, sph->getGasStiffness());
+ EXPECT_THROW(sph->setGasStiffness(0.0), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(sph->setGasStiffness(-1.0), SurgSim::Framework::AssertionFailure);
+ sph->setGasStiffness(0.04);
+ EXPECT_DOUBLE_EQ(0.04, sph->getGasStiffness());
+
+ EXPECT_DOUBLE_EQ(0.0, sph->getSurfaceTension());
+ EXPECT_NO_THROW(sph->setSurfaceTension(0.0));
+ EXPECT_THROW(sph->setSurfaceTension(-1.0), SurgSim::Framework::AssertionFailure);
+ sph->setSurfaceTension(0.04);
+ EXPECT_DOUBLE_EQ(0.04, sph->getSurfaceTension());
+
+ EXPECT_TRUE(sph->getGravity().isApprox(SurgSim::Math::Vector3d(0.0, -9.81, 0.0)));
+ sph->setGravity(SurgSim::Math::Vector3d::Ones() * 0.56);
+ EXPECT_TRUE(sph->getGravity().isApprox(SurgSim::Math::Vector3d::Ones() * 0.56));
+
+ EXPECT_DOUBLE_EQ(0.0, sph->getViscosity());
+ EXPECT_NO_THROW(sph->setViscosity(0.0));
+ EXPECT_THROW(sph->setViscosity(-1.0), SurgSim::Framework::AssertionFailure);
+ sph->setViscosity(0.04);
+ EXPECT_DOUBLE_EQ(0.04, sph->getViscosity());
+
+ EXPECT_DOUBLE_EQ(0.0, sph->getStiffness());
+ EXPECT_NO_THROW(sph->setStiffness(0.0));
+ sph->setStiffness(0.04);
+ EXPECT_DOUBLE_EQ(0.04, sph->getStiffness());
+
+ EXPECT_DOUBLE_EQ(0.0, sph->getDamping());
+ EXPECT_NO_THROW(sph->setDamping(0.0));
+ sph->setDamping(0.04);
+ EXPECT_DOUBLE_EQ(0.04, sph->getDamping());
+
+ EXPECT_DOUBLE_EQ(0.0, sph->getFriction());
+ EXPECT_NO_THROW(sph->setFriction(0.0));
+ sph->setFriction(0.04);
+ EXPECT_DOUBLE_EQ(0.04, sph->getFriction());
+
+ EXPECT_DOUBLE_EQ(0.0, sph->getKernelSupport());
+ EXPECT_THROW(sph->setKernelSupport(0.0), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(sph->setKernelSupport(-1.0), SurgSim::Framework::AssertionFailure);
+ sph->setKernelSupport(0.04);
+ EXPECT_DOUBLE_EQ(0.04, sph->getKernelSupport());
+}
+
+TEST(SphRepresentationTest, DoInitializeTest)
+{
+ {
+ SCOPED_TRACE("Bad Mass Per Particle");
+
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ auto sph = std::make_shared<SphRepresentation>("representation");
+ EXPECT_THROW(sph->initialize(runtime), SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("Bad density reference");
+
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ auto sph = std::make_shared<SphRepresentation>("representation");
+ sph->setMassPerParticle(0.02);
+ EXPECT_THROW(sph->initialize(runtime), SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("Bad gas stiffness");
+
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ auto sph = std::make_shared<SphRepresentation>("representation");
+ sph->setMassPerParticle(0.02);
+ sph->setDensity(0.02);
+ EXPECT_THROW(sph->initialize(runtime), SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("Bad kernel support");
+
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ auto sph = std::make_shared<SphRepresentation>("representation");
+ sph->setMassPerParticle(0.02);
+ sph->setDensity(0.02);
+ sph->setGasStiffness(0.02);
+ EXPECT_THROW(sph->initialize(runtime), SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("All set");
+
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ auto sph = std::make_shared<SphRepresentation>("representation");
+ sph->setMassPerParticle(0.02);
+ sph->setDensity(0.02);
+ sph->setGasStiffness(0.02);
+ sph->setKernelSupport(0.02);
+ EXPECT_NO_THROW(sph->initialize(runtime));
+ }
+}
+
+TEST(SphRepresentationTest, DoUpdate1ParticleTest)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ auto sph = std::make_shared<SphRepresentation>("representation");
+ double dt = 1e-3; // 1ms
+
+ sph->setMaxParticles(1);
+ sph->setMassPerParticle(0.02);
+ sph->setDensity(1000.0);
+ sph->setGasStiffness(3.0);
+ sph->setKernelSupport(0.01);
+ sph->setViscosity(0.01);
+ sph->setSurfaceTension(0.0);
+
+ sph->initialize(runtime);
+
+ sph->addParticle(Math::Vector3d::Zero(), Math::Vector3d::Zero(), 10);
+ EXPECT_EQ(1u, sph->getParticles().unsafeGet().getNumVertices());
+
+ EXPECT_NO_THROW(sph->update(dt));
+ auto particles = sph->getParticles().safeGet()->getVertices();
+ EXPECT_EQ(1u, particles.size());
+ EXPECT_DOUBLE_EQ(0.0, particles[0].position[0]);
+ EXPECT_GT(0.0, particles[0].position[1]);
+ EXPECT_DOUBLE_EQ(0.0, particles[0].position[2]);
+
+ EXPECT_DOUBLE_EQ(0.0, particles[0].data.velocity[0]);
+ EXPECT_GT(0.0, particles[0].data.velocity[1]);
+ EXPECT_DOUBLE_EQ(0.0, particles[0].data.velocity[2]);
+}
+
+namespace
+{
+/// Set up a Unit Test where 2 particles interact with a radius R (h/2) of null interaction
+/// If they are closer than R, they repel each other
+/// If they are at distance R from each other, they are in equilibrium
+/// If they are further than R, the attract each other
+std::shared_ptr<SphRepresentation> set2ParticlesInteracting(double h, double distance)
+{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ auto sph = std::make_shared<SphRepresentation>("representation");
+ double dt = 1e-3; // 1ms
+
+ sph->setMaxParticles(2);
+ sph->setMassPerParticle(0.02); // 50 particles for 0.001 m3 at 1000Kg.m-3
+ // If you note R the radius of 1 particle, then (50 * 4/3.PI.R^3) = 0.001 => R = 0.01683890300960629672761734255721m
+ sph->setDensity(1000.0);
+ sph->setGasStiffness(3.0);
+ sph->setKernelSupport(h);
+ sph->setViscosity(0.01);
+ sph->setSurfaceTension(0.0);
+
+ sph->initialize(runtime);
+
+ sph->addParticle(Math::Vector3d::Zero(), Math::Vector3d::Zero(), 10);
+ sph->addParticle(Math::Vector3d(distance, 0.0, 0.0), Math::Vector3d::Zero(), 10);
+ EXPECT_EQ(2u, sph->getParticles().unsafeGet().getNumVertices());
+
+ EXPECT_NO_THROW(sph->update(dt));
+
+ EXPECT_EQ(2u, sph->getParticles().unsafeGet().getNumVertices());
+ size_t index = 0;
+ for (auto particle : sph->getParticles().unsafeGet().getVertices())
+ {
+ std::string scope = "Particle "+boost::to_string(index);
+ SCOPED_TRACE(scope);
+ EXPECT_LT(particle.position[1], 0.0);
+ EXPECT_DOUBLE_EQ(0.0, particle.position[2]);
+
+ EXPECT_GT(0.0, particle.data.velocity[1]);
+ EXPECT_DOUBLE_EQ(0.0, particle.data.velocity[2]);
+ index++;
+ }
+
+ return sph;
+}
+}; // namespace anonymous
+
+TEST(SphRepresentationTest, DoUpdate2ParticlesNotInteractingTest)
+{
+ // If you note R the radius of 1 particle, then a volume of 1l covered by 50 particles requires:
+ // (50 * 4/3.PI.R^3) = 0.001 => R = 0.01683890300960629672761734255721m
+ // Let's take a smooth kernel support of 2R, anything above that distance is not interacting
+ double h = 2.0 * 0.01683890300960629672761734255721;
+ double distance = 10.0 * h; // Far from their radius of influence
+ auto sph = set2ParticlesInteracting(h, distance);
+
+ auto& particles = sph->getParticles().unsafeGet().getVertices();
+ EXPECT_DOUBLE_EQ(particles[0].position[1], particles[1].position[1]);
+ EXPECT_DOUBLE_EQ(particles[0].data.velocity[1], particles[1].data.velocity[1]);
+ double finalDistance = (particles[0].position - particles[1].position).norm();
+ EXPECT_DOUBLE_EQ(distance, finalDistance);
+}
+
+TEST(SphRepresentationTest, DoUpdate2ParticlesAttractingTest)
+{
+ // If you note R the radius of 1 particle, then a volume of 1l covered by 50 particles requires:
+ // (50 * 4/3.PI.R^3) = 0.001 => R = 0.01683890300960629672761734255721m
+ // Let's take a smooth kernel support of 2R, anything above that distance is not interacting
+ double h = 2.0 * 0.01683890300960629672761734255721;
+ double distance = h * 3.0 / 4.0;
+ auto sph = set2ParticlesInteracting(h, distance);
+
+ auto& particles = sph->getParticles().unsafeGet().getVertices();
+ EXPECT_DOUBLE_EQ(particles[0].position[1], particles[1].position[1]);
+ EXPECT_DOUBLE_EQ(particles[0].data.velocity[1], particles[1].data.velocity[1]);
+ double finalDistance = (particles[0].position - particles[1].position).norm();
+ EXPECT_GT(distance, finalDistance);
+}
+
+TEST(SphRepresentationTest, DoUpdate2ParticlesRetractingTest)
+{
+ // If you note R the radius of 1 particle, then a volume of 1l covered by 50 particles requires:
+ // (50 * 4/3.PI.R^3) = 0.001 => R = 0.01683890300960629672761734255721m
+ // Let's take a smooth kernel support of 2R, anything above that distance is not interacting
+ double h = 2.0 * 0.01683890300960629672761734255721;
+ double distance = h * 1.0 / 4.0;
+ auto sph = set2ParticlesInteracting(h, distance);
+
+ auto& particles = sph->getParticles().unsafeGet().getVertices();
+ EXPECT_DOUBLE_EQ(particles[0].position[1], particles[1].position[1]);
+ EXPECT_DOUBLE_EQ(particles[0].data.velocity[1], particles[1].data.velocity[1]);
+ double finalDistance = (particles[0].position - particles[1].position).norm();
+ EXPECT_LT(distance, finalDistance);
+}
+
+TEST(SphRepresentationTest, DoUpdate2ParticlesInEquilibriumTest)
+{
+ // If you note R the radius of 1 particle, then a volume of 1l covered by 50 particles requires:
+ // (50 * 4/3.PI.R^3) = 0.001 => R = 0.01683890300960629672761734255721m
+ // Let's take a smooth kernel support of 2R, anything above that distance is not interacting
+ double h = 2.0 * 0.01683890300960629672761734255721;
+ double distance = h / 2.0;
+ auto sph = set2ParticlesInteracting(h, distance);
+
+ auto& particles = sph->getParticles().unsafeGet().getVertices();
+ EXPECT_DOUBLE_EQ(particles[0].position[1], particles[1].position[1]);
+ EXPECT_DOUBLE_EQ(particles[0].data.velocity[1], particles[1].data.velocity[1]);
+ double finalDistance = (particles[0].position - particles[1].position).norm();
+ EXPECT_NEAR(distance, finalDistance, pow(h, 2));
+}
+
+TEST(SphRepresentationTest, SerializationTest)
+{
+ auto sph = std::make_shared<SphRepresentation>("TestSphRepresentation");
+ sph->setDensity(1.1);
+ sph->setGasStiffness(2.2);
+ sph->setGravity(SurgSim::Math::Vector3d::Ones());
+ sph->setKernelSupport(3.3);
+ sph->setMassPerParticle(4.4);
+ sph->setMaxParticles(5);
+ sph->setSurfaceTension(10.1);
+ sph->setViscosity(11.11);
+ sph->setStiffness(12.12);
+ sph->setDamping(13.13);
+ sph->setFriction(0.14);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*sph));
+
+ std::shared_ptr<SphRepresentation> newRepresentation;
+ EXPECT_NO_THROW(newRepresentation =
+ std::dynamic_pointer_cast<SphRepresentation>(node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
+
+ EXPECT_DOUBLE_EQ(sph->getDensity(), newRepresentation->getValue<double>("Density"));
+ EXPECT_DOUBLE_EQ(sph->getGasStiffness(), newRepresentation->getValue<double>("GasStiffness"));
+ EXPECT_TRUE(sph->getGravity().isApprox(newRepresentation->getValue<SurgSim::Math::Vector3d>("Gravity")));
+ EXPECT_DOUBLE_EQ(sph->getKernelSupport(), newRepresentation->getValue<double>("KernelSupport"));
+ EXPECT_DOUBLE_EQ(sph->getMassPerParticle(), newRepresentation->getValue<double>("MassPerParticle"));
+ EXPECT_EQ(sph->getMaxParticles(), newRepresentation->getValue<size_t>("MaxParticles"));
+ EXPECT_DOUBLE_EQ(sph->getSurfaceTension(), newRepresentation->getValue<double>("SurfaceTension"));
+ EXPECT_DOUBLE_EQ(sph->getViscosity(), newRepresentation->getValue<double>("Viscosity"));
+ EXPECT_DOUBLE_EQ(sph->getStiffness(), newRepresentation->getValue<double>("Stiffness"));
+ EXPECT_DOUBLE_EQ(sph->getDamping(), newRepresentation->getValue<double>("Damping"));
+ EXPECT_DOUBLE_EQ(sph->getFriction(), newRepresentation->getValue<double>("Friction"));
+}
+
+}; // namespace Particles
+}; // namespace SurgSim
diff --git a/SurgSim/Particles/UnitTests/config.txt.in b/SurgSim/Particles/UnitTests/config.txt.in
new file mode 100644
index 0000000..76e073b
--- /dev/null
+++ b/SurgSim/Particles/UnitTests/config.txt.in
@@ -0,0 +1,2 @@
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data/
+
diff --git a/SurgSim/Physics/BuildMlcp.cpp b/SurgSim/Physics/BuildMlcp.cpp
index a524f4b..476f8eb 100644
--- a/SurgSim/Physics/BuildMlcp.cpp
+++ b/SurgSim/Physics/BuildMlcp.cpp
@@ -37,7 +37,7 @@ BuildMlcp::~BuildMlcp()
{}
std::shared_ptr<PhysicsManagerState>
- BuildMlcp::doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+BuildMlcp::doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
{
// Copy state to new state
std::shared_ptr<PhysicsManagerState> result = state;
@@ -45,12 +45,10 @@ std::shared_ptr<PhysicsManagerState>
MlcpMapping<Constraint> constraintsMapping;
size_t numAtomicConstraint = 0;
- size_t numConstraint = 0;
size_t numDof = 0;
- // Calculate numAtomicConstraint and numConstraint
+ // Calculate numAtomicConstraint
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));
@@ -59,7 +57,7 @@ std::shared_ptr<PhysicsManagerState>
result->setConstraintsMapping(constraintsMapping);
// Calculate numDof size
- auto const activeRepresentations = result->getActiveRepresentations();
+ const auto& activeRepresentations = result->getActiveRepresentations();
for (auto it = activeRepresentations.cbegin(); it != activeRepresentations.cend(); it++)
{
representationsMapping.setValue((*it).get(), numDof);
@@ -68,23 +66,16 @@ std::shared_ptr<PhysicsManagerState>
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().A.setZero(numAtomicConstraint, numAtomicConstraint);
+ result->getMlcpProblem().b.setZero(numAtomicConstraint);
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().CHt.setZero(numDof, numAtomicConstraint);
+ result->getMlcpProblem().mu.setZero(numAtomicConstraint);
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();
+ result->getMlcpSolution().dofCorrection.setZero(numDof);
+ result->getMlcpSolution().x.setZero(numAtomicConstraint);
// Fill up the Mlcp problem
for (auto it = activeConstraints.begin(); it != activeConstraints.end(); it++)
diff --git a/SurgSim/Physics/BuildMlcp.h b/SurgSim/Physics/BuildMlcp.h
index 698c8a6..20aa634 100644
--- a/SurgSim/Physics/BuildMlcp.h
+++ b/SurgSim/Physics/BuildMlcp.h
@@ -16,6 +16,7 @@
#ifndef SURGSIM_PHYSICS_BUILDMLCP_H
#define SURGSIM_PHYSICS_BUILDMLCP_H
+#include "SurgSim/Framework/Macros.h"
#include "SurgSim/Physics/Computation.h"
namespace SurgSim
@@ -31,13 +32,15 @@ public:
/// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
explicit BuildMlcp(bool doCopyState = false);
+ SURGSIM_CLASSNAME(SurgSim::Physics::BuildMlcp);
+
/// Destructor
virtual ~BuildMlcp();
protected:
/// Override doUpdate from superclass
- virtual std::shared_ptr<PhysicsManagerState>
- doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state) override;
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+ override;
};
}; // namespace Physics
diff --git a/SurgSim/Physics/CMakeLists.txt b/SurgSim/Physics/CMakeLists.txt
index 751cf0c..d00bd8d 100644
--- a/SurgSim/Physics/CMakeLists.txt
+++ b/SurgSim/Physics/CMakeLists.txt
@@ -1,5 +1,5 @@
# This file is a part of the OpenSurgSim project.
-# Copyright 2012-2013, SimQuest Solutions Inc.
+# Copyright 2012-2016, SimQuest Solutions Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -16,143 +16,195 @@
set(SURGSIM_PHYSICS_SOURCES
BuildMlcp.cpp
+ CcdCollision.cpp
+ CcdCollisionLoop.cpp
Computation.cpp
+ ComputationGroup.cpp
Constraint.cpp
ConstraintComponent.cpp
ConstraintData.cpp
ConstraintImplementation.cpp
ConstraintImplementationFactory.cpp
ContactConstraintGeneration.cpp
+ ContactFiltering.cpp
DcdCollision.cpp
DeformableCollisionRepresentation.cpp
DeformableRepresentation.cpp
+ Fem1D.cpp
Fem1DElementBeam.cpp
+ Fem1DLocalization.cpp
Fem1DPlyReaderDelegate.cpp
Fem1DRepresentation.cpp
- Fem1DRepresentationLocalization.cpp
+ Fem2D.cpp
Fem2DElementTriangle.cpp
+ Fem2DLocalization.cpp
Fem2DPlyReaderDelegate.cpp
Fem2DRepresentation.cpp
- Fem2DRepresentationLocalization.cpp
+ Fem3D.cpp
Fem3DElementCorotationalTetrahedron.cpp
Fem3DElementCube.cpp
Fem3DElementTetrahedron.cpp
+ Fem3DLocalization.cpp
Fem3DPlyReaderDelegate.cpp
Fem3DRepresentation.cpp
- Fem3DRepresentationBilateral3D.cpp
- Fem3DRepresentationContact.cpp
- Fem3DRepresentationLocalization.cpp
+ FemConstraintFixedPoint.cpp
+ FemConstraintFixedRotationVector.cpp
+ FemConstraintFrictionalSliding.cpp
+ FemConstraintFrictionlessContact.cpp
+ FemConstraintFrictionlessSliding.cpp
FemElement.cpp
+ FemLocalization.cpp
FemPlyReaderDelegate.cpp
FemRepresentation.cpp
- FemRepresentationParameters.cpp
+ FixedConstraintFixedPoint.cpp
+ FixedConstraintFixedRotationVector.cpp
+ FixedConstraintFrictionlessContact.cpp
FixedRepresentation.cpp
- FixedRepresentationBilateral3D.cpp
- FixedRepresentationContact.cpp
FreeMotion.cpp
LinearSpring.cpp
Localization.cpp
+ MassSpringConstraintFixedPoint.cpp
+ MassSpringConstraintFrictionlessContact.cpp
+ MassSpringLocalization.cpp
MassSpringRepresentation.cpp
- MassSpringRepresentationContact.cpp
- MassSpringRepresentationLocalization.cpp
MlcpPhysicsProblem.cpp
+ ParticleCollisionResponse.cpp
PhysicsConvert.cpp
PhysicsManager.cpp
PhysicsManagerState.cpp
PostUpdate.cpp
+ PrepareCollisionPairs.cpp
PreUpdate.cpp
PushResults.cpp
Representation.cpp
RigidCollisionRepresentation.cpp
+ RigidConstraintFixedPoint.cpp
+ RigidConstraintFixedRotationVector.cpp
+ RigidConstraintFrictionlessContact.cpp
+ RigidLocalization.cpp
RigidRepresentation.cpp
RigidRepresentationBase.cpp
- RigidRepresentationBilateral3D.cpp
- RigidRepresentationContact.cpp
- RigidRepresentationLocalization.cpp
- RigidRepresentationState.cpp
+ RigidState.cpp
+ RotationVectorConstraint.cpp
+ SlidingConstraint.cpp
+ SlidingConstraintData.cpp
SolveMlcp.cpp
+ Spring.cpp
+ UpdateCcdData.cpp
+ UpdateCollisionData.cpp
UpdateCollisionRepresentations.cpp
+ UpdateDcdData.cpp
VirtualToolCoupler.cpp
)
set(SURGSIM_PHYSICS_HEADERS
BuildMlcp.h
+ CcdCollision.h
+ CcdCollisionLoop.h
Computation.h
+ ComputationGroup.h
Constraint.h
ConstraintComponent.h
ConstraintData.h
ConstraintImplementation.h
ConstraintImplementationFactory.h
+ ConstraintType.h
ContactConstraintData.h
ContactConstraintGeneration.h
+ ContactFiltering.h
DcdCollision.h
DeformableCollisionRepresentation.h
DeformableRepresentation.h
+ Fem.h
+ Fem-inl.h
+ Fem1D.h
Fem1DElementBeam.h
+ Fem1DLocalization.h
Fem1DPlyReaderDelegate.h
Fem1DRepresentation.h
- Fem1DRepresentationLocalization.h
+ Fem2D.h
Fem2DElementTriangle.h
+ Fem2DLocalization.h
Fem2DPlyReaderDelegate.h
Fem2DRepresentation.h
- Fem2DRepresentationLocalization.h
+ Fem3D.h
Fem3DElementCorotationalTetrahedron.h
Fem3DElementCube.h
Fem3DElementTetrahedron.h
+ Fem3DLocalization.h
Fem3DPlyReaderDelegate.h
Fem3DRepresentation.h
- Fem3DRepresentationBilateral3D.h
- Fem3DRepresentationContact.h
- Fem3DRepresentationLocalization.h
+ FemConstraintFixedPoint.h
+ FemConstraintFixedRotationVector.h
+ FemConstraintFrictionalSliding.h
+ FemConstraintFrictionlessContact.h
+ FemConstraintFrictionlessSliding.h
FemElement.h
+ FemElement-inl.h
+ FemElementStructs.h
+ FemLocalization.h
FemPlyReaderDelegate.h
FemRepresentation.h
- FemRepresentationParameters.h
+ FixedConstraintFixedPoint.h
+ FixedConstraintFixedRotationVector.h
+ FixedConstraintFrictionlessContact.h
FixedRepresentation.h
- FixedRepresentationBilateral3D.h
- FixedRepresentationContact.h
- FixedRepresentationLocalization.h
FreeMotion.h
LinearSpring.h
Localization.h
Mass.h
+ MassSpringConstraintFixedPoint.h
+ MassSpringConstraintFrictionlessContact.h
+ MassSpringLocalization.h
MassSpringRepresentation.h
- MassSpringRepresentationContact.h
- MassSpringRepresentationLocalization.h
MlcpMapping.h
MlcpPhysicsProblem.h
- MlcpPhysicsProblem-inl.h
MlcpPhysicsSolution.h
+ ParticleCollisionResponse.h
PhysicsConvert.h
PhysicsManager.h
PhysicsManagerState.h
PostUpdate.h
+ PrepareCollisionPairs.h
PreUpdate.h
PushResults.h
Representation.h
RigidCollisionRepresentation.h
+ RigidConstraintFixedPoint.h
+ RigidConstraintFixedRotationVector.h
+ RigidConstraintFrictionlessContact.h
+ RigidLocalization.h
RigidRepresentation.h
RigidRepresentationBase.h
RigidRepresentationBase-inl.h
- RigidRepresentationBilateral3D.h
- RigidRepresentationContact.h
- RigidRepresentationLocalization.h
- RigidRepresentationState.h
+ RigidState.h
+ RotationVectorConstraint.h
+ RotationVectorConstraintData.h
+ SlidingConstraint.h
+ SlidingConstraintData.h
SolveMlcp.h
Spring.h
+ UpdateCcdData.h
+ UpdateCollisionData.h
UpdateCollisionRepresentations.h
+ UpdateDcdData.h
VirtualToolCoupler.h
)
+surgsim_create_library_header(Physics.h "${SURGSIM_PHYSICS_HEADERS}")
surgsim_add_library(
SurgSimPhysics
"${SURGSIM_PHYSICS_SOURCES}"
"${SURGSIM_PHYSICS_HEADERS}"
- "SurgSim/Physics"
)
SET(LIBS
SurgSimCollision
+ SurgSimDataStructures
+ SurgSimFramework
+ SurgSimInput
+ SurgSimMath
+ SurgSimParticles
)
target_link_libraries(SurgSimPhysics ${LIBS})
diff --git a/SurgSim/Physics/CcdCollision.cpp b/SurgSim/Physics/CcdCollision.cpp
new file mode 100644
index 0000000..a58453b
--- /dev/null
+++ b/SurgSim/Physics/CcdCollision.cpp
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Collision/CollisionPair.h"
+#include "SurgSim/Collision/ContactCalculation.h"
+#include "SurgSim/Collision/CcdDcdCollision.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/ThreadPool.h"
+#include "SurgSim/Physics/CcdCollision.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+
+using SurgSim::Collision::CollisionPair;
+using SurgSim::Collision::ContactCalculation;
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+CcdCollision::CcdCollision(bool doCopyState) :
+ Computation(doCopyState)
+{
+}
+
+CcdCollision::~CcdCollision()
+{
+}
+
+std::shared_ptr<PhysicsManagerState> CcdCollision::doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ std::shared_ptr<PhysicsManagerState> result = state;
+ auto threadPool = Framework::Runtime::getThreadPool();
+ std::vector<std::future<void>> tasks;
+
+ const auto& calculations = ContactCalculation::getCcdContactTable();
+
+ for (auto& pair : result->getCollisionPairs())
+ {
+ if (pair->getType() == Collision::COLLISION_DETECTION_TYPE_CONTINUOUS)
+ {
+ tasks.push_back(threadPool->enqueue<void>([&]()
+ {
+ calculations[pair->getFirst()->getShapeType()]
+ [pair->getSecond()->getShapeType()]->calculateContact(pair);
+ }));
+ }
+ }
+
+ std::for_each(tasks.begin(), tasks.end(), [](std::future<void>& p){p.get();});
+
+ return result;
+}
+
+}; // Physics
+}; // SurgSim
+
diff --git a/SurgSim/Physics/CcdCollision.h b/SurgSim/Physics/CcdCollision.h
new file mode 100644
index 0000000..8b44e4b
--- /dev/null
+++ b/SurgSim/Physics/CcdCollision.h
@@ -0,0 +1,69 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_CCDCOLLISION_H
+#define SURGSIM_PHYSICS_CCDCOLLISION_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 When a new ContactCalculation type gets implemented, the type needs to be registered with the table
+/// inside of ContactCalculation
+class CcdCollision : public Computation
+{
+public:
+
+ /// Constructor
+ /// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
+ explicit CcdCollision(bool doCopyState = false);
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::CcdCollision);
+
+ /// Destructor
+ virtual ~CcdCollision();
+
+protected:
+
+ /// Executes the update operation, overridden from Computation.
+ /// \param dt The time passed.
+ /// \param state The PhysicsManagerState from previous computation.
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+ override;
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif // SURGSIM_PHYSICS_CCDCOLLISION_H
diff --git a/SurgSim/Physics/CcdCollisionLoop.cpp b/SurgSim/Physics/CcdCollisionLoop.cpp
new file mode 100644
index 0000000..20711b6
--- /dev/null
+++ b/SurgSim/Physics/CcdCollisionLoop.cpp
@@ -0,0 +1,228 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/CollisionPair.h"
+#include "SurgSim/Physics/BuildMlcp.h"
+#include "SurgSim/Physics/CcdCollision.h"
+#include "SurgSim/Physics/CcdCollisionLoop.h"
+#include "SurgSim/Physics/ContactConstraintGeneration.h"
+#include "SurgSim/Physics/ContactFiltering.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/PushResults.h"
+#include "SurgSim/Physics/SolveMlcp.h"
+#include "SurgSim/Physics/UpdateCcdData.h"
+
+#include <unordered_set>
+#include <limits>
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+CcdCollisionLoop::CcdCollisionLoop(bool copyState) :
+ Computation(copyState),
+ m_ccdCollision(new CcdCollision(copyState)),
+ m_updateCcdData(new UpdateCcdData(copyState)),
+ m_contactFilter(new ContactFiltering(copyState)),
+ m_constraintGeneration(new ContactConstraintGeneration(copyState)),
+ m_buildMlcp(new BuildMlcp(copyState)),
+ m_solveMlcp(new SolveMlcp(copyState)),
+ m_pushResults(new PushResults(copyState)),
+ m_maxIterations(20),
+ m_epsilonFactor(100),
+ m_logger(SurgSim::Framework::Logger::getLogger("Physics/CCDCollisionLoop"))
+{
+}
+
+CcdCollisionLoop::~CcdCollisionLoop()
+{
+
+}
+
+std::shared_ptr<PhysicsManagerState> CcdCollisionLoop::doUpdate(const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ auto ccdState = state;
+
+ auto& collisionPairs = state->getCollisionPairs();
+ std::vector<std::shared_ptr<Collision::CollisionPair>> ccdPairs;
+ ccdPairs.reserve(collisionPairs.size());
+
+ std::copy_if(collisionPairs.cbegin(), collisionPairs.cend(), std::back_inserter(ccdPairs),
+ [](const std::shared_ptr<Collision::CollisionPair>& p)
+ {
+ return p->getType() == Collision::COLLISION_DETECTION_TYPE_CONTINUOUS;
+ });
+
+ double timeOfImpact = 0.0;
+ double localTimeOfImpact = 0.0;
+ std::vector<std::list<std::shared_ptr<Collision::Contact>>> oldContacts;
+
+ bool executedOnce = false;
+ size_t iterations = 0;
+ for (; iterations < m_maxIterations; ++iterations)
+ {
+ double epsilon = 1.0 / ((1 - timeOfImpact) * m_epsilonFactor);
+
+ ccdState = m_updateCcdData->update(localTimeOfImpact, ccdState);
+ ccdState = m_ccdCollision->update(dt, ccdState);
+ ccdState = m_contactFilter->update(dt, ccdState);
+
+ if (m_logger->getThreshold() <= SurgSim::Framework::LOG_LEVEL_DEBUG)
+ {
+ printContacts(ccdPairs);
+ }
+
+ // Find the first impact and filter all contacts beyond a given epsilon
+ if (!findEarliestContact(ccdPairs, &localTimeOfImpact))
+ {
+ break;
+ }
+ filterLaterContacts(&ccdPairs, epsilon, localTimeOfImpact);
+
+ restoreContacts(&ccdPairs, &oldContacts);
+
+ ccdState = m_constraintGeneration->update(dt, ccdState);
+ ccdState = m_buildMlcp->update(dt, ccdState);
+ ccdState = m_solveMlcp->update(dt, ccdState);
+ ccdState = m_pushResults->update(dt, ccdState);
+ executedOnce = true;
+
+ backupContacts(&ccdPairs, &oldContacts);
+
+ timeOfImpact += (1.0 - timeOfImpact) * localTimeOfImpact;
+ if (timeOfImpact > 1.0)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << "Calculated time of impact is greater " <<
+ "than the parametric upper bound of 1.0 (" <<
+ timeOfImpact << ")" << std::endl;
+ break;
+ }
+
+ // Lambda == 0 means we are no longer generating corrections. Exit the loop.
+ // We will take up the collision detection at the start of the next time step.
+ if (ccdState->getMlcpSolution().x.isZero())
+ {
+ break;
+ }
+ }
+
+ SURGSIM_LOG_IF(iterations == m_maxIterations, m_logger, WARNING) <<
+ "Maxed out iterations (" << m_maxIterations << ")";
+
+ restoreContacts(&ccdPairs, &oldContacts);
+ if (!executedOnce)
+ {
+ ccdState = m_constraintGeneration->update(dt, ccdState);
+ ccdState = m_buildMlcp->update(dt, ccdState);
+ ccdState = m_solveMlcp->update(dt, ccdState);
+ ccdState = m_pushResults->update(dt, ccdState);
+ }
+
+ return ccdState;
+}
+
+bool CcdCollisionLoop::findEarliestContact(
+ const std::vector<std::shared_ptr<Collision::CollisionPair>>& ccdPairs,
+ double* currentTimeOfImpact)
+{
+ SURGSIM_ASSERT(currentTimeOfImpact != nullptr) << "Please provide a valid currentTimeOfImpact variable";
+
+ double timeOfImpact = std::numeric_limits<double>::max();
+
+ // Find earliest time of impact
+ for (auto& pair : ccdPairs)
+ {
+ std::for_each(pair->getContacts().begin(),
+ pair->getContacts().end(),
+ [&timeOfImpact](const std::shared_ptr<Collision::Contact>& contact)
+ {
+ timeOfImpact = std::min<double>(timeOfImpact, contact->time);
+ });
+ }
+
+ // Did not find any contacts return false
+ if (!(timeOfImpact < std::numeric_limits<double>::max()))
+ {
+ return false;
+ }
+
+ *currentTimeOfImpact = timeOfImpact;
+
+ return true;
+}
+
+void CcdCollisionLoop::filterLaterContacts(
+ std::vector<std::shared_ptr<Collision::CollisionPair>>* ccdPairs,
+ double epsilon,
+ double timeOfImpact)
+{
+ for (auto& pair : (*ccdPairs))
+ {
+ pair->getContacts().remove_if([timeOfImpact, epsilon](const std::shared_ptr<Collision::Contact>& contact)
+ {
+ return contact->time > timeOfImpact + epsilon;
+ });
+ }
+}
+
+void CcdCollisionLoop::backupContacts(std::vector<std::shared_ptr<Collision::CollisionPair>>* ccdPairs,
+ std::vector<std::list<std::shared_ptr<Collision::Contact>>>* oldContacts)
+{
+ SURGSIM_ASSERT(oldContacts != nullptr) << "Invalid container found.";
+ for (auto& pair : (*ccdPairs))
+ {
+ oldContacts->push_back(std::move(pair->getContacts()));
+ pair->getContacts().clear();
+ }
+}
+
+void CcdCollisionLoop::restoreContacts(std::vector<std::shared_ptr<Collision::CollisionPair>>* ccdPairs,
+ std::vector<std::list<std::shared_ptr<Collision::Contact>>>* oldContacts)
+{
+ SURGSIM_ASSERT(oldContacts != nullptr) << "Invalid container found.";
+ if (oldContacts->size() == 0)
+ {
+ return;
+ }
+
+ SURGSIM_ASSERT(oldContacts->size() == ccdPairs->size()) << "Contact size exception detected";
+ for (size_t i = 0; i < oldContacts->size(); ++i)
+ {
+ auto& newContacts = ccdPairs->at(i)->getContacts();
+ newContacts.splice(newContacts.end(), std::move(oldContacts->at(i)));
+ }
+ oldContacts->clear();
+}
+
+void CcdCollisionLoop::printContacts(const std::vector<std::shared_ptr<Collision::CollisionPair>>& ccdPairs)
+{
+ std::stringstream out;
+ size_t contactCount = 0;
+ for (const auto& pair : ccdPairs)
+ {
+ for (const auto& contact : pair->getContacts())
+ {
+ out << *contact;
+ contactCount++;
+ }
+ }
+ SURGSIM_LOG_IF(contactCount != 0, m_logger, DEBUG) << "Number of Contacts: " <<
+ contactCount << std::endl << out.str();
+}
+}
+}
diff --git a/SurgSim/Physics/CcdCollisionLoop.h b/SurgSim/Physics/CcdCollisionLoop.h
new file mode 100644
index 0000000..f7444c0
--- /dev/null
+++ b/SurgSim/Physics/CcdCollisionLoop.h
@@ -0,0 +1,124 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_CCDCOLLISIONLOOP_H
+#define SURGSIM_PHYSICS_CCDCOLLISIONLOOP_H
+
+#include "SurgSim/Physics/ComputationGroup.h"
+
+namespace SurgSim
+{
+
+namespace Framework
+{
+class Logger;
+}
+
+namespace Collision
+{
+class CollisionPair;
+struct Contact;
+}
+namespace Physics
+{
+
+class CcdCollision;
+class ContactFiltering;
+class UpdateCcdData;
+class ContactConstraintGeneration;
+class BuildMlcp;
+class SolveMlcp;
+class PushResults;
+class PhysicsManager;
+
+class CcdCollisionLoop : public Computation
+{
+public:
+
+ /// Constructor
+ explicit CcdCollisionLoop(bool copyState);
+
+ /// Destructor
+ ~CcdCollisionLoop();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::CcdCollisionLoop);
+
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state) override;
+
+ ///@{
+ /// Test access
+ friend class CcdCollisionLoopTest_FilterContacts_Test;
+ friend class CcdCollisionLoopTest_FilterContactsWithEpsilon_Test;
+ ///@}
+
+private:
+ ///@{
+ /// Computations
+ std::unique_ptr<CcdCollision> m_ccdCollision;
+ std::unique_ptr<UpdateCcdData> m_updateCcdData;
+ std::unique_ptr<ContactFiltering> m_contactFilter;
+ std::unique_ptr<ContactConstraintGeneration> m_constraintGeneration;
+ std::unique_ptr<BuildMlcp> m_buildMlcp;
+ std::unique_ptr<SolveMlcp> m_solveMlcp;
+ std::unique_ptr<PushResults> m_pushResults;
+ ///@}
+
+ size_t m_maxIterations; ///< maximum number of iterations to run
+
+ /// epsilon as a fraction of dt, i.e. if this is 100, the epsilon will be dt/100
+ /// during the iteration epsilon will be scaled to remain dt/100 as it pertains to the ever shrinking interval
+ /// that is the iterations intervall
+ double m_epsilonFactor;
+
+ /// Takes all the contacts from ccdPairs, finds the first contact wrt contact time
+ /// \param ccdPairs the list of pairs that should be checked for contacts
+ /// \param [out] currentTimeOfImpact the earliest contact time found in ccdPairs
+ /// \return true if there were any contacts found in ccdPairs
+ bool findEarliestContact(const std::vector<std::shared_ptr<Collision::CollisionPair>>& ccdPairs,
+ double* currentTimeOfImpact);
+
+ /// Removes all contacts with contact time greater than the first contact time + epsilon
+ /// \param ccdPairs the list of pairs that should be checked for contacts
+ /// \param epsilon the epsilon to be added to the first contactTime for filtering
+ /// \param contactTime times outside of epsilon from contactTime will be reomved from consideration
+ void filterLaterContacts(std::vector<std::shared_ptr<Collision::CollisionPair>>* ccdPairs,
+ double epsilon,
+ double contactTime);
+
+ /// Backs up all current contacts into oldContacts and then clears the ccdPairs
+ /// \param ccdPairs the list of current contact pairs
+ /// \param oldContacts the backup of the contacts
+ void backupContacts(std::vector<std::shared_ptr<Collision::CollisionPair>>* ccdPairs,
+ std::vector<std::list<std::shared_ptr<Collision::Contact>>>* oldContacts);
+
+ /// Adds all of the backed up contacts back into the current contacts. Contacts already in 'ccdPairs'
+ /// will be kept..
+ /// \param ccdPairs the list of current contact pairs
+ /// \param oldContacts the backup of the contacts
+ void restoreContacts(std::vector<std::shared_ptr<Collision::CollisionPair>>* ccdPairs,
+ std::vector<std::list<std::shared_ptr<Collision::Contact>>>* oldContacts);
+
+ /// Logs all of the contacts
+ /// \param ccdPairs the list of current contact pairs
+ void printContacts(const std::vector<std::shared_ptr<Collision::CollisionPair>>& ccdPairs);
+
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Physics/Computation.cpp b/SurgSim/Physics/Computation.cpp
index caac79f..a59bed5 100644
--- a/SurgSim/Physics/Computation.cpp
+++ b/SurgSim/Physics/Computation.cpp
@@ -15,6 +15,7 @@
#include "SurgSim/Physics/Computation.h"
+#include "SurgSim/Framework/Component.h"
#include "SurgSim/Physics/PhysicsManagerState.h"
namespace SurgSim
@@ -34,7 +35,10 @@ 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))));
+ m_timer.beginFrame();
+ auto newState = doUpdate(dt, preparePhysicsState(state));
+ m_timer.endFrame();
+ return newState;
}
void Computation::setDoCopyState(bool val)
@@ -47,20 +51,37 @@ bool Computation::isCopyingState()
return m_copyState;
}
+Framework::Timer& Computation::getTimer()
+{
+ return m_timer;
+}
+
std::shared_ptr<PhysicsManagerState> Computation::preparePhysicsState(const std::shared_ptr<PhysicsManagerState>& state)
{
+ auto isInactive = [](std::shared_ptr<Framework::Component> component)
+ {
+ return !component->isActive();
+ };
+
// 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);
+ representations.erase(std::remove_if(representations.begin(), representations.end(), isInactive),
+ representations.end());
+ state->setActiveRepresentations(representations);
+
+ // Compile the list of active collision representations and set it on the state.
+ auto collisionRepresentations = state->getCollisionRepresentations();
+ collisionRepresentations.erase(
+ std::remove_if(collisionRepresentations.begin(), collisionRepresentations.end(), isInactive),
+ collisionRepresentations.end());
+ state->setActiveCollisionRepresentations(collisionRepresentations);
+
+ // Compile the list of active particle representations and set it on the state.
+ auto particleRepresentations = state->getParticleRepresentations();
+ particleRepresentations.erase(
+ std::remove_if(particleRepresentations.begin(), particleRepresentations.end(), isInactive),
+ particleRepresentations.end());
+ state->setActiveParticleRepresentations(particleRepresentations);
// Compile the list of active constraints and set it on the state.
std::vector<std::shared_ptr<Constraint>> activeConstraints;
diff --git a/SurgSim/Physics/Computation.h b/SurgSim/Physics/Computation.h
index bd9cbf9..4e9730f 100644
--- a/SurgSim/Physics/Computation.h
+++ b/SurgSim/Physics/Computation.h
@@ -19,6 +19,8 @@
#include <vector>
#include <memory>
+#include "SurgSim/Framework/Timer.h"
+
namespace SurgSim
{
namespace Physics
@@ -54,6 +56,15 @@ public:
/// \return true if copying the state, false if not.
bool isCopyingState();
+ /// The class name for this class
+ /// \note Use the SURGSIM_CLASSNAME macro in derived classes.
+ /// \return The fully namespace qualified name of this class.
+ virtual std::string getClassName() const = 0;
+
+ /// Provides access to the update timer.
+ /// \return The timer.
+ Framework::Timer& getTimer();
+
protected:
/// Override this function to implement the computations specific behavior
@@ -66,6 +77,9 @@ private:
/// Copy the PhysicsManagerState object when isCopyingState() is true
std::shared_ptr<PhysicsManagerState> preparePhysicsState(const std::shared_ptr<PhysicsManagerState>& state);
+
+ /// The update timer.
+ Framework::Timer m_timer;
};
diff --git a/SurgSim/Physics/ComputationGroup.cpp b/SurgSim/Physics/ComputationGroup.cpp
new file mode 100644
index 0000000..a7a98a6
--- /dev/null
+++ b/SurgSim/Physics/ComputationGroup.cpp
@@ -0,0 +1,93 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/ComputationGroup.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+ComputationGroup::ComputationGroup(bool copyState) :
+ Computation(copyState)
+{
+
+}
+
+ComputationGroup::~ComputationGroup()
+{
+
+}
+
+std::shared_ptr<PhysicsManagerState> ComputationGroup::doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ boost::unique_lock<boost::mutex> lock(m_computationsMutex);
+ m_iterations = 0;
+ bool keepRunning = true;
+ auto lastState = state;
+ while (keepRunning)
+ {
+ ++m_iterations;
+ for (const auto& computation : m_computations)
+ {
+ lastState = computation->update(dt, lastState);
+ if (lastState->shouldAbortGroup())
+ {
+ lastState->setAbortGroup(false);
+ keepRunning = false;
+ break;
+ }
+ }
+
+ keepRunning = keepRunning && !endIteration();
+ }
+ return lastState;
+}
+
+
+void ComputationGroup::addComputation(const std::shared_ptr<Computation>& computation)
+{
+ boost::unique_lock<boost::mutex> lock(m_computationsMutex);
+ m_computations.push_back(computation);
+}
+
+bool ComputationGroup::endIteration()
+{
+ return true;
+}
+
+std::vector<std::shared_ptr<Computation>> ComputationGroup::getComputations() const
+{
+ boost::unique_lock<boost::mutex> lock(m_computationsMutex);
+ return m_computations;
+}
+
+void ComputationGroup::setComputations(const std::vector<std::shared_ptr<Computation>>& val)
+{
+ boost::unique_lock<boost::mutex> lock(m_computationsMutex);
+ m_computations = val;
+}
+
+size_t ComputationGroup::getIterations()
+{
+ return m_iterations;
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Physics/ComputationGroup.h b/SurgSim/Physics/ComputationGroup.h
new file mode 100644
index 0000000..837292e
--- /dev/null
+++ b/SurgSim/Physics/ComputationGroup.h
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_COMPUTATIONGROUP_H
+#define SURGSIM_PHYSICS_COMPUTATIONGROUP_H
+
+#include "SurgSim/Physics/Computation.h"
+#include "SurgSim/Framework/Macros.h"
+
+#include <boost/thread.hpp>
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Implements a mechanism to group and loop computations, the computations will be called in sequence repeatedly
+/// until one of the exit criteria is met. That is either the endIteration() call, or shouldAbortLoop()
+/// in the \sa PhysicsManagerState return true.
+class ComputationGroup : public Computation
+{
+public:
+ /// Constructor
+ explicit ComputationGroup(bool copyState);
+
+ /// Destructor
+ ~ComputationGroup();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::ComputationGroup);
+
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state) override;
+
+ /// Override this function to implement a custom criterion to exit this computation, when this returns true
+ /// the computation will exit and return the last state
+ /// \note the computation can also be stopped by calling setAbortLoop(true) on the physics state by any calculation
+ /// \return true to stop calculating.
+ virtual bool endIteration();
+
+ /// Adds a computation to this group, the computation will be appended at the end
+ /// \param computation the computation to be added to this group
+ void addComputation(const std::shared_ptr<Computation>& computation);
+
+ /// \return the computations used by this class
+ std::vector<std::shared_ptr<Computation>> getComputations() const;
+
+ /// \param val set the computations used by this class
+ void setComputations(const std::vector<std::shared_ptr<Computation>>& val);
+
+ /// \return the number of iterations that have passed
+ size_t getIterations();
+
+private:
+ size_t m_iterations;
+
+ mutable boost::mutex m_computationsMutex;
+
+ std::vector<std::shared_ptr<Computation>> m_computations;
+
+
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Physics/Constraint.cpp b/SurgSim/Physics/Constraint.cpp
index a789fad..5d9a0f0 100644
--- a/SurgSim/Physics/Constraint.cpp
+++ b/SurgSim/Physics/Constraint.cpp
@@ -13,6 +13,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "SurgSim/Math/MlcpConstraintType.h"
#include "SurgSim/Physics/Constraint.h"
#include "SurgSim/Physics/ConstraintData.h"
#include "SurgSim/Physics/Localization.h"
@@ -25,14 +26,21 @@ namespace SurgSim
namespace Physics
{
-Constraint::Constraint(
+Constraint::Constraint(ConstraintType constraintType,
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)
+ std::shared_ptr<Representation> representation0,
+ const SurgSim::DataStructures::Location& location0,
+ std::shared_ptr<Representation> representation1,
+ const SurgSim::DataStructures::Location& location1)
+ : m_active(true)
{
- setInformation(data, implementation0, localization0, implementation1, localization1);
+ m_mlcpMap[FIXED_3DPOINT] = Math::MLCP_BILATERAL_3D_CONSTRAINT;
+ m_mlcpMap[FIXED_3DROTATION_VECTOR] = Math::MLCP_BILATERAL_3D_CONSTRAINT;
+ m_mlcpMap[FRICTIONAL_3DCONTACT] = Math::MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT;
+ m_mlcpMap[FRICTIONLESS_3DCONTACT] = Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;
+ m_mlcpMap[FRICTIONAL_SLIDING] = Math::MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT;
+ m_mlcpMap[FRICTIONLESS_SLIDING] = Math::MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT;
+ setInformation(constraintType, data, representation0, location0, representation1, location1);
}
Constraint::~Constraint()
@@ -62,7 +70,7 @@ size_t Constraint::getNumDof() const
return m_numDof;
}
-SurgSim::Math::MlcpConstraintType Constraint::getType()
+ConstraintType Constraint::getType()
{
return m_constraintType;
}
@@ -74,7 +82,26 @@ void Constraint::build(double dt,
size_t indexOfConstraint)
{
doBuild(dt, *m_data.get(), mlcp, indexOfRepresentation0, indexOfRepresentation1, indexOfConstraint);
+}
+
+bool Constraint::isActive()
+{
+ return m_active && m_localizations.first->getRepresentation()->isActive() &&
+ m_localizations.second->getRepresentation()->isActive();
+}
+
+void Constraint::setActive(bool flag)
+{
+ m_active = flag;
+}
+void Constraint::doBuild(double dt,
+ const ConstraintData& data,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation0,
+ size_t indexOfRepresentation1,
+ size_t indexOfConstraint)
+{
m_implementations.first->build(
dt,
*m_data.get(),
@@ -93,66 +120,41 @@ void Constraint::build(double dt,
indexOfConstraint,
CONSTRAINT_NEGATIVE_SIDE);
- mlcp->constraintTypes.push_back(m_constraintType);
+ mlcp->constraintTypes.push_back(
+ (m_constraintType != INVALID_CONSTRAINT) ? m_mlcpMap[m_constraintType] : Math::MLCP_INVALID_CONSTRAINT);
}
-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(
+void Constraint::setInformation(ConstraintType constraintType,
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)
+ std::shared_ptr<Representation> representation0,
+ const SurgSim::DataStructures::Location& location0,
+ std::shared_ptr<Representation> representation1,
+ const SurgSim::DataStructures::Location& location1)
{
+ m_constraintType = constraintType;
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() <<")";
+ SURGSIM_ASSERT(representation0 != nullptr) << "First representation can't be nullptr";
+ SURGSIM_ASSERT(representation1 != nullptr) << "Second representation can't be nullptr";
+
+ auto localization0 = representation0->createLocalization(location0);
+ SURGSIM_ASSERT(localization0 != nullptr) << "Could not create localization for " << representation0->getName();
+
+ auto localization1 = representation1->createLocalization(location1);
+ SURGSIM_ASSERT(localization1 != nullptr) << "Could not create localization for " << representation1->getName();
+
+ auto implementation0 = representation0->getConstraintImplementation(m_constraintType);
+ SURGSIM_ASSERT(implementation0 != nullptr) << "Could not get implementation for " << representation0->getName();
+
+ auto implementation1 = representation1->getConstraintImplementation(m_constraintType);
+ SURGSIM_ASSERT(implementation1 != nullptr) << "Could not get implementation for " << representation1->getName();
+
+ SURGSIM_ASSERT(implementation0->getNumDof() == implementation1->getNumDof())
+ << "The number of DOFs does not match between the two implementations";
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
diff --git a/SurgSim/Physics/Constraint.h b/SurgSim/Physics/Constraint.h
index f2d71bc..0be7e2a 100644
--- a/SurgSim/Physics/Constraint.h
+++ b/SurgSim/Physics/Constraint.h
@@ -16,10 +16,12 @@
#ifndef SURGSIM_PHYSICS_CONSTRAINT_H
#define SURGSIM_PHYSICS_CONSTRAINT_H
+#include "SurgSim/DataStructures/Location.h"
#include "SurgSim/Physics/ConstraintData.h"
#include "SurgSim/Physics/ConstraintImplementation.h"
#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include <array>
#include <memory>
namespace SurgSim
@@ -32,32 +34,36 @@ namespace Physics
class Constraint
{
public:
- /// Sets all the values for this constraints, does validation on the parameters and will throw it something
+ /// Sets all the values for this constraints, does validation on the parameters and will throw if something
/// is wrong with the constraint.
+ /// \param constraintType The constraint type.
/// \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.
+ /// \param representation0, representation1 Both representations in this constraint.
+ /// \param location0, location1 Both locations of the representations involved in this constraint.
Constraint(
+ ConstraintType constraintType,
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);
+ std::shared_ptr<Representation> representation0,
+ const SurgSim::DataStructures::Location& location0,
+ std::shared_ptr<Representation> representation1,
+ const SurgSim::DataStructures::Location& location1);
/// Destructor
virtual ~Constraint();
- /// Sets all the values for this constraints, does validation on the parameters and will throw it something
+ /// Sets all the values for this constraints, does validation on the parameters and will throw if something
/// is wrong with the constraint.
+ /// \param constraintType The constraint type.
/// \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.
+ /// \param representation0, representation1 Both representations in this constraint.
+ /// \param location0, location1 Both locations of the representations involved in this constraint.
void setInformation(
+ ConstraintType constraintType,
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);
+ std::shared_ptr<Representation> representation0,
+ const SurgSim::DataStructures::Location& location0,
+ std::shared_ptr<Representation> representation1,
+ const SurgSim::DataStructures::Location& location1);
/// Gets both sides implementation as a pair.
/// \return the pair of implementations forming this constraint.
@@ -77,9 +83,9 @@ public:
/// \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();
+ /// Gets the ConstraintType
+ /// \return The type
+ ConstraintType getType();
/// Builds subset of an Mlcp physics problem associated to this constraint.
/// \param dt The time step.
@@ -96,7 +102,13 @@ public:
/// \return Whether this constraint is active.
bool isActive();
-private:
+ /// \param flag Whether this constraint is active.
+ void setActive(bool flag);
+
+protected:
+ /// Constraint-MLCP mapping
+ std::array<Math::MlcpConstraintType, NUM_CONSTRAINT_TYPES> m_mlcpMap;
+
/// Specific data associated to this constraint
std::shared_ptr<ConstraintData> m_data;
@@ -108,7 +120,7 @@ private:
size_t m_numDof;
/// The type of this constraint
- SurgSim::Math::MlcpConstraintType m_constraintType;
+ ConstraintType m_constraintType;
/// Builds subset of an Mlcp physics problem associated to this constraint user-defined call for extra treatment
/// \param dt The time step
@@ -123,6 +135,9 @@ private:
size_t indexOfRepresentation0,
size_t indexOfRepresentation1,
size_t indexOfConstraint);
+
+ /// Flag to indicate whether this constraint is active or not.
+ bool m_active;
};
}; // namespace Physics
diff --git a/SurgSim/Physics/ConstraintComponent.h b/SurgSim/Physics/ConstraintComponent.h
index 8435631..809fb5b 100644
--- a/SurgSim/Physics/ConstraintComponent.h
+++ b/SurgSim/Physics/ConstraintComponent.h
@@ -53,11 +53,11 @@ protected:
/// Initialize the component
/// \return true if successful
- virtual bool doInitialize() override;
+ bool doInitialize() override;
/// Wakeup the component
/// \return true if successful
- virtual bool doWakeUp() override;
+ bool doWakeUp() override;
};
}; // namespace SurgSim
diff --git a/SurgSim/Physics/ConstraintImplementation.cpp b/SurgSim/Physics/ConstraintImplementation.cpp
index 548b275..58cac13 100644
--- a/SurgSim/Physics/ConstraintImplementation.cpp
+++ b/SurgSim/Physics/ConstraintImplementation.cpp
@@ -31,6 +31,28 @@ ConstraintImplementation::~ConstraintImplementation()
{
}
+ConstraintImplementationFactory& ConstraintImplementation::getFactory()
+{
+ static ConstraintImplementationFactory factory;
+ return factory;
+}
+
+size_t ConstraintImplementation::getNumDof() const
+{
+ return doGetNumDof();
+}
+
+void ConstraintImplementation::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);
+}
+
}; // namespace Physics
}; // namespace SurgSim
diff --git a/SurgSim/Physics/ConstraintImplementation.h b/SurgSim/Physics/ConstraintImplementation.h
index 51607ea..dc86bb3 100644
--- a/SurgSim/Physics/ConstraintImplementation.h
+++ b/SurgSim/Physics/ConstraintImplementation.h
@@ -22,6 +22,8 @@
#include "SurgSim/Physics/Representation.h"
#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/ConstraintImplementationFactory.h"
+#include "SurgSim/Physics/ConstraintType.h"
#include "SurgSim/Physics/Localization.h"
#include "SurgSim/Physics/MlcpPhysicsProblem.h"
@@ -41,26 +43,21 @@ class ConstraintImplementation
{
public:
/// Constructor
- /// \note Localization embbed the representation, so it is fully defined
ConstraintImplementation();
/// Destructor
virtual ~ConstraintImplementation();
+ /// \return The static class factory that contains the implementations for a given Representation type.
+ static ConstraintImplementationFactory& getFactory();
+
/// 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;
+ size_t getNumDof() const;
- /// Gets the Type of representation that this implementation is concerned with
- /// \return RepresentationType for this implementation
- virtual RepresentationType getRepresentationType() const = 0;
+ /// Gets the constraint type for this ConstraintImplementation
+ /// \return The constraint type corresponding to this constraint implementation
+ virtual SurgSim::Physics::ConstraintType getConstraintType() const = 0;
/// Builds the subset of an Mlcp physics problem associated to this implementation
/// \param dt The time step
@@ -76,14 +73,11 @@ public:
MlcpPhysicsProblem* mlcp,
size_t indexOfRepresentation,
size_t indexOfConstraint,
- ConstraintSideSign sign)
- {
- doBuild(dt, data, localization, mlcp, indexOfRepresentation, indexOfConstraint, sign);
- }
+ ConstraintSideSign sign);
protected:
/// Preallocated variable for derived implementations of doBuild.
- Eigen::SparseVector<double, 0, ptrdiff_t> m_newH;
+ Eigen::SparseVector<double, Eigen::RowMajor, ptrdiff_t> m_newH;
private:
diff --git a/SurgSim/Physics/ConstraintImplementationFactory.cpp b/SurgSim/Physics/ConstraintImplementationFactory.cpp
index 2e05ca1..d5da58e 100644
--- a/SurgSim/Physics/ConstraintImplementationFactory.cpp
+++ b/SurgSim/Physics/ConstraintImplementationFactory.cpp
@@ -13,11 +13,30 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "SurgSim/Blocks/MassSpring1DRepresentation.h"
+#include "SurgSim/Blocks/MassSpring2DRepresentation.h"
+#include "SurgSim/Blocks/MassSpring3DRepresentation.h"
#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"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/FemConstraintFixedPoint.h"
+#include "SurgSim/Physics/FemConstraintFixedRotationVector.h"
+#include "SurgSim/Physics/FemConstraintFrictionalSliding.h"
+#include "SurgSim/Physics/FemConstraintFrictionlessContact.h"
+#include "SurgSim/Physics/FemConstraintFrictionlessSliding.h"
+#include "SurgSim/Physics/FixedConstraintFixedPoint.h"
+#include "SurgSim/Physics/FixedConstraintFixedRotationVector.h"
+#include "SurgSim/Physics/FixedConstraintFrictionlessContact.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/MassSpringConstraintFixedPoint.h"
+#include "SurgSim/Physics/MassSpringConstraintFrictionlessContact.h"
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+#include "SurgSim/Physics/RigidConstraintFixedPoint.h"
+#include "SurgSim/Physics/RigidConstraintFixedRotationVector.h"
+#include "SurgSim/Physics/RigidConstraintFrictionlessContact.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
namespace SurgSim
{
@@ -26,9 +45,39 @@ namespace Physics
ConstraintImplementationFactory::ConstraintImplementationFactory()
{
- addImplementation(std::make_shared<FixedRepresentationContact>());
- addImplementation(std::make_shared<RigidRepresentationContact>());
- addImplementation(std::make_shared<Fem3DRepresentationContact>());
+ using SurgSim::Blocks::MassSpring1DRepresentation;
+ using SurgSim::Blocks::MassSpring2DRepresentation;
+ using SurgSim::Blocks::MassSpring3DRepresentation;
+
+ addImplementation(typeid(FixedRepresentation), std::make_shared<FixedConstraintFrictionlessContact>());
+ addImplementation(typeid(RigidRepresentation), std::make_shared<RigidConstraintFrictionlessContact>());
+ addImplementation(typeid(Fem1DRepresentation), std::make_shared<FemConstraintFrictionlessContact>());
+ addImplementation(typeid(Fem2DRepresentation), std::make_shared<FemConstraintFrictionlessContact>());
+ addImplementation(typeid(Fem3DRepresentation), std::make_shared<FemConstraintFrictionlessContact>());
+ addImplementation(typeid(FixedRepresentation), std::make_shared<FixedConstraintFixedPoint>());
+ addImplementation(typeid(RigidRepresentation), std::make_shared<RigidConstraintFixedPoint>());
+ addImplementation(typeid(Fem1DRepresentation), std::make_shared<FemConstraintFixedPoint>());
+ addImplementation(typeid(Fem2DRepresentation), std::make_shared<FemConstraintFixedPoint>());
+ addImplementation(typeid(Fem3DRepresentation), std::make_shared<FemConstraintFixedPoint>());
+ addImplementation(typeid(Fem1DRepresentation), std::make_shared<FemConstraintFrictionlessSliding>());
+ addImplementation(typeid(Fem2DRepresentation), std::make_shared<FemConstraintFrictionlessSliding>());
+ addImplementation(typeid(Fem3DRepresentation), std::make_shared<FemConstraintFrictionlessSliding>());
+ addImplementation(typeid(Fem1DRepresentation), std::make_shared<FemConstraintFrictionalSliding>());
+ addImplementation(typeid(Fem2DRepresentation), std::make_shared<FemConstraintFrictionalSliding>());
+ addImplementation(typeid(Fem3DRepresentation), std::make_shared<FemConstraintFrictionalSliding>());
+
+ addImplementation(typeid(FixedRepresentation), std::make_shared<FixedConstraintFixedRotationVector>());
+ addImplementation(typeid(RigidRepresentation), std::make_shared<RigidConstraintFixedRotationVector>());
+ addImplementation(typeid(Fem1DRepresentation), std::make_shared<FemConstraintFixedRotationVector>());
+
+ addImplementation(typeid(MassSpringRepresentation), std::make_shared<MassSpringConstraintFrictionlessContact>());
+ addImplementation(typeid(MassSpringRepresentation), std::make_shared<MassSpringConstraintFixedPoint>());
+ addImplementation(typeid(MassSpring1DRepresentation), std::make_shared<MassSpringConstraintFrictionlessContact>());
+ addImplementation(typeid(MassSpring1DRepresentation), std::make_shared<MassSpringConstraintFixedPoint>());
+ addImplementation(typeid(MassSpring2DRepresentation), std::make_shared<MassSpringConstraintFrictionlessContact>());
+ addImplementation(typeid(MassSpring2DRepresentation), std::make_shared<MassSpringConstraintFixedPoint>());
+ addImplementation(typeid(MassSpring3DRepresentation), std::make_shared<MassSpringConstraintFrictionlessContact>());
+ addImplementation(typeid(MassSpring3DRepresentation), std::make_shared<MassSpringConstraintFixedPoint>());
}
ConstraintImplementationFactory::~ConstraintImplementationFactory()
@@ -36,25 +85,23 @@ ConstraintImplementationFactory::~ConstraintImplementationFactory()
}
std::shared_ptr<ConstraintImplementation> ConstraintImplementationFactory::getImplementation(
- RepresentationType representationType,
- SurgSim::Math::MlcpConstraintType constraintType) const
+ std::type_index representationType, ConstraintType constraintType)
{
- SURGSIM_ASSERT(representationType >= 0 && representationType < REPRESENTATION_TYPE_COUNT) <<
- "Invalid representation type " << representationType;
- SURGSIM_ASSERT(constraintType >= 0 && constraintType < SurgSim::Math::MLCP_NUM_CONSTRAINT_TYPES) <<
+ SURGSIM_ASSERT(constraintType >= 0 && constraintType < 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 <<
+ "No constraint implementation for representation type (" << representationType.name() <<
") and constraint type (" << constraintType << ")";
return implementation;
}
-void ConstraintImplementationFactory::addImplementation(std::shared_ptr<ConstraintImplementation> implementation)
+void ConstraintImplementationFactory::addImplementation(
+ std::type_index typeIndex, std::shared_ptr<ConstraintImplementation> implementation)
{
- m_implementations[implementation->getRepresentationType()][implementation->getMlcpConstraintType()] =
+ m_implementations[typeIndex][implementation->getConstraintType()] =
implementation;
}
diff --git a/SurgSim/Physics/ConstraintImplementationFactory.h b/SurgSim/Physics/ConstraintImplementationFactory.h
index 58f60cc..66861ee 100644
--- a/SurgSim/Physics/ConstraintImplementationFactory.h
+++ b/SurgSim/Physics/ConstraintImplementationFactory.h
@@ -16,9 +16,12 @@
#ifndef SURGSIM_PHYSICS_CONSTRAINTIMPLEMENTATIONFACTORY_H
#define SURGSIM_PHYSICS_CONSTRAINTIMPLEMENTATIONFACTORY_H
+#include <array>
#include <memory>
+#include <typeindex>
+#include <unordered_map>
-#include "SurgSim/Math/MlcpConstraintType.h"
+#include "SurgSim/Physics/ConstraintType.h"
#include "SurgSim/Physics/Representation.h"
namespace SurgSim
@@ -44,27 +47,27 @@ public:
/// Get the instance of a ConstraintImplementation for a specific representation and
/// constraint type.
- /// \param representationType Type of the representation.
+ /// \param representationType Type index 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:
+ std::type_index representationType,
+ ConstraintType constraintType);
/// Add an implementation to the internal directory.
+ /// \param typeIndex The type of representation associated with the implementation.
/// \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];
+ void addImplementation(std::type_index typeIndex, std::shared_ptr<ConstraintImplementation> implementation);
+private:
+ /// Lookup table for constraint implementations
+ std::unordered_map<std::type_index,
+ std::array<std::shared_ptr<ConstraintImplementation>, NUM_CONSTRAINT_TYPES>>
+ m_implementations;
};
}; // Physics
}; // SurgSim
-#endif
\ No newline at end of file
+#endif
diff --git a/SurgSim/Physics/ConstraintType.h b/SurgSim/Physics/ConstraintType.h
new file mode 100644
index 0000000..d96e3e5
--- /dev/null
+++ b/SurgSim/Physics/ConstraintType.h
@@ -0,0 +1,39 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_CONSTRAINTTYPE_H
+#define SURGSIM_PHYSICS_CONSTRAINTTYPE_H
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+enum ConstraintType
+{
+ INVALID_CONSTRAINT = -1,
+ FIXED_3DPOINT = 0,
+ FIXED_3DROTATION_VECTOR,
+ FRICTIONLESS_3DCONTACT,
+ FRICTIONAL_3DCONTACT,
+ FRICTIONLESS_SLIDING,
+ FRICTIONAL_SLIDING,
+ NUM_CONSTRAINT_TYPES
+};
+
+} // namespace Physics
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_CONSTRAINTTYPE_H
diff --git a/SurgSim/Physics/ContactConstraintData.h b/SurgSim/Physics/ContactConstraintData.h
index 77f03fb..8484b3c 100644
--- a/SurgSim/Physics/ContactConstraintData.h
+++ b/SurgSim/Physics/ContactConstraintData.h
@@ -16,8 +16,8 @@
#ifndef SURGSIM_PHYSICS_CONTACTCONSTRAINTDATA_H
#define SURGSIM_PHYSICS_CONTACTCONSTRAINTDATA_H
+#include "SurgSim/Collision/CollisionPair.h"
#include "SurgSim/Physics/ConstraintData.h"
-
#include "SurgSim/Math/Vector.h"
namespace SurgSim
@@ -32,8 +32,8 @@ class ContactConstraintData : public ConstraintData
public:
/// Default constructor
ContactConstraintData() :
- ConstraintData(),
- m_distance(0.0)
+ ConstraintData(),
+ m_distance(0.0)
{
m_normal.setZero();
}
@@ -67,12 +67,34 @@ public:
return m_distance;
}
+ /// Gets the time of contact for the collision
+ /// \return The time of contact for the collision
+ double getContactTime() const
+ {
+ return m_contact->time;
+ }
+
+ /// \return The contact that uses this constraint data.
+ std::shared_ptr<Collision::Contact> getContact()
+ {
+ return m_contact;
+ }
+
+ /// \param contacts The contacts that use this constraint data.
+ void setContact(const std::shared_ptr<Collision::Contact>& contacts)
+ {
+ m_contact = contacts;
+ }
+
private:
/// Plane equation normal vector (normalized vector)
SurgSim::Math::Vector3d m_normal;
/// Plane equation distance to origin
double m_distance;
+
+ /// The contact that uses this constraint data.
+ std::shared_ptr<Collision::Contact> m_contact;
};
}; // namespace Physics
diff --git a/SurgSim/Physics/ContactConstraintGeneration.cpp b/SurgSim/Physics/ContactConstraintGeneration.cpp
index ce9afe6..e14277a 100644
--- a/SurgSim/Physics/ContactConstraintGeneration.cpp
+++ b/SurgSim/Physics/ContactConstraintGeneration.cpp
@@ -25,7 +25,6 @@
#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"
@@ -45,14 +44,13 @@ ContactConstraintGeneration::~ContactConstraintGeneration()
}
std::shared_ptr<PhysicsManagerState> ContactConstraintGeneration::doUpdate(
- const double& dt,
- const std::shared_ptr<PhysicsManagerState>& state)
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
{
- auto result = state;
- auto pairs = result->getCollisionPairs();
+ using SurgSim::DataStructures::Location;
- auto pairsIt = std::begin(pairs);
- auto pairsEnd = std::end(pairs);
+ auto result = state;
+ auto& pairs = result->getCollisionPairs();
std::vector<std::shared_ptr<Constraint>> constraints;
@@ -61,74 +59,57 @@ std::shared_ptr<PhysicsManagerState> ContactConstraintGeneration::doUpdate(
// 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)
+ for (auto& pair : pairs)
{
- if ((*pairsIt)->hasContacts())
+ if (pair->hasContacts())
{
- auto contactsIt = std::begin((*pairsIt)->getContacts());
- auto contactsEnd = std::end((*pairsIt)->getContacts());
- for (; contactsIt != contactsEnd; ++contactsIt)
+ auto collisionRepresentations = pair->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()))
{
- 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));
- }
- }
+ 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;
+ }
+
+ const auto& contacts = pair->getContacts();
+ for (auto& contact : contacts)
+ {
+ std::pair<std::shared_ptr<Location>, std::shared_ptr<Location>> locations;
+
+ locations.first = makeLocation(physicsRepresentations.first, collisionRepresentations.first,
+ contact->penetrationPoints.first);
+ locations.second = makeLocation(physicsRepresentations.second, collisionRepresentations.second,
+ contact->penetrationPoints.second);
+
+ // 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
+ auto data = std::make_shared<ContactConstraintData>();
+ data->setPlaneEquation(contact->normal, contact->depth);
+ data->setContact(contact);
+
+ constraints.push_back(std::make_shared<Constraint>(
+ SurgSim::Physics::FRICTIONLESS_3DCONTACT, data,
+ physicsRepresentations.first, *locations.first,
+ physicsRepresentations.second, *locations.second));
}
}
}
@@ -138,49 +119,24 @@ std::shared_ptr<PhysicsManagerState> ContactConstraintGeneration::doUpdate(
return std::move(result);
}
-std::shared_ptr<Localization> ContactConstraintGeneration::makeLocalization(
+std::shared_ptr<SurgSim::DataStructures::Location> ContactConstraintGeneration::makeLocation(
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);
- }
+ auto physicsLocation = std::make_shared<SurgSim::DataStructures::Location>(location);
+ SURGSIM_ASSERT(physicsLocation != nullptr) << "Cannot create Location object.";
- 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
+ if (location.rigidLocalPosition.hasValue())
{
- SURGSIM_LOG_WARNING(m_logger) << __FUNCTION__ <<
- " Cannot create Localization from nullptr.";
+ // Move the local position from the collision representation that created the location
+ // to local coordinates of the physics representation that is creating a localization
+ physicsLocation->rigidLocalPosition.setValue(
+ physicsRepresentation->getLocalPose().inverse() *
+ collisionRepresentation->getLocalPose() *
+ location.rigidLocalPosition.getValue());
}
- return result;
+ return physicsLocation;
}
}; // Physics
diff --git a/SurgSim/Physics/ContactConstraintGeneration.h b/SurgSim/Physics/ContactConstraintGeneration.h
index c6b155a..e7ca394 100644
--- a/SurgSim/Physics/ContactConstraintGeneration.h
+++ b/SurgSim/Physics/ContactConstraintGeneration.h
@@ -18,8 +18,8 @@
#include <memory>
+#include "SurgSim/Framework/Macros.h"
#include "SurgSim/Physics/Computation.h"
-#include "SurgSim/Physics/ConstraintImplementationFactory.h"
namespace SurgSim
@@ -42,8 +42,9 @@ class Representation;
namespace Physics
{
-class PhysicsManagerState;
class Localization;
+class PhysicsManagerState;
+class Representation;
/// Generate a constraint for every contact that was calculated.
/// The general algorithm is such, for each pair of Collision Representations that has Contacts
@@ -62,14 +63,12 @@ public:
/// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
explicit ContactConstraintGeneration(bool doCopyState = false);
+ SURGSIM_CLASSNAME(SurgSim::Physics::ContactConstraintGeneration);
+
/// 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;
@@ -78,16 +77,16 @@ private:
/// \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;
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+ override;
- /// Generate a localization from a Collision Representation.
+ /// Generate a location 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(
+ /// \note Asserts if cannot Location create shared object.
+ std::shared_ptr<SurgSim::DataStructures::Location> makeLocation(
std::shared_ptr<SurgSim::Physics::Representation> physicsRepresentation,
std::shared_ptr<SurgSim::Collision::Representation> collisionRepresentation,
const SurgSim::DataStructures::Location& location);
diff --git a/SurgSim/Physics/ContactFiltering.cpp b/SurgSim/Physics/ContactFiltering.cpp
new file mode 100644
index 0000000..bb581bf
--- /dev/null
+++ b/SurgSim/Physics/ContactFiltering.cpp
@@ -0,0 +1,98 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Collision/ContactFilter.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/ThreadPool.h"
+#include "SurgSim/Physics/ContactFiltering.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+ContactFiltering::ContactFiltering(bool doCopyState) :
+ Computation(doCopyState)
+{
+}
+
+ContactFiltering::~ContactFiltering()
+{
+}
+
+std::shared_ptr<PhysicsManagerState> ContactFiltering::doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ static auto isActive = [](std::shared_ptr<Collision::ContactFilter> f)
+ {
+ return f->isActive();
+ };
+
+ static auto hasContacts = [](std::shared_ptr<Collision::CollisionPair> p)
+ {
+ return p->hasContacts();
+ };
+
+ std::shared_ptr<PhysicsManagerState> result = state;
+ auto threadPool = Framework::Runtime::getThreadPool();
+ std::vector<std::future<void>> tasks;
+
+ const auto& stateFilters = state->getContactFilters();
+ std::vector<std::shared_ptr<Collision::ContactFilter>> filters;
+ filters.reserve(stateFilters.size());
+ std::copy_if(stateFilters.begin(), stateFilters.end(), std::back_inserter(filters), isActive);
+
+ if (filters.size() == 0)
+ {
+ return result;
+ }
+
+ for (const auto& filter : filters)
+ {
+ filter->update(dt);
+ }
+
+ const auto& statePairs = state->getCollisionPairs();
+ std::vector <std::shared_ptr<Collision::CollisionPair>> pairs;
+ pairs.reserve(statePairs.size());
+ std::copy_if(statePairs.begin(), statePairs.end(), std::back_inserter(pairs), hasContacts);
+
+ for (auto& pair : pairs)
+ {
+ tasks.push_back(threadPool->enqueue<void>([&state, &filters, &pair]()
+ {
+ for (const auto& filter : filters)
+ {
+ filter->filterContacts(state, pair);
+ }
+ }));
+ }
+
+
+ std::for_each(tasks.begin(), tasks.end(), [](std::future<void>& p)
+ {
+ p.get();
+ });
+
+ return result;
+}
+
+}; // Physics
+}; // SurgSim
+
diff --git a/SurgSim/Physics/ContactFiltering.h b/SurgSim/Physics/ContactFiltering.h
new file mode 100644
index 0000000..dce925b
--- /dev/null
+++ b/SurgSim/Physics/ContactFiltering.h
@@ -0,0 +1,57 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_CONTACTFILTERING_H
+#define SURGSIM_PHYSICS_CONTACTFILTERING_H
+
+#include <memory>
+
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Physics/Computation.h"
+
+namespace SurgSim
+{
+
+
+namespace Physics
+{
+class PhysicsManagerState;
+
+/// Computation to determine filter the contacts on collisions pairs.
+/// This Computation class takes the list of collision pairs and the list of contact filters and will apply all
+/// the filters to the collision pairs' contacts.
+class ContactFiltering : public Computation
+{
+public:
+
+ /// Constructor
+ /// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
+ explicit ContactFiltering(bool doCopyState = false);
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::ContactFiltering);
+
+ /// Destructor
+ virtual ~ContactFiltering();
+
+protected:
+
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+ override;
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif
diff --git a/SurgSim/Physics/DcdCollision.cpp b/SurgSim/Physics/DcdCollision.cpp
index 255d7db..74ca840 100644
--- a/SurgSim/Physics/DcdCollision.cpp
+++ b/SurgSim/Physics/DcdCollision.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -15,23 +15,26 @@
#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/CcdDcdCollision.h"
#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/ThreadPool.h"
+#include "SurgSim/Physics/DcdCollision.h"
#include "SurgSim/Physics/PhysicsManagerState.h"
using SurgSim::Collision::CollisionPair;
+using SurgSim::Collision::ContactCalculation;
namespace SurgSim
{
namespace Physics
{
-DcdCollision::DcdCollision(bool doCopyState) : Computation(doCopyState)
+DcdCollision::DcdCollision(bool doCopyState) :
+ Computation(doCopyState)
{
- populateCalculationTable();
}
DcdCollision::~DcdCollision()
@@ -43,116 +46,26 @@ std::shared_ptr<PhysicsManagerState> DcdCollision::doUpdate(
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>()));
+ auto threadPool = Framework::Runtime::getThreadPool();
+ std::vector<std::future<void>> tasks;
- 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();
+ const auto& calculations = ContactCalculation::getDcdContactTable();
- if (representations.size() > 1)
+ for (auto& pair : result->getCollisionPairs())
{
- 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)
+ if (pair->getType() == Collision::COLLISION_DETECTION_TYPE_DISCRETE)
{
- auto second = first;
- ++second;
- for (; second != std::end(representations); ++second)
+ tasks.push_back(threadPool->enqueue<void>([&calculations, &pair]()
{
- std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>();
- pair->setRepresentations(*first,*second);
- pairs.push_back(pair);
- }
+ calculations[pair->getFirst()->getShapeType()]
+ [pair->getSecond()->getShapeType()]->calculateContact(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;
- }
+ std::for_each(tasks.begin(), tasks.end(), [](std::future<void>& p){p.get();});
+
+ return result;
}
}; // Physics
diff --git a/SurgSim/Physics/DcdCollision.h b/SurgSim/Physics/DcdCollision.h
index fc547ea..fcbba87 100644
--- a/SurgSim/Physics/DcdCollision.h
+++ b/SurgSim/Physics/DcdCollision.h
@@ -18,6 +18,7 @@
#include <memory>
+#include "SurgSim/Framework/Macros.h"
#include "SurgSim/Math/Shape.h"
#include "SurgSim/Physics/Computation.h"
@@ -39,8 +40,8 @@ class PhysicsManagerState;
/// 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
-
+/// \note When a new ContactCalculation type gets implemented, the type needs to be registered with the table
+/// inside of ContactCalculation
class DcdCollision : public Computation
{
public:
@@ -49,6 +50,8 @@ public:
/// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
explicit DcdCollision(bool doCopyState = false);
+ SURGSIM_CLASSNAME(SurgSim::Physics::DcdCollision);
+
/// Destructor
virtual ~DcdCollision();
@@ -57,26 +60,8 @@ 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];
-
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+ override;
};
}; // Physics
diff --git a/SurgSim/Physics/DeformableCollisionRepresentation.cpp b/SurgSim/Physics/DeformableCollisionRepresentation.cpp
index e724e21..00d0ec2 100644
--- a/SurgSim/Physics/DeformableCollisionRepresentation.cpp
+++ b/SurgSim/Physics/DeformableCollisionRepresentation.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,10 +16,14 @@
#include "SurgSim/DataStructures/TriangleMesh.h"
#include "SurgSim/Framework/ObjectFactory.h"
#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/SceneElement.h"
#include "SurgSim/Math/MathConvert.h"
#include "SurgSim/Math/MeshShape.h"
#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SegmentMeshShape.h"
#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Math/SurfaceMeshShape.h"
#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
#include "SurgSim/Physics/DeformableRepresentation.h"
@@ -41,39 +45,43 @@ DeformableCollisionRepresentation::~DeformableCollisionRepresentation()
{
}
-void DeformableCollisionRepresentation::setMesh(std::shared_ptr<SurgSim::DataStructures::TriangleMesh> mesh)
+namespace
{
- 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)
+bool updateShapeFromOdeState(const Math::OdeState& odeState, SurgSim::Math::Shape* shape)
{
- 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.";
+ bool result = false;
+ const size_t numNodes = odeState.getNumNodes();
- for (size_t nodeId = 0; nodeId < numNodes; ++nodeId)
+ if (shape->getType() == SurgSim::Math::SHAPE_TYPE_MESH ||
+ shape->getType() == SurgSim::Math::SHAPE_TYPE_SURFACEMESH)
+ {
+ auto meshShape = dynamic_cast<SurgSim::Math::MeshShape*>(shape);
+ SURGSIM_ASSERT(meshShape != nullptr) << "The shape is neither a mesh nor a surface mesh";
+ SURGSIM_ASSERT(meshShape->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)
+ {
+ meshShape->setVertexPosition(nodeId, odeState.getPosition(nodeId));
+ }
+ result = meshShape->update();
+ }
+ else if (shape->getType() == SurgSim::Math::SHAPE_TYPE_SEGMENTMESH)
{
- m_mesh->setVertexPosition(nodeId, odeState->getPosition(nodeId));
+ auto meshShape = dynamic_cast<SurgSim::Math::SegmentMeshShape*>(shape);
+ SURGSIM_ASSERT(meshShape != nullptr) << "The shape is of type mesh but is not a mesh";
+ SURGSIM_ASSERT(meshShape->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)
+ {
+ meshShape->setVertexPosition(nodeId, odeState.getPosition(nodeId));
+ }
+ result = meshShape->update();
}
- m_mesh->update();
- m_shape->updateAabbTree();
+
+ return result;
+}
}
bool DeformableCollisionRepresentation::doInitialize()
@@ -81,7 +89,6 @@ bool DeformableCollisionRepresentation::doInitialize()
bool result = false;
if (nullptr != m_shape && m_shape->isValid())
{
- m_mesh = m_shape->getMesh();
result = true;
}
@@ -91,15 +98,17 @@ bool DeformableCollisionRepresentation::doInitialize()
bool DeformableCollisionRepresentation::doWakeUp()
{
auto physicsRepresentation = m_deformable.lock();
- SURGSIM_ASSERT(nullptr != physicsRepresentation) <<
- "The Physics::Representation referred by this DeformableCollisionRepresentation has expired.";
+ 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.";
+ SURGSIM_ASSERT(nullptr != state)
+ << "DeformableRepresentation " << physicsRepresentation->getName() << " holds an empty OdeState.";
+ auto shape = std::dynamic_pointer_cast<DataStructures::VerticesPlain>(m_shape);
+ SURGSIM_ASSERT(shape != nullptr)
+ << "The shape object is not inherited from DataStructures::VerticesPlain, but should be.";
+ SURGSIM_ASSERT(shape->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;
@@ -113,12 +122,12 @@ int DeformableCollisionRepresentation::getShapeType() const
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();
+ SURGSIM_ASSERT(shape->getType() == SurgSim::Math::SHAPE_TYPE_MESH ||
+ shape->getType() == SurgSim::Math::SHAPE_TYPE_SEGMENTMESH ||
+ shape->getType() == SurgSim::Math::SHAPE_TYPE_SURFACEMESH)
+ << "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();
+ m_shape = shape;
}
const std::shared_ptr<SurgSim::Math::Shape> DeformableCollisionRepresentation::getShape() const
@@ -133,15 +142,89 @@ void DeformableCollisionRepresentation::setDeformableRepresentation(
}
const std::shared_ptr<SurgSim::Physics::DeformableRepresentation>
- DeformableCollisionRepresentation::getDeformableRepresentation() const
+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.";
+ "Failed to get the deformable representation. The DeformableCollisionRepresentation either was not "
+ "attached to a Physics::Representation or the Physics::Representation has expired.";
return physicsRepresentation;
}
+
+void DeformableCollisionRepresentation::updateShapeData()
+{
+ 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.";
+
+ // Write current shape
+ if (!updateShapeFromOdeState(*physicsRepresentation->getCurrentState().get(), m_shape.get()))
+ {
+ setLocalActive(false);
+ SURGSIM_LOG_SEVERE(Framework::Logger::getLogger("Collision/DeformableCollisionRepresentation")) <<
+ "CollisionRepresentation '" << getFullName() << "' went inactive because its shape failed to update.";
+ }
+}
+
+
+void DeformableCollisionRepresentation::updateDcdData()
+{
+ // HS-2-Mar-2016
+ // #todo should only have to build the AABB tree here
+}
+
+void DeformableCollisionRepresentation::updateCcdData(double interval)
+{
+ 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.";
+
+ if (m_previousShape == nullptr)
+ {
+ if (m_shape->getType() == SurgSim::Math::SHAPE_TYPE_MESH ||
+ m_shape->getType() == SurgSim::Math::SHAPE_TYPE_SEGMENTMESH ||
+ m_shape->getType() == SurgSim::Math::SHAPE_TYPE_SURFACEMESH)
+ {
+ m_previousShape = m_shape->getTransformed(Math::RigidTransform3d::Identity());
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Invalid type, should be MeshShape(" << SurgSim::Math::SHAPE_TYPE_MESH <<
+ ") or SegmentMeshShape(" << SurgSim::Math::SHAPE_TYPE_SEGMENTMESH <<
+ ") or SurfaceMeshShape(" << SurgSim::Math::SHAPE_TYPE_SURFACEMESH <<"), but it is " <<
+ m_shape->getType();
+ }
+ }
+
+ if (!updateShapeFromOdeState(*physicsRepresentation->getPreviousState().get(), m_previousShape.get()))
+ {
+ setLocalActive(false);
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getLogger("Collision/DeformableCollisionRepresentation")) <<
+ "CollisionRepresentation '" << getFullName() << "' went inactive because its shape failed to update.";
+ }
+
+
+ // Write current shape
+ if (!updateShapeFromOdeState(*physicsRepresentation->getCurrentState().get(), m_shape.get()))
+ {
+ setLocalActive(false);
+ SURGSIM_LOG_SEVERE(Framework::Logger::getLogger("Collision/DeformableCollisionRepresentation")) <<
+ "CollisionRepresentation '" << getFullName() << "' went inactive because its shape failed to update.";
+ }
+
+ Math::PosedShape<std::shared_ptr<Math::Shape>> posedShapeFirst(m_previousShape, Math::RigidTransform3d::Identity());
+ Math::PosedShape<std::shared_ptr<Math::Shape>> posedShapeSecond(m_shape, Math::RigidTransform3d::Identity());
+ Math::PosedShapeMotion<std::shared_ptr<Math::Shape>> posedShapeMotion(posedShapeFirst, posedShapeSecond);
+ setPosedShapeMotion(posedShapeMotion);
+
+ // HS-2-Mar-2016
+ // #todo Add AABB tree for the posedShapeMotion (i.e. that is the tree where each bounding box consists of the
+ // corresponding elements from posedShape1 and posedShape2
+}
+
} // namespace Physics
-} // namespace SurgSim
\ No newline at end of file
+} // namespace SurgSim
diff --git a/SurgSim/Physics/DeformableCollisionRepresentation.h b/SurgSim/Physics/DeformableCollisionRepresentation.h
index f7543ed..236f544 100644
--- a/SurgSim/Physics/DeformableCollisionRepresentation.h
+++ b/SurgSim/Physics/DeformableCollisionRepresentation.h
@@ -24,11 +24,6 @@
namespace SurgSim
{
-namespace DataStructures
-{
-class TriangleMesh;
-}
-
namespace Math
{
class Shape;
@@ -57,21 +52,12 @@ public:
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.
+ /// The vertices in the mesh need to be the same number as the vertices in the deformable representation.
/// \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;
+ 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
@@ -80,19 +66,20 @@ public:
/// \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;
+ int getShapeType() const override;
+
+ void updateDcdData() override;
- virtual void update(const double& dt) override;
+ void updateCcdData(double interval) override;
+
+ void updateShapeData() override;
private:
- virtual bool doInitialize() override;
- virtual bool doWakeUp() override;
+ bool doInitialize() override;
+ 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;
+ std::shared_ptr<SurgSim::Math::Shape> m_shape, m_previousShape;
/// Reference to the deformable driving changes to this mesh
std::weak_ptr<SurgSim::Physics::DeformableRepresentation> m_deformable;
diff --git a/SurgSim/Physics/DeformableRepresentation.cpp b/SurgSim/Physics/DeformableRepresentation.cpp
index e99f8e2..850ee03 100644
--- a/SurgSim/Physics/DeformableRepresentation.cpp
+++ b/SurgSim/Physics/DeformableRepresentation.cpp
@@ -17,6 +17,7 @@
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/SceneElement.h"
#include "SurgSim/Framework/PoseComponent.h"
+#include "SurgSim/Math/LinearSparseSolveAndInverse.h"
#include "SurgSim/Math/MathConvert.h"
#include "SurgSim/Math/OdeSolverEulerExplicit.h"
#include "SurgSim/Math/OdeSolverEulerExplicitModified.h"
@@ -32,6 +33,11 @@
#include "SurgSim/Physics/DeformableRepresentation.h"
#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
+using SurgSim::Math::Matrix;
+using SurgSim::Math::OdeState;
+using SurgSim::Math::SparseMatrix;
+using SurgSim::Math::Vector;
+
namespace SurgSim
{
@@ -42,10 +48,13 @@ DeformableRepresentation::DeformableRepresentation(const std::string& name) :
Representation(name),
SurgSim::Math::OdeEquation(),
m_numDofPerNode(0),
- m_integrationScheme(SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER)
+ m_integrationScheme(SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT),
+ m_linearSolver(SurgSim::Math::LINEARSOLVER_LU)
{
SURGSIM_ADD_SERIALIZABLE_PROPERTY(DeformableRepresentation, SurgSim::Math::IntegrationScheme, IntegrationScheme,
getIntegrationScheme, setIntegrationScheme);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(DeformableRepresentation, SurgSim::Math::LinearSolver, LinearSolver,
+ getLinearSolver, setLinearSolver);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(DeformableRepresentation, std::shared_ptr<SurgSim::Collision::Representation>,
CollisionRepresentation, getCollisionRepresentation, setCollisionRepresentation);
}
@@ -53,6 +62,7 @@ DeformableRepresentation::DeformableRepresentation(const std::string& name) :
DeformableRepresentation::~DeformableRepresentation()
{
}
+
void DeformableRepresentation::resetState()
{
Representation::resetState();
@@ -64,6 +74,14 @@ void DeformableRepresentation::resetState()
*m_finalState = *m_initialState;
}
+void DeformableRepresentation::setLocalPose(const SurgSim::Math::RigidTransform3d& pose)
+{
+ SURGSIM_ASSERT(!isInitialized())
+ << "Cannot set the local pose of a DeformableRepresentation after it has been initialized";
+
+ Representation::setLocalPose(pose);
+}
+
void DeformableRepresentation::setInitialState(
std::shared_ptr<SurgSim::Math::OdeState> initialState)
{
@@ -78,12 +96,15 @@ void DeformableRepresentation::setInitialState(
// 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();
+ m_hasExternalGeneralizedForce = false;
+ m_externalGeneralizedForce.setZero(getNumDof());
+ m_externalGeneralizedStiffness.resize(static_cast<SparseMatrix::Index>(getNumDof()),
+ static_cast<SparseMatrix::Index>(getNumDof()));
+ m_externalGeneralizedDamping.resize(static_cast<SparseMatrix::Index>(getNumDof()),
+ static_cast<SparseMatrix::Index>(getNumDof()));
+ m_previousHasExternalGeneralizedForce = m_hasExternalGeneralizedForce;
+ m_previousExternalGeneralizedStiffness = m_externalGeneralizedStiffness;
+ m_previousExternalGeneralizedDamping = m_externalGeneralizedDamping;
}
const std::shared_ptr<SurgSim::Math::OdeState> DeformableRepresentation::getCurrentState() const
@@ -108,7 +129,8 @@ size_t DeformableRepresentation::getNumDofPerNode() const
void DeformableRepresentation::setIntegrationScheme(SurgSim::Math::IntegrationScheme integrationScheme)
{
- SURGSIM_ASSERT(!isAwake()) << "You cannot set the integration scheme after the component has been awoken";
+ SURGSIM_ASSERT(!isInitialized()) <<
+ "You cannot set the integration scheme after the component has been initialized";
m_integrationScheme = integrationScheme;
}
@@ -117,26 +139,59 @@ SurgSim::Math::IntegrationScheme DeformableRepresentation::getIntegrationScheme(
return m_integrationScheme;
}
+std::shared_ptr<SurgSim::Math::OdeSolver> DeformableRepresentation::getOdeSolver() const
+{
+ return m_odeSolver;
+}
+
+void DeformableRepresentation::setLinearSolver(SurgSim::Math::LinearSolver linearSolver)
+{
+ SURGSIM_ASSERT(!isInitialized()) <<
+ "You cannot set the linear solver after the component has been initialized";
+ m_linearSolver = linearSolver;
+}
+
+SurgSim::Math::LinearSolver DeformableRepresentation::getLinearSolver() const
+{
+ return m_linearSolver;
+}
+
const SurgSim::Math::Vector& DeformableRepresentation::getExternalGeneralizedForce() const
{
return m_externalGeneralizedForce;
}
-const SurgSim::Math::Matrix& DeformableRepresentation::getExternalGeneralizedStiffness() const
+const SurgSim::Math::SparseMatrix& DeformableRepresentation::getExternalGeneralizedStiffness() const
{
return m_externalGeneralizedStiffness;
}
-const SurgSim::Math::Matrix& DeformableRepresentation::getExternalGeneralizedDamping() const
+const SurgSim::Math::SparseMatrix& DeformableRepresentation::getExternalGeneralizedDamping() const
{
return m_externalGeneralizedDamping;
}
-const SurgSim::Math::Matrix& DeformableRepresentation::getComplianceMatrix() const
+Math::Matrix DeformableRepresentation::applyCompliance(const Math::OdeState& state, const Math::Matrix& b)
{
SURGSIM_ASSERT(m_odeSolver) << "Ode solver not initialized, it should have been initialized on wake-up";
- return m_odeSolver->getCompliance();
+ Math::Matrix bTemp = b;
+ for (auto condition : state.getBoundaryConditions())
+ {
+ Math::zeroRow(condition, &bTemp);
+ }
+ auto solution = m_odeSolver->getLinearSolver()->solve(bTemp);
+ for (auto condition : state.getBoundaryConditions())
+ {
+ Math::zeroRow(condition, &solution);
+ }
+ return solution;
+}
+
+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->getComplianceMatrix();
}
void DeformableRepresentation::update(double dt)
@@ -180,12 +235,20 @@ void DeformableRepresentation::afterUpdate(double dt)
driveSceneElementPose(SurgSim::Math::RigidTransform3d::Identity());
// Back up the current state into the final state
+ // #threadsafety
*m_finalState = *m_currentState;
// Reset the external generalized force, stiffness and damping
- m_externalGeneralizedForce.setZero();
- m_externalGeneralizedStiffness.setZero();
- m_externalGeneralizedDamping.setZero();
+ m_previousHasExternalGeneralizedForce = m_hasExternalGeneralizedForce;
+ if (m_hasExternalGeneralizedForce)
+ {
+ std::swap(m_previousExternalGeneralizedStiffness, m_externalGeneralizedStiffness);
+ std::swap(m_previousExternalGeneralizedDamping, m_externalGeneralizedDamping);
+ m_externalGeneralizedForce.setZero();
+ m_externalGeneralizedStiffness.setZero();
+ m_externalGeneralizedDamping.setZero();
+ m_hasExternalGeneralizedForce = false;
+ }
}
void DeformableRepresentation::applyCorrection(double dt,
@@ -210,7 +273,7 @@ void DeformableRepresentation::applyCorrection(double dt,
}
}
-void DeformableRepresentation::deactivateAndReset(void)
+void DeformableRepresentation::deactivateAndReset()
{
SURGSIM_LOG(SurgSim::Framework::Logger::getDefaultLogger(), DEBUG)
<< getName() << " deactivated and reset:" << std::endl
@@ -247,20 +310,9 @@ void DeformableRepresentation::setCollisionRepresentation(
}
}
-bool DeformableRepresentation::doWakeUp()
+bool DeformableRepresentation::doInitialize()
{
- 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;
+ SURGSIM_ASSERT(m_initialState != nullptr) << "You must set the initial state before calling Initialize";
// Transform the state with the initial pose
transformState(m_initialState, getPose());
@@ -280,35 +332,35 @@ bool DeformableRepresentation::doWakeUp()
// 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);
+ case SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT:
+ m_odeSolver = std::make_shared<SurgSim::Math::OdeSolverEulerExplicit>(this);
break;
- case SurgSim::Math::INTEGRATIONSCHEME_MODIFIED_EXPLICIT_EULER:
- m_odeSolver = std::make_shared<OdeSolverEulerExplicitModified>(this);
+ case SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED:
+ m_odeSolver = std::make_shared<SurgSim::Math::OdeSolverEulerExplicitModified>(this);
break;
- case SurgSim::Math::INTEGRATIONSCHEME_IMPLICIT_EULER:
- m_odeSolver = std::make_shared<OdeSolverEulerImplicit>(this);
+ case SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT:
+ m_odeSolver = std::make_shared<SurgSim::Math::OdeSolverEulerImplicit>(this);
break;
case SurgSim::Math::INTEGRATIONSCHEME_STATIC:
- m_odeSolver = std::make_shared<OdeSolverStatic>(this);
+ m_odeSolver = std::make_shared<SurgSim::Math::OdeSolverStatic>(this);
break;
case SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4:
- m_odeSolver = std::make_shared<OdeSolverRungeKutta4>(this);
+ m_odeSolver = std::make_shared<SurgSim::Math::OdeSolverRungeKutta4>(this);
break;
- case SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EXPLICIT_EULER:
- m_odeSolver = std::make_shared<OdeSolverLinearEulerExplicit>(this);
+ case SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT:
+ m_odeSolver = std::make_shared<SurgSim::Math::OdeSolverLinearEulerExplicit>(this);
break;
- case SurgSim::Math::INTEGRATIONSCHEME_LINEAR_MODIFIED_EXPLICIT_EULER:
- m_odeSolver = std::make_shared<OdeSolverLinearEulerExplicitModified>(this);
+ case SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT_MODIFIED:
+ m_odeSolver = std::make_shared<SurgSim::Math::OdeSolverLinearEulerExplicitModified>(this);
break;
- case SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER:
- m_odeSolver = std::make_shared<OdeSolverLinearEulerImplicit>(this);
+ case SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT:
+ m_odeSolver = std::make_shared<SurgSim::Math::OdeSolverLinearEulerImplicit>(this);
break;
case SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC:
- m_odeSolver = std::make_shared<OdeSolverLinearStatic>(this);
+ m_odeSolver = std::make_shared<SurgSim::Math::OdeSolverLinearStatic>(this);
break;
case SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4:
- m_odeSolver = std::make_shared<OdeSolverLinearRungeKutta4>(this);
+ m_odeSolver = std::make_shared<SurgSim::Math::OdeSolverLinearRungeKutta4>(this);
break;
default:
SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
@@ -316,12 +368,45 @@ bool DeformableRepresentation::doWakeUp()
return false;
}
- // No assumption is made on the linear solver, we instantiate a general dense matrix solver
- m_odeSolver->setLinearSolver(std::make_shared<LinearSolveAndInverseDenseMatrix>());
+ // Set the linear solver with initial settings on the ode solver
+ switch (m_linearSolver)
+ {
+ case SurgSim::Math::LINEARSOLVER_LU:
+ m_odeSolver->setLinearSolver(std::make_shared<SurgSim::Math::LinearSparseSolveAndInverseLU>());
+ break;
+ case SurgSim::Math::LINEARSOLVER_CONJUGATEGRADIENT:
+ m_odeSolver->setLinearSolver(std::make_shared<SurgSim::Math::LinearSparseSolveAndInverseCG>());
+ break;
+ default:
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger())
+ << "Linear solver not initialized, the linear solver is invalid";
+ return false;
+ }
+
+ return true;
+}
+
+bool DeformableRepresentation::doWakeUp()
+{
+ std::shared_ptr<SurgSim::Framework::SceneElement> sceneElement = getSceneElement();
+ if (sceneElement != nullptr)
+ {
+ bool isIdentity = sceneElement->getPose().isApprox(SurgSim::Math::RigidTransform3d::Identity());
+ auto logger = SurgSim::Framework::Logger::getLogger("Physics");
+ SURGSIM_LOG_IF(!isIdentity, logger, WARNING) <<
+ "SceneElement '" << sceneElement->getName() << "' pose has been changed in between initialize() " <<
+ "and wakeUp() which can produce unrealistic behavior for the DeformableRepresentation '" <<
+ getName() << "'";
+ }
return true;
}
+void DeformableRepresentation::interpolatePreviousState(double t)
+{
+ m_previousState = std::make_shared<OdeState>(getPreviousState()->interpolate(*getCurrentState(), t));
+}
+
}; // namespace Physics
}; // namespace SurgSim
diff --git a/SurgSim/Physics/DeformableRepresentation.h b/SurgSim/Physics/DeformableRepresentation.h
index e486680..80efda2 100644
--- a/SurgSim/Physics/DeformableRepresentation.h
+++ b/SurgSim/Physics/DeformableRepresentation.h
@@ -21,8 +21,12 @@
#include <memory>
+#include "SurgSim/Math/LinearSparseSolveAndInverse.h"
+#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Math/OdeEquation.h"
#include "SurgSim/Math/OdeSolver.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
#include "SurgSim/Physics/Representation.h"
namespace SurgSim
@@ -55,72 +59,103 @@ public:
/// Destructor
virtual ~DeformableRepresentation();
- virtual void resetState() override;
+ void resetState() override;
+ /// Initialize the state variables to initialState
+ /// \param initialState is the state to be set as the starting state
virtual void setInitialState(std::shared_ptr<SurgSim::Math::OdeState> initialState);
+ /// Return the current state of the deformable representation
+ /// \return the current state
virtual const std::shared_ptr<SurgSim::Math::OdeState> getCurrentState() const;
+ /// Return the previous state of the deformable representation
+ /// \return the previous state
virtual const std::shared_ptr<SurgSim::Math::OdeState> getPreviousState() const;
+ /// Return the final state of the deformable representation
+ /// \return the final state
virtual const std::shared_ptr<SurgSim::Math::OdeState> getFinalState() const;
+ /// Declare a new previous state by interpolating between the old previous
+ /// state and the current state using parametric time variable t
+ /// \param t parametric time at which to calculate the new state
+ virtual void interpolatePreviousState(double t);
+
/// 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
+ /// \exception SurgSim::Framework::AssertionFailure raised if called after the component has been initialized.
void setIntegrationScheme(SurgSim::Math::IntegrationScheme integrationScheme);
/// Gets the numerical integration scheme
/// \return The integration scheme currently in use
+ /// \note Default is SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT
SurgSim::Math::IntegrationScheme getIntegrationScheme() const;
+ /// \return The ode solver (dependent on the integration scheme)
+ /// \note Will return nullptr if called before initialization.
+ std::shared_ptr<SurgSim::Math::OdeSolver> getOdeSolver() const;
+
+ /// Sets the linear algebraic solver
+ /// \param linearSolver The linear algebraic solver to use
+ /// \exception SurgSim::Framework::AssertionFailure raised if called after the component has been initialized.
+ void setLinearSolver(SurgSim::Math::LinearSolver linearSolver);
+
+ /// Gets the linear algebraic solver
+ /// \return The linear solver currently in use
+ /// \note Default is SurgSim::Math::LINEARSOLVER_LU
+ SurgSim::Math::LinearSolver getLinearSolver() 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;
+ const 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;
+ const SurgSim::Math::SparseMatrix& getExternalGeneralizedStiffness() const;
/// \return the external generalized damping matrix
- const SurgSim::Math::Matrix& getExternalGeneralizedDamping() const;
+ const SurgSim::Math::SparseMatrix& getExternalGeneralizedDamping() const;
+
+ Math::Matrix applyCompliance(const Math::OdeState& state, const Math::Matrix& b) override;
/// 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 const SurgSim::Math::Matrix& getComplianceMatrix() const;
- virtual void update(double dt) override;
+ void update(double dt) override;
- virtual void afterUpdate(double dt) override;
+ void afterUpdate(double dt) override;
- virtual void applyCorrection(double dt, const Eigen::VectorBlock<SurgSim::Math::Vector>& deltaVelocity) override;
+ void applyCorrection(double dt, const Eigen::VectorBlock<SurgSim::Math::Vector>& deltaVelocity) override;
/// Deactivate and call resetState
- void deactivateAndReset(void);
+ void deactivateAndReset();
/// 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;
+ void setCollisionRepresentation(std::shared_ptr<SurgSim::Collision::Representation> representation) override;
+
+ void setLocalPose(const SurgSim::Math::RigidTransform3d& pose) override;
+
+
protected:
- virtual bool doWakeUp() override;
+ bool doInitialize() override;
+ bool doWakeUp() override;
/// Transform a state using a given transformation
/// \param[in,out] state The state to be transformed
@@ -144,23 +179,15 @@ protected:
/// External generalized force, stiffness and damping applied on the deformable representation
/// @{
+ bool m_hasExternalGeneralizedForce;
SurgSim::Math::Vector m_externalGeneralizedForce;
- SurgSim::Math::Matrix m_externalGeneralizedStiffness;
- SurgSim::Math::Matrix m_externalGeneralizedDamping;
+ SurgSim::Math::SparseMatrix m_externalGeneralizedStiffness;
+ SurgSim::Math::SparseMatrix m_externalGeneralizedDamping;
+ bool m_previousHasExternalGeneralizedForce;
+ SurgSim::Math::SparseMatrix m_previousExternalGeneralizedStiffness;
+ SurgSim::Math::SparseMatrix m_previousExternalGeneralizedDamping;
/// @}
- /// 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;
@@ -168,8 +195,8 @@ protected:
/// 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;
+ /// Linear algebraic solver used
+ SurgSim::Math::LinearSolver m_linearSolver;
/// Ode solver (its type depends on the numerical integration scheme)
std::shared_ptr<SurgSim::Math::OdeSolver> m_odeSolver;
diff --git a/SurgSim/Physics/Fem-inl.h b/SurgSim/Physics/Fem-inl.h
new file mode 100644
index 0000000..39d624e
--- /dev/null
+++ b/SurgSim/Physics/Fem-inl.h
@@ -0,0 +1,116 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_FEM_INL_H
+#define SURGSIM_PHYSICS_FEM_INL_H
+
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Framework/Log.h"
+
+using SurgSim::DataStructures::PlyReader;
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+template <class VertexData, class Element>
+Fem<VertexData, Element>::Fem()
+{
+}
+
+template <class VertexData, class Element>
+size_t Fem<VertexData, Element>::addElement(std::shared_ptr<Element> element)
+{
+ m_elements.push_back(element);
+ return m_elements.size() - 1;
+}
+
+template <class VertexData, class Element>
+size_t Fem<VertexData, Element>::getNumElements() const
+{
+ return m_elements.size();
+}
+
+template <class VertexData, class Element>
+const std::vector<std::shared_ptr<Element>>&
+ Fem<VertexData, Element>::getElements() const
+{
+ return m_elements;
+}
+
+template <class VertexData, class Element>
+std::vector<std::shared_ptr<Element>>& Fem<VertexData, Element>::getElements()
+{
+ return m_elements;
+}
+
+template <class VertexData, class Element>
+std::shared_ptr<Element> Fem<VertexData, Element>::getElement(size_t id) const
+{
+ return m_elements[id];
+}
+
+template <class VertexData, class Element>
+size_t Fem<VertexData, Element>::addBoundaryCondition(size_t boundaryCondition)
+{
+ m_boundaryConditions.push_back(boundaryCondition);
+ return m_boundaryConditions.size() - 1;
+}
+
+template <class VertexData, class Element>
+const std::vector<size_t>& Fem<VertexData, Element>::getBoundaryConditions() const
+{
+ return m_boundaryConditions;
+}
+
+template <class VertexData, class Element>
+std::vector<size_t>& Fem<VertexData, Element>::getBoundaryConditions()
+{
+ return m_boundaryConditions;
+}
+
+template <class VertexData, class Element>
+size_t Fem<VertexData, Element>::getBoundaryCondition(size_t id) const
+{
+ return m_boundaryConditions[id];
+}
+template <class VertexData, class Element> template <class PlyType, class FemType>
+bool Fem<VertexData, Element>::loadFemFile(const std::string& filename)
+{
+ SurgSim::DataStructures::PlyReader reader(filename);
+ if (!reader.isValid())
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger())
+ << "'" << filename << "' is an invalid .ply file.";
+ return false;
+ }
+
+ auto delegate = std::make_shared<PlyType>(
+ std::dynamic_pointer_cast<FemType>(this->shared_from_this()));
+ if (!reader.parseWithDelegate(delegate))
+ {
+ SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger())
+ << "The input file '" << filename << "' does not have the property required by FEM element mesh.";
+ return false;
+ }
+
+ return true;
+}
+
+} // namespace Physics
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM_INL_H
diff --git a/SurgSim/Physics/Fem.h b/SurgSim/Physics/Fem.h
new file mode 100644
index 0000000..03c66e0
--- /dev/null
+++ b/SurgSim/Physics/Fem.h
@@ -0,0 +1,104 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_FEM_H
+#define SURGSIM_PHYSICS_FEM_H
+
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Framework/Asset.h"
+#include "SurgSim/Physics/FemElementStructs.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+/// Base class for a data structure for holding FEM mesh data of different dimensions
+///
+/// Fem itself should not be used directly itself as it contains no override for doLoad since the implementation is
+/// dependent on the dimension of the FEM you are trying to load. Each dimension overrides the doLoad function present
+/// in Asset using its own version of an FemPlyReaderDelegate. Each dimension supports loading both linear and
+/// corotational models.
+///
+/// \tparam VertexData Type of extra data stored in each vertex
+/// \tparam Element Type of FEM element the mesh will be storing
+/// \sa TriangleMesh
+template <class VertexData, class Element>
+class Fem : public SurgSim::DataStructures::Vertices<VertexData>, public SurgSim::Framework::Asset,
+ public std::enable_shared_from_this<Fem<VertexData, Element>>
+{
+public:
+ /// Default constructor
+ Fem();
+
+ /// Adds FEM element to mesh of Element template type
+ /// \param element A shared pointer of the Element template type
+ /// \return The new size of the vector of elements
+ size_t addElement(std::shared_ptr<Element> element);
+
+ /// Gets number of FEM elements in the mesh
+ /// \return The number of FEM elements stored
+ size_t getNumElements() const;
+
+ /// Gets entire FEM element vector
+ /// \return A const vector of all FEM elements stored in the mesh
+ const std::vector<std::shared_ptr<Element>>& getElements() const;
+
+ /// Gets entire FEM element vector (non-const)
+ /// \return A vector of all FEM elements stored in the mesh
+ std::vector<std::shared_ptr<Element>>& getElements();
+
+ /// Retrieve a specific element from the mesh
+ /// \param id The id in the element vector
+ /// \return A shared pointer to the element
+ std::shared_ptr<Element> getElement(size_t id) const;
+
+ /// Add boundary condition to mesh
+ /// \param boundaryCondition A vertex id that has a boundary condition
+ /// \return The new size of the vector of boundary conditions
+ size_t addBoundaryCondition(size_t boundaryCondition);
+
+ /// Gets entire vector of boundary conditions
+ /// \return A vector of boundary conditions
+ const std::vector<size_t>& getBoundaryConditions() const;
+
+ /// Gets entire vector of boundary conditions (non-const)
+ /// \return A vector of boundary conditions
+ std::vector<size_t>& getBoundaryConditions();
+
+ /// Retrieves a specific boundary condition
+ /// \param id The id of the boundary condition in the vector
+ /// \return The vertex id which has a boundary condition
+ size_t getBoundaryCondition(size_t id) const;
+
+protected:
+ /// Shared loading method for all 3 dimensions
+ /// \note Assign template class to the proper dimension PlyReaderDelegate
+ template <class PlyType, class FemType>
+ bool loadFemFile(const std::string& filename);
+
+ /// Vector of individual elements
+ std::vector<std::shared_ptr<Element>> m_elements;
+
+ /// Vector of vertex ids that have boundary conditions
+ std::vector<size_t> m_boundaryConditions;
+};
+
+} // namespace Physics
+} // namespace SurgSim
+
+#include "SurgSim/Physics/Fem-inl.h"
+
+#endif // SURGSIM_PHYSICS_FEM_H
diff --git a/SurgSim/Physics/Fem1D.cpp b/SurgSim/Physics/Fem1D.cpp
new file mode 100644
index 0000000..3832e90
--- /dev/null
+++ b/SurgSim/Physics/Fem1D.cpp
@@ -0,0 +1,35 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Fem1D.h"
+#include "SurgSim/Physics/Fem1DPlyReaderDelegate.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Asset, SurgSim::Physics::Fem1D, Fem1D)
+
+Fem1D::Fem1D()
+{
+}
+
+bool Fem1D::doLoad(const std::string& filePath)
+{
+ return loadFemFile<Fem1DPlyReaderDelegate, Fem1D>(filePath);
+}
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem1D.h b/SurgSim/Physics/Fem1D.h
new file mode 100644
index 0000000..19beaa7
--- /dev/null
+++ b/SurgSim/Physics/Fem1D.h
@@ -0,0 +1,45 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_FEM1D_H
+#define SURGSIM_PHYSICS_FEM1D_H
+
+#include "SurgSim/Physics/Fem.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+SURGSIM_STATIC_REGISTRATION(Fem1D);
+
+/// Fem class data structure implementation for 1-Dimensional FEMs
+/// \sa Fem
+class Fem1D : public Fem<FemElementStructs::RotationVectorData, FemElementStructs::FemElement1DParameter>
+{
+public:
+ Fem1D();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::Fem1D);
+
+protected:
+ // Asset API override
+ bool doLoad(const std::string& filePath) override;
+};
+
+} // namespace Physics
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM1D_H
diff --git a/SurgSim/Physics/Fem1DElementBeam.cpp b/SurgSim/Physics/Fem1DElementBeam.cpp
index 32470cf..e34d6f4 100644
--- a/SurgSim/Physics/Fem1DElementBeam.cpp
+++ b/SurgSim/Physics/Fem1DElementBeam.cpp
@@ -15,10 +15,10 @@
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/OdeEquation.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;
@@ -31,28 +31,33 @@ namespace SurgSim
namespace Physics
{
+SURGSIM_REGISTER(SurgSim::Physics::FemElement, SurgSim::Physics::Fem1DElementBeam, Fem1DElementBeam)
-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)
+Fem1DElementBeam::Fem1DElementBeam()
{
- // 6 dof per node (x, y, z, thetaX, thetaY, thetaZ)
- setNumDofPerNode(6);
+ initializeMembers();
+}
+Fem1DElementBeam::Fem1DElementBeam(std::array<size_t, 2> nodeIds)
+{
+ initializeMembers();
m_nodeIds.assign(nodeIds.cbegin(), nodeIds.cend());
}
+Fem1DElementBeam::Fem1DElementBeam(std::shared_ptr<FemElementStructs::FemElementParameter> elementData)
+{
+ initializeMembers();
+ auto element1DData = std::dynamic_pointer_cast<FemElementStructs::FemElement1DParameter>(elementData);
+ SURGSIM_ASSERT(element1DData != nullptr) << "Incorrect struct type passed";
+ SURGSIM_ASSERT(element1DData->nodeIds.size() == 2) << "Incorrect number of nodes for a Fem1D Beam";
+ m_nodeIds.assign(element1DData->nodeIds.begin(), element1DData->nodeIds.end());
+ setShearingEnabled(element1DData->enableShear);
+ setRadius(element1DData->radius);
+ setMassDensity(element1DData->massDensity);
+ setPoissonRatio(element1DData->poissonRatio);
+ setYoungModulus(element1DData->youngModulus);
+}
+
void Fem1DElementBeam::setRadius(double radius)
{
SURGSIM_ASSERT(radius != 0.0) << "The beam radius cannot be set to 0";
@@ -84,6 +89,26 @@ double Fem1DElementBeam::getVolume(const SurgSim::Math::OdeState& state) const
return m_A * (B - A).norm();
}
+void Fem1DElementBeam::initializeMembers()
+{
+ 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);
+}
+
void Fem1DElementBeam::initialize(const SurgSim::Math::OdeState& state)
{
// Test the validity of the physical parameters
@@ -106,84 +131,16 @@ void Fem1DElementBeam::initialize(const SurgSim::Math::OdeState& state)
computeInitialRotation(state);
// Pre-compute the mass and stiffness matrix
- computeMass(state, &m_M);
- computeStiffness(state, &m_K);
+ computeMass(state);
+ computeStiffness(state);
}
-void Fem1DElementBeam::addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F, double scale)
+SurgSim::Math::Matrix33d Fem1DElementBeam::getInitialRotation() const
{
- 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);
+ return m_R0.block<3, 3>(0, 0);
}
-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)
+void Fem1DElementBeam::computeMass(const SurgSim::Math::OdeState& state)
{
double& L = m_restLength;
double L2 = L * L;
@@ -243,22 +200,19 @@ void Fem1DElementBeam::computeMass(const SurgSim::Math::OdeState& state,
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)
+void Fem1DElementBeam::computeStiffness(const SurgSim::Math::OdeState& state)
{
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
+ // General expression 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)
@@ -343,7 +297,7 @@ void Fem1DElementBeam::computeInitialRotation(const SurgSim::Math::OdeState& sta
Vector3d j, k;
SURGSIM_ASSERT(SurgSim::Math::buildOrthonormalBasis(&i, &j, &k))
- << "Invalid beam formed by extremities A=(" << A.transpose() << ") B=(" << B.transpose() << ")";
+ << "Invalid beam formed by extremities A=(" << A.transpose() << ") B=(" << B.transpose() << ")";
// Set up a temporary 3x3 initial rotation matrix
SurgSim::Math::Matrix33d rotation3x3;
@@ -360,8 +314,8 @@ void Fem1DElementBeam::computeInitialRotation(const SurgSim::Math::OdeState& sta
}
SurgSim::Math::Vector Fem1DElementBeam::computeCartesianCoordinate(
- const SurgSim::Math::OdeState& state,
- const SurgSim::Math::Vector& naturalCoordinate) const
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const
{
SURGSIM_ASSERT(isValidCoordinate(naturalCoordinate)) << "naturalCoordinate must be normalized and length 2.";
@@ -377,9 +331,23 @@ SurgSim::Math::Vector Fem1DElementBeam::computeCartesianCoordinate(
return cartesianCoordinate;
}
+void Fem1DElementBeam::doUpdateFMDK(const Math::OdeState& state, int options)
+{
+ if (options & SurgSim::Math::OdeEquationUpdate::ODEEQUATIONUPDATE_F)
+ {
+ Eigen::Matrix<double, 12, 1> x;
+
+ // 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);
+ m_f = -m_K * (x - m_x0);
+ }
+}
+
SurgSim::Math::Vector Fem1DElementBeam::computeNaturalCoordinate(
- const SurgSim::Math::OdeState& state,
- const SurgSim::Math::Vector& cartesianCoordinate) const
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& cartesianCoordinate) const
{
SURGSIM_FAILURE() << "Function " << __FUNCTION__ << " not yet implemented.";
return SurgSim::Math::Vector3d::Zero();
diff --git a/SurgSim/Physics/Fem1DElementBeam.h b/SurgSim/Physics/Fem1DElementBeam.h
index e494d09..19dbb64 100644
--- a/SurgSim/Physics/Fem1DElementBeam.h
+++ b/SurgSim/Physics/Fem1DElementBeam.h
@@ -26,6 +26,8 @@ namespace SurgSim
namespace Physics
{
+SURGSIM_STATIC_REGISTRATION(Fem1DElementBeam);
+
/// 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
@@ -36,10 +38,21 @@ 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.
+ Fem1DElementBeam();
+
+ /// Constructor
+ /// \param nodeIds An array of 2 node ids defining this beam element with respect to a
+ /// DeformableRepresentationState which is passed to the initialize method.
explicit Fem1DElementBeam(std::array<size_t, 2> nodeIds);
+ /// Constructor for FemElement object factory
+ /// \param elementData A FemElement struct defining this beam element with respect to a
+ /// DeformableRepresentationState which is passed to the initialize method.
+ /// \exception SurgSim::Framework::AssertionFailure if nodeIds has a size different than 2
+ explicit Fem1DElementBeam(std::shared_ptr<FemElementStructs::FemElementParameter> elementData);
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::Fem1DElementBeam)
+
/// Sets the beam's circular cross-section radius
/// \param radius The radius of the beam
void setRadius(double radius);
@@ -48,15 +61,9 @@ public:
/// \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;
+ 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;
+ double getVolume(const SurgSim::Math::OdeState& state) const override;
/// Gets whether shearing is enabled for the element
/// \return True if shearing is enabled
@@ -68,83 +75,21 @@ public:
/// \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;
+ SurgSim::Math::Vector computeCartesianCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const override;
+
+ SurgSim::Math::Vector computeNaturalCoordinate(
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& cartesianCoordinate) const override;
+
+ /// \return the initial beam rotation
+ SurgSim::Math::Matrix33d getInitialRotation() const;
protected:
+ /// Initializes variables needed before Initialize() is called
+ void initializeMembers();
+
/// 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
@@ -152,14 +97,13 @@ protected:
/// 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);
+ void computeStiffness(const SurgSim::Math::OdeState& state);
/// 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);
+ void computeMass(const SurgSim::Math::OdeState& state);
+
+ void doUpdateFMDK(const Math::OdeState& state, int options) override;
/// The element's rest state
Eigen::Matrix<double, 12, 1> m_x0;
@@ -167,12 +111,8 @@ protected:
/// 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;
diff --git a/SurgSim/Physics/Fem1DLocalization.cpp b/SurgSim/Physics/Fem1DLocalization.cpp
new file mode 100644
index 0000000..4349514
--- /dev/null
+++ b/SurgSim/Physics/Fem1DLocalization.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/Physics/Fem1DLocalization.h"
+
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/FemElement.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+Fem1DLocalization::Fem1DLocalization(
+ std::shared_ptr<Representation> representation,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition) :
+ FemLocalization(representation, localPosition)
+{
+}
+
+Fem1DLocalization::~Fem1DLocalization()
+{
+}
+
+bool Fem1DLocalization::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);
+}
+
+bool Fem1DLocalization::moveClosestTo(const Math::Vector3d& point, bool* hasReachedEnd)
+{
+ if (hasReachedEnd != nullptr)
+ {
+ *hasReachedEnd = false;
+ }
+
+ auto femRepresentation = std::static_pointer_cast<FemRepresentation>(getRepresentation());
+ auto& currentState = femRepresentation->getCurrentState();
+
+ double closestDistance = std::numeric_limits<double>::max(), newDistance;
+ Math::Vector3d closestPoint, newPoint;
+ std::array<Math::Vector3d, 2> closestNodePositions;
+ size_t closestNodeIndex = std::numeric_limits<size_t>::max();
+
+ auto numFemElements = femRepresentation->getNumFemElements();
+ for (size_t i = 0; i < numFemElements; ++i)
+ {
+ auto femElement = femRepresentation->getFemElement(i);
+ const auto& nodeIds = femElement->getNodeIds();
+ std::array<Math::Vector3d, 2> nodePositions = {currentState->getPosition(nodeIds[0]),
+ currentState->getPosition(nodeIds[1])
+ };
+
+ Math::distancePointSegment(point, nodePositions[0], nodePositions[1], &newPoint);
+ newDistance = (newPoint - point).norm();
+ if (newDistance < closestDistance)
+ {
+ closestNodeIndex = i;
+ closestDistance = newDistance;
+ closestPoint = newPoint;
+ closestNodePositions[0] = nodePositions[0];
+ closestNodePositions[1] = nodePositions[1];
+ }
+ }
+
+ if (closestNodeIndex < numFemElements)
+ {
+ Math::Vector2d bary;
+ Math::barycentricCoordinates(closestPoint, closestNodePositions[0], closestNodePositions[1], &bary);
+ auto position = getLocalPosition();
+ position.index = closestNodeIndex;
+ position.coordinate[0] = bary[0];
+ position.coordinate[1] = bary[1];
+ if (hasReachedEnd != nullptr)
+ {
+ *hasReachedEnd =
+ (closestNodeIndex == 0 && std::abs(position.coordinate[0] - 1.0) < Math::Geometry::DistanceEpsilon) ||
+ (closestNodeIndex == numFemElements - 1 &&
+ std::abs(position.coordinate[1] - 1.0) < Math::Geometry::DistanceEpsilon);
+ }
+ setLocalPosition(position);
+ return true;
+ }
+
+ return false;
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem1DLocalization.h b/SurgSim/Physics/Fem1DLocalization.h
new file mode 100644
index 0000000..d10d87d
--- /dev/null
+++ b/SurgSim/Physics/Fem1DLocalization.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_PHYSICS_FEM1DLOCALIZATION_H
+#define SURGSIM_PHYSICS_FEM1DLOCALIZATION_H
+
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/Physics/FemLocalization.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Implementation of Localization for Fem1DRepresentation
+///
+/// Fem1DLocalization tracks the global coordinates of an IndexedLocalCoordinate associated with an
+/// Fem1DRepresentation. The IndexedLocalCoordinate must be related to an FemElement (the index is an FemElement id and
+/// the local coordinates are the barycentric coordinates of the nodes in this FemElement).
+class Fem1DLocalization : public FemLocalization
+{
+public:
+ /// Constructor
+ /// \param representation The representation to assign to this localization.
+ /// \param localCoordinate The indexed local coordinate relative to the representation.
+ Fem1DLocalization(std::shared_ptr<Representation> representation,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localCoordinate);
+
+ /// Destructor
+ virtual ~Fem1DLocalization();
+
+ bool isValidRepresentation(std::shared_ptr<Representation> representation) override;
+
+ bool moveClosestTo(const Math::Vector3d& point, bool *hasReachedEnd) override;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM1DLOCALIZATION_H
diff --git a/SurgSim/Physics/Fem1DPlyReaderDelegate.cpp b/SurgSim/Physics/Fem1DPlyReaderDelegate.cpp
index 6dba519..926db83 100644
--- a/SurgSim/Physics/Fem1DPlyReaderDelegate.cpp
+++ b/SurgSim/Physics/Fem1DPlyReaderDelegate.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2014, SimQuest Solutions Inc.
+// Copyright 2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -13,26 +13,28 @@
// 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"
+
+using SurgSim::DataStructures::PlyReader;
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())
+Fem1DPlyReaderDelegate::Fem1DPlyReaderDelegate()
{
}
+Fem1DPlyReaderDelegate::Fem1DPlyReaderDelegate(std::shared_ptr<Fem1D> mesh) :
+ m_enableShear(false),
+ m_mesh(mesh)
+{
+ SURGSIM_ASSERT(mesh != nullptr) << "The mesh cannot be null.";
+ mesh->clear();
+}
+
std::string Fem1DPlyReaderDelegate::getElementName() const
{
return "1d_element";
@@ -41,12 +43,11 @@ std::string Fem1DPlyReaderDelegate::getElementName() const
bool Fem1DPlyReaderDelegate::registerDelegate(PlyReader* reader)
{
FemPlyReaderDelegate::registerDelegate(reader);
-
// Radius Processing
reader->requestElement(
"radius",
std::bind(
- &Fem1DPlyReaderDelegate::beginRadius, this, std::placeholders::_1, std::placeholders::_2),
+ &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);
@@ -62,14 +63,55 @@ bool Fem1DPlyReaderDelegate::fileIsAcceptable(const PlyReader& reader)
return result;
}
+void Fem1DPlyReaderDelegate::endParseFile()
+{
+ for (auto element : m_mesh->getElements())
+ {
+ element->radius = m_radius;
+ element->enableShear = m_enableShear;
+ if (!m_hasPerElementMaterial && m_hasMaterial)
+ {
+ element->massDensity = m_materialData.massDensity;
+ element->poissonRatio = m_materialData.poissonRatio;
+ element->youngModulus = m_materialData.youngModulus;
+ }
+ }
+ m_mesh->update();
+}
+
+void Fem1DPlyReaderDelegate::processVertex(const std::string& elementName)
+{
+ FemElementStructs::RotationVectorData data;
+
+ if (m_hasRotationDOF)
+ {
+ data.thetaX = m_vertexData.thetaX;
+ data.thetaY = m_vertexData.thetaY;
+ data.thetaZ = m_vertexData.thetaZ;
+ }
+
+ Fem1D::VertexType vertex(SurgSim::Math::Vector3d(m_vertexData.x, m_vertexData.y, m_vertexData.z), data);
+
+ m_mesh->addVertex(vertex);
+}
+
void Fem1DPlyReaderDelegate::processFemElement(const std::string& elementName)
{
- SURGSIM_ASSERT(m_femData.vertexCount == 2) << "Cannot process 1D Element with "
- << m_femData.vertexCount << " vertices.";
+ SURGSIM_ASSERT(m_elementData.vertexCount == 2) << "Cannot process 1D Element with "
+ << m_elementData.vertexCount << " vertices.";
+
+ auto femElement = std::make_shared<FemElementStructs::FemElement1DParameter>();
+ femElement->nodeIds.resize(m_elementData.vertexCount);
+ std::copy(m_elementData.indices, m_elementData.indices + m_elementData.vertexCount, femElement->nodeIds.data());
- 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));
+ if (m_hasPerElementMaterial)
+ {
+ femElement->massDensity = m_elementData.massDensity;
+ femElement->poissonRatio = m_elementData.poissonRatio;
+ femElement->youngModulus = m_elementData.youngModulus;
+ }
+
+ m_mesh->addElement(femElement);
}
void* Fem1DPlyReaderDelegate::beginRadius(const std::string& elementName, size_t radiusCount)
@@ -82,15 +124,10 @@ void Fem1DPlyReaderDelegate::endRadius(const std::string& elementName)
SURGSIM_ASSERT(SurgSim::Math::isValid(m_radius)) << "No radius information processed.";
}
-void Fem1DPlyReaderDelegate::endParseFile()
+void Fem1DPlyReaderDelegate::processBoundaryCondition(const std::string& elementName)
{
- for (size_t i = 0; i < m_fem->getNumFemElements(); ++i)
- {
- std::static_pointer_cast<Fem1DElementBeam>(m_fem->getFemElement(i))->setRadius(m_radius);
- }
-
- FemPlyReaderDelegate::endParseFile();
+ m_mesh->addBoundaryCondition(static_cast<size_t>(m_boundaryConditionData));
}
-}; // namespace Physics
-}; // namespace SurgSim
\ No newline at end of file
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem1DPlyReaderDelegate.h b/SurgSim/Physics/Fem1DPlyReaderDelegate.h
index 2dfe0c7..bef8c40 100644
--- a/SurgSim/Physics/Fem1DPlyReaderDelegate.h
+++ b/SurgSim/Physics/Fem1DPlyReaderDelegate.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2014, SimQuest Solutions Inc.
+// Copyright 2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,33 +16,41 @@
#ifndef SURGSIM_PHYSICS_FEM1DPLYREADERDELEGATE_H
#define SURGSIM_PHYSICS_FEM1DPLYREADERDELEGATE_H
+#include <array>
#include <memory>
-#include <string>
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Physics/Fem1D.h"
#include "SurgSim/Physics/FemPlyReaderDelegate.h"
namespace SurgSim
{
namespace Physics
{
-class Fem1DRepresentation;
-/// Implementation of PlyReaderDelegate for Fem1DRepresentation
-class Fem1DPlyReaderDelegate : public SurgSim::Physics::FemPlyReaderDelegate
+class Fem1DPlyReaderDelegate : public FemPlyReaderDelegate
{
public:
- /// Constructor
- /// \param fem The object that is updated when PlyReader::parseFile is called.
- explicit Fem1DPlyReaderDelegate(std::shared_ptr<Fem1DRepresentation> fem);
+ /// Default constructor.
+ Fem1DPlyReaderDelegate();
+
+ /// Constructor.
+ /// \param mesh The mesh to be used, it will be cleared by the constructor.
+ explicit Fem1DPlyReaderDelegate(std::shared_ptr<Fem1D> mesh);
protected:
- virtual std::string getElementName() const override;
+ std::string getElementName() const override;
+
+ bool registerDelegate(PlyReader* reader) override;
+
+ bool fileIsAcceptable(const PlyReader& reader) override;
+
+ void endParseFile() override;
- virtual bool registerDelegate(SurgSim::DataStructures::PlyReader* reader) override;
- virtual bool fileIsAcceptable(const SurgSim::DataStructures::PlyReader& reader) override;
+ void processVertex(const std::string& elementName) override;
- virtual void endParseFile() override;
- virtual void processFemElement(const std::string& elementName) override;
+ void processFemElement(const std::string& elementName) override;
/// Callback function, begin the processing of radius.
/// \param elementName Name of the element.
@@ -54,11 +62,20 @@ protected:
/// \param elementName Name of the element.
void endRadius(const std::string& elementName);
+ void processBoundaryCondition(const std::string& elementName) override;
+
private:
+ /// Element's radius information
double m_radius;
+ /// Element's shear information
+ bool m_enableShear;
+
+ /// Fem1D mesh asset to contain the ply file information
+ std::shared_ptr<Fem1D> m_mesh;
};
+
} // namespace Physics
} // namespace SurgSim
-#endif // SURGSIM_PHYSICS_FEM1DPLYREADERDELEGATE_H
\ No newline at end of file
+#endif // SURGSIM_PHYSICS_FEM1DPLYREADERDELEGATE_H
diff --git a/SurgSim/Physics/Fem1DRepresentation.cpp b/SurgSim/Physics/Fem1DRepresentation.cpp
index 372d71d..c2ba384 100644
--- a/SurgSim/Physics/Fem1DRepresentation.cpp
+++ b/SurgSim/Physics/Fem1DRepresentation.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -13,19 +13,20 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/DataStructures/Location.h"
#include "SurgSim/Framework/Assert.h"
-#include "SurgSim/Framework/Log.h"
-#include "SurgSim/Math/LinearSolveAndInverse.h"
+#include "SurgSim/Framework/Asset.h"
#include "SurgSim/Math/OdeState.h"
-#include "SurgSim/Physics/Fem1DPlyReaderDelegate.h"
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+#include "SurgSim/Physics/Fem1DLocalization.h"
#include "SurgSim/Physics/Fem1DRepresentation.h"
-#include "SurgSim/Physics/Fem1DRepresentationLocalization.h"
#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/Localization.h"
namespace
{
-
void transformVectorByBlockOf3(const SurgSim::Math::RigidTransform3d& transform, SurgSim::Math::Vector* x,
bool rotationOnly = false)
{
@@ -34,65 +35,122 @@ void transformVectorByBlockOf3(const SurgSim::Math::RigidTransform3d& transform,
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)";
+ << "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
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Fem1DRepresentation, std::shared_ptr<SurgSim::Framework::Asset>, Fem, getFem,
+ setFem);
+ SURGSIM_ADD_SETTER(Fem1DRepresentation, std::string, FemFileName, loadFem)
+ // Reminder: m_numDofPerNode is held by DeformableRepresentation but needs to be set by all
+ // concrete derived classes
m_numDofPerNode = 6;
+ m_fem = std::make_shared<Fem1D>();
}
Fem1DRepresentation::~Fem1DRepresentation()
{
}
-RepresentationType Fem1DRepresentation::getType() const
+void Fem1DRepresentation::loadFem(const std::string& fileName)
+{
+ auto mesh = std::make_shared<Fem1D>();
+ mesh->load(fileName);
+ setFem(mesh);
+}
+
+void Fem1DRepresentation::setFem(std::shared_ptr<Framework::Asset> mesh)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "The Fem cannot be set after initialization";
+
+ SURGSIM_ASSERT(mesh != nullptr) << "Mesh for Fem1DRepresentation cannot be a nullptr";
+ auto femMesh = std::dynamic_pointer_cast<Fem1D>(mesh);
+ SURGSIM_ASSERT(femMesh != nullptr)
+ << "Mesh for Fem1DRepresentation needs to be a SurgSim::Physics::Fem1D";
+ m_fem = femMesh;
+ auto state = std::make_shared<Math::OdeState>();
+
+ state->setNumDof(getNumDofPerNode(), m_fem->getNumVertices());
+ for (size_t i = 0; i < m_fem->getNumVertices(); i++)
+ {
+ state->getPositions().segment<3>(getNumDofPerNode() * i) = m_fem->getVertexPosition(i);
+ }
+ for (auto boundaryCondition : m_fem->getBoundaryConditions())
+ {
+ state->addBoundaryCondition(boundaryCondition);
+ }
+
+ // If we have elements, ensure that they are all of the same nature
+ if (femMesh->getNumElements() > 0)
+ {
+ const auto& e0 = femMesh->getElement(0);
+ for (auto const& e : femMesh->getElements())
+ {
+ SURGSIM_ASSERT(e->nodeIds.size() == e0->nodeIds.size()) <<
+ "Cannot mix and match elements of different nature." <<
+ " Found an element with " << e->nodeIds.size() << " nodes but was expecting " << e0->nodeIds.size();
+ }
+
+ // If the FemElement types hasn't been registered yet, let's set a default one
+ if (getFemElementType().empty())
+ {
+ if (e0->nodeIds.size() == 2)
+ {
+ Fem1DElementBeam beam;
+ setFemElementType(beam.getClassName());
+ }
+ }
+ }
+
+ setInitialState(state);
+}
+
+std::shared_ptr<Fem1D> Fem1DRepresentation::getFem() const
{
- return REPRESENTATION_TYPE_FEM1D;
+ return m_fem;
}
void Fem1DRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
- SurgSim::Math::Vector& generalizedForce,
- const SurgSim::Math::Matrix& K,
- const SurgSim::Math::Matrix& D)
+ const Math::Vector& generalizedForce,
+ const Math::Matrix& K,
+ const Math::Matrix& D)
{
+ using Math::SparseMatrix;
+
const size_t dofPerNode = getNumDofPerNode();
- const SurgSim::Math::Matrix::Index expectedSize = static_cast<const SurgSim::Math::Matrix::Index>(dofPerNode);
+ const Math::Matrix::Index expectedSize = static_cast<const 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;
+ "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;
+ "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;
+ "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)";
+ std::shared_ptr<Fem1DLocalization> localization1D =
+ std::dynamic_pointer_cast<Fem1DLocalization>(localization);
+ SURGSIM_ASSERT(localization1D != nullptr) << "Invalid localization type (not a Fem1DLocalization)";
const size_t elementId = localization1D->getLocalPosition().index;
- const SurgSim::Math::Vector& coordinate = localization1D->getLocalPosition().coordinate;
+ const Math::Vector& coordinate = localization1D->getLocalPosition().coordinate;
std::shared_ptr<FemElement> element = getFemElement(elementId);
size_t index = 0;
@@ -112,13 +170,15 @@ void Fem1DRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localizati
{
if (K.size() != 0)
{
- m_externalGeneralizedStiffness.block(dofPerNode * nodeId1, dofPerNode * nodeId2,
- dofPerNode, dofPerNode) += coordinate[index1] * coordinate[index2] * K;
+ Math::addSubMatrix(coordinate[index1] * coordinate[index2] * K,
+ nodeId1, nodeId2,
+ &m_externalGeneralizedStiffness, true);
}
if (D.size() != 0)
{
- m_externalGeneralizedDamping.block(dofPerNode * nodeId1, dofPerNode * nodeId2,
- dofPerNode, dofPerNode) += coordinate[index1] * coordinate[index2] * D;
+ Math::addSubMatrix(coordinate[index1] * coordinate[index2] * D,
+ nodeId1, nodeId2,
+ &m_externalGeneralizedDamping, true);
}
index2++;
}
@@ -126,38 +186,82 @@ void Fem1DRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localizati
index1++;
}
}
+ m_externalGeneralizedStiffness.makeCompressed();
+ m_externalGeneralizedDamping.makeCompressed();
+ m_hasExternalGeneralizedForce = true;
}
-bool Fem1DRepresentation::doWakeUp()
+
+std::shared_ptr<Localization> Fem1DRepresentation::createNodeLocalization(size_t nodeId)
{
- using SurgSim::Math::LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix;
+ DataStructures::IndexedLocalCoordinate coordinate;
- if (!FemRepresentation::doWakeUp())
+ SURGSIM_ASSERT(nodeId >= 0 && nodeId < getCurrentState()->getNumNodes()) << "Invalid node id";
+
+ // Look for any element that contains this node
+ bool foundNodeId = false;
+ for (size_t elementId = 0; elementId < getNumFemElements(); elementId++)
{
- return false;
+ auto element = getFemElement(elementId);
+ auto found = std::find(element->getNodeIds().begin(), element->getNodeIds().end(), nodeId);
+ if (found != element->getNodeIds().end())
+ {
+ coordinate.index = elementId;
+ coordinate.coordinate.setZero(element->getNumNodes());
+ coordinate.coordinate[found - element->getNodeIds().begin()] = 1.0;
+ foundNodeId = true;
+ break;
+ }
}
+ SURGSIM_ASSERT(foundNodeId) << "Could not find any element containing the node " << nodeId;
- // 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>>());
+ // Fem1DLocalization will verify the coordinate (2nd parameter) based on
+ // the Fem1DRepresentation passed as 1st parameter.
+ return std::make_shared<Fem1DLocalization>(
+ std::static_pointer_cast<Physics::Representation>(getSharedPtr()), coordinate);
+}
- return true;
+std::shared_ptr<Localization> Fem1DRepresentation::createElementLocalization(
+ const DataStructures::IndexedLocalCoordinate& location)
+{
+ return std::make_shared<Fem1DLocalization>(
+ std::static_pointer_cast<Physics::Representation>(getSharedPtr()), location);
}
-void Fem1DRepresentation::transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
- const SurgSim::Math::RigidTransform3d& transform)
+std::shared_ptr<Localization> Fem1DRepresentation::createLocalization(const DataStructures::Location& location)
+{
+ if (location.index.hasValue())
+ {
+ return createNodeLocalization(*location.index);
+ }
+ else if (location.elementMeshLocalCoordinate.hasValue())
+ {
+ return createElementLocalization(*location.elementMeshLocalCoordinate);
+ }
+
+ SURGSIM_FAILURE() << "Fem1DRepresentation supports only node and element-based location (not triangle-based)";
+
+ return nullptr;
+}
+
+void Fem1DRepresentation::transformState(std::shared_ptr<Math::OdeState> state,
+ const Math::RigidTransform3d& transform)
{
transformVectorByBlockOf3(transform, &state->getPositions());
transformVectorByBlockOf3(transform, &state->getVelocities(), true);
}
-std::shared_ptr<FemPlyReaderDelegate> Fem1DRepresentation::getDelegate()
+bool Fem1DRepresentation::doInitialize()
{
- auto thisAsSharedPtr = std::static_pointer_cast<Fem1DRepresentation>(shared_from_this());
- auto readerDelegate = std::make_shared<Fem1DPlyReaderDelegate>(thisAsSharedPtr);
+ for (auto& element : m_fem->getElements())
+ {
+ std::shared_ptr<FemElement> femElement;
+ femElement = FemElement::getFactory().create(getFemElementType(), element);
+ m_femElements.push_back(femElement);
+ }
- return readerDelegate;
+ return FemRepresentation::doInitialize();
}
} // namespace Physics
-
} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem1DRepresentation.h b/SurgSim/Physics/Fem1DRepresentation.h
index 0270180..46e7270 100644
--- a/SurgSim/Physics/Fem1DRepresentation.h
+++ b/SurgSim/Physics/Fem1DRepresentation.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -19,17 +19,34 @@
#include <memory>
#include <string>
+#include "SurgSim/Framework/FrameworkConvert.h"
#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem1D.h"
#include "SurgSim/Physics/FemRepresentation.h"
namespace SurgSim
{
+namespace DataStructures
+{
+struct IndexedLocalCoordinate;
+struct Location;
+}
+namespace Framework
+{
+class Asset;
+}
+namespace Math
+{
+class OdeState;
+}
namespace Physics
{
-SURGSIM_STATIC_REGISTRATION(Fem1DRepresentation);
+class Localization;
-class FemPlyReaderDelegate;
+SURGSIM_STATIC_REGISTRATION(Fem1DRepresentation);
/// Finite Element Model 1D is a fem built with 1D FemElement
///
@@ -46,26 +63,43 @@ public:
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;
+ void loadFem(const std::string& fileName) override;
+
+ /// Sets the fem mesh asset
+ /// \param mesh The fem mesh to assign to this representation
+ /// \exception SurgSim::Framework::AssertionFailure if mesh is nullptr or it's actual type is not Fem1D
+ void setFem(std::shared_ptr<SurgSim::Framework::Asset> mesh);
+
+ /// \return The fem mesh asset as a Fem1D
+ std::shared_ptr<Fem1D> getFem() const;
+
+ void addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ const 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;
+ std::shared_ptr<Localization> createLocalization(const SurgSim::DataStructures::Location& location) override;
protected:
- virtual bool doWakeUp() override;
+ void transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
+ const SurgSim::Math::RigidTransform3d& transform) 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;
+ bool doInitialize() override;
private:
- virtual std::shared_ptr<FemPlyReaderDelegate> getDelegate() override;
+ /// Helper method: create a localization for a node
+ /// \param nodeId The node index
+ /// \return Localization of the node for this representation
+ std::shared_ptr<Localization> createNodeLocalization(size_t nodeId);
+
+ /// Helper method: create a localization for an element-based IndexedLocalCoordinate (beam)
+ /// \param location The IndexedLocalCoordinate defining a point on the element mesh
+ /// \return Localization of the point for this representation
+ std::shared_ptr<Localization> createElementLocalization(
+ const SurgSim::DataStructures::IndexedLocalCoordinate& location);
+
+ /// The Fem1DRepresentation's asset as a Fem1D
+ std::shared_ptr<Fem1D> m_fem;
};
} // namespace Physics
diff --git a/SurgSim/Physics/Fem1DRepresentationLocalization.cpp b/SurgSim/Physics/Fem1DRepresentationLocalization.cpp
deleted file mode 100644
index 4342ab9..0000000
--- a/SurgSim/Physics/Fem1DRepresentationLocalization.cpp
+++ /dev/null
@@ -1,98 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index be7029a..0000000
--- a/SurgSim/Physics/Fem1DRepresentationLocalization.h
+++ /dev/null
@@ -1,72 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/Fem2D.cpp b/SurgSim/Physics/Fem2D.cpp
new file mode 100644
index 0000000..b21a549
--- /dev/null
+++ b/SurgSim/Physics/Fem2D.cpp
@@ -0,0 +1,36 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Fem2D.h"
+#include "SurgSim/Physics/Fem2DPlyReaderDelegate.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Asset, SurgSim::Physics::Fem2D, Fem2D)
+
+Fem2D::Fem2D() : Fem()
+{
+}
+
+bool Fem2D::doLoad(const std::string& filePath)
+{
+ return loadFemFile<Fem2DPlyReaderDelegate, Fem2D>(filePath);
+}
+
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem2D.h b/SurgSim/Physics/Fem2D.h
new file mode 100644
index 0000000..a6e3614
--- /dev/null
+++ b/SurgSim/Physics/Fem2D.h
@@ -0,0 +1,46 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_FEM2D_H
+#define SURGSIM_PHYSICS_FEM2D_H
+
+#include "SurgSim/Physics/Fem.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+SURGSIM_STATIC_REGISTRATION(Fem2D);
+
+/// Fem class data structure implementation for 2-Dimensional FEMs
+/// \sa Fem
+class Fem2D : public Fem<FemElementStructs::RotationVectorData, FemElementStructs::FemElement2DParameter>
+{
+public:
+ /// Default constructor
+ Fem2D();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::Fem2D);
+
+protected:
+ // Asset API override
+ bool doLoad(const std::string& filePath) override;
+};
+
+} // namespace Physics
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM2D_H
diff --git a/SurgSim/Physics/Fem2DElementTriangle.cpp b/SurgSim/Physics/Fem2DElementTriangle.cpp
index d88a6a4..d95a81e 100644
--- a/SurgSim/Physics/Fem2DElementTriangle.cpp
+++ b/SurgSim/Physics/Fem2DElementTriangle.cpp
@@ -14,13 +14,15 @@
// limitations under the License.
#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/GaussLegendreQuadrature.h"
#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/OdeEquation.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::gaussQuadrature2DTriangle3Points;
using SurgSim::Math::getSubMatrix;
using SurgSim::Math::getSubVector;
using SurgSim::Math::setSubMatrix;
@@ -38,17 +40,32 @@ namespace SurgSim
namespace Physics
{
+SURGSIM_REGISTER(SurgSim::Physics::FemElement, SurgSim::Physics::Fem2DElementTriangle, Fem2DElementTriangle)
-Fem2DElementTriangle::Fem2DElementTriangle(std::array<size_t, 3> nodeIds)
- : m_restArea(0.0),
- m_thickness(0.0)
+Fem2DElementTriangle::Fem2DElementTriangle()
{
- // 6 dof per node (x, y, z, thetaX, thetaY, thetaZ)
- setNumDofPerNode(6);
+ initializeMembers();
+}
+Fem2DElementTriangle::Fem2DElementTriangle(std::array<size_t, 3> nodeIds)
+{
+ initializeMembers();
m_nodeIds.assign(nodeIds.cbegin(), nodeIds.cend());
}
+Fem2DElementTriangle::Fem2DElementTriangle(std::shared_ptr<FemElementStructs::FemElementParameter> elementData)
+{
+ initializeMembers();
+ auto element2DData = std::dynamic_pointer_cast<FemElementStructs::FemElement2DParameter>(elementData);
+ SURGSIM_ASSERT(element2DData != nullptr) << "Incorrect struct type passed";
+ SURGSIM_ASSERT(element2DData->nodeIds.size() == 3) << "Incorrect number of nodes for Fem2D Triangle";
+ m_nodeIds.assign(element2DData->nodeIds.begin(), element2DData->nodeIds.end());
+ setThickness(element2DData->thickness);
+ setMassDensity(element2DData->massDensity);
+ setPoissonRatio(element2DData->poissonRatio);
+ setYoungModulus(element2DData->youngModulus);
+}
+
void Fem2DElementTriangle::setThickness(double thickness)
{
SURGSIM_ASSERT(thickness != 0.0) << "The thickness cannot be set to 0";
@@ -71,19 +88,28 @@ double Fem2DElementTriangle::getVolume(const SurgSim::Math::OdeState& state) con
return m_thickness * (B - A).cross(C - A).norm() / 2.0;
}
+void Fem2DElementTriangle::initializeMembers()
+{
+ m_restArea = 0.0;
+ m_thickness = 0.0;
+
+ // 6 dof per node (x, y, z, thetaX, thetaY, thetaZ)
+ setNumDofPerNode(6);
+}
+
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) ?";
+ "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);
+ m_initialRotation = computeRotation(state);
// computeShapeFunctionsParameters needs the initial rotation and
// is required to compute the stiffness and mass matrices
@@ -94,176 +120,73 @@ void Fem2DElementTriangle::initialize(const SurgSim::Math::OdeState& state)
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)
+void Fem2DElementTriangle::computeLocalMembraneMass(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 18, 18>* localMassMatrix)
{
- // Assemble the mass matrix
- addMass(state, M);
-
- // No damping matrix as we are using linear elasticity (not visco-elasticity)
+ double mass = m_rho * m_restArea * m_thickness;
- // Assemble the stiffness matrix
- addStiffness(state, K);
+ for (size_t i = 0; i < 3; ++i)
+ {
+ size_t j = (i + 1) % 3;
+ size_t k = (j + 1) % 3;
- // Assemble the force vector
- addForce(state, F);
+ localMassMatrix->block<2, 2>(i * 6, i * 6).diagonal().setConstant(mass / 6.0);
+ localMassMatrix->block<2, 2>(i * 6, j * 6).diagonal().setConstant(mass / 12.0);
+ localMassMatrix->block<2, 2>(i * 6, k * 6).diagonal().setConstant(mass / 12.0);
+ }
}
-void Fem2DElementTriangle::addMatVec(const SurgSim::Math::OdeState& state, double alphaM, double alphaD,
- double alphaK, const SurgSim::Math::Vector& x, SurgSim::Math::Vector* F)
+void Fem2DElementTriangle::computeLocalPlateMass(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 18, 18>* localMassMatrix)
{
- 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);
+ double coefZ = 2.0 * m_restArea * m_rho * m_thickness;
+ double coefTheta = m_restArea * m_rho * (m_thickness * m_thickness * m_thickness) / 6.0;
- // Adds the mass contribution
- if (alphaM != 0.0)
+ for (size_t nodeId1 = 0; nodeId1 < 3; ++nodeId1)
{
- extractedResult = alphaM * (m_M * extractedX);
- addSubVector(extractedResult, m_nodeIds, 6, F);
+ for (size_t nodeId2 = 0; nodeId2 < 3; ++nodeId2)
+ {
+ localMassMatrix->block<3, 3>(6 * nodeId1 + 2, 6 * nodeId2 + 2) =
+ coefZ * m_integral_dT_d.block<3, 3>(3 * nodeId1, 3 * nodeId2) +
+ coefTheta * m_integralHxiHxj.block<3, 3>(3 * nodeId1, 3 * nodeId2) +
+ coefTheta * m_integralHyiHyj.block<3, 3>(3 * nodeId1, 3 * nodeId2);
+ }
}
+}
- // 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::computeLocalMass(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 18, 18>* localMassMatrix)
+{
+ localMassMatrix->setIdentity();
+ computeLocalMembraneMass(state, localMassMatrix);
+ computeLocalPlateMass(state, localMassMatrix);
}
void Fem2DElementTriangle::computeMass(const SurgSim::Math::OdeState& state,
- Eigen::Matrix<double, 18, 18>* M)
+ SurgSim::Math::Matrix* massMatrix)
{
- 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;
- }
+ computeLocalMass(state, &m_MLocal);
// 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)
+ for (size_t rowBlockId = 0; rowBlockId < 6; ++rowBlockId)
{
- auto MLocal3x3block = getSubMatrix(m_MLocal, rowId, rowId, 3, 3);
- setSubMatrix(rotation * MLocal3x3block * rotationTranspose, rowId, rowId, 3, 3, &m_M);
+ for (size_t colBlockId = 0; colBlockId < 6; ++colBlockId)
+ {
+ auto MLocal3x3block = getSubMatrix(m_MLocal, rowBlockId, colBlockId, 3, 3);
+ setSubMatrix(rotation * MLocal3x3block * rotationTranspose, rowBlockId, colBlockId, 3, 3, massMatrix);
+ }
}
}
-void Fem2DElementTriangle::computeStiffness(const SurgSim::Math::OdeState& state,
- Eigen::Matrix<double, 18, 18>* k)
+void Fem2DElementTriangle::computeLocalStiffness(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 18, 18>* localStiffnessMatrix)
{
// 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)
+ Matrix36Type membraneStrainDisplacement = Matrix36Type::Zero();
+ 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
@@ -285,51 +208,52 @@ void Fem2DElementTriangle::computeStiffness(const SurgSim::Math::OdeState& state
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);
+ membraneElasticMaterial *= m_E / (1.0 - m_nu * m_nu);
// Membrane local stiffness matrix = integral(strain:stress)
Matrix66Type membraneKLocal =
- membraneStrainDisplacement.transpose() * membraneElasticMaterial * membraneStrainDisplacement;
+ 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
+ // Thin-Plate stiffness matrix evaluation using a 3 point Gauss quadrature for exact integration (quadratic terms)
+ Matrix99Type plateKLocal = Matrix99Type::Zero();
+ for (size_t pointId = 0; pointId < 3; ++pointId)
+ {
+ const double xi = gaussQuadrature2DTriangle3Points[pointId].coordinateXi;
+ const double eta = gaussQuadrature2DTriangle3Points[pointId].coordinateEta;
+ const double weight = gaussQuadrature2DTriangle3Points[pointId].weight;
+
+ Matrix39Type strainDisplacementAtGaussPoint = batozStrainDisplacement(xi , eta);
+ plateKLocal += ((2.0 * m_restArea) * 0.5 * weight) *
+ strainDisplacementAtGaussPoint.transpose() * plateElasticMaterial *
+ strainDisplacementAtGaussPoint;
+ }
// 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)
+ localStiffnessMatrix->setIdentity();
+ for (size_t row = 0; row < 3; ++row)
{
- for(size_t column = 0; column < 3; ++column)
+ 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);
+ localStiffnessMatrix->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);
+ localStiffnessMatrix->block<3, 3>(6 * row + 2, 6 * column + 2) =
+ plateKLocal.block<3, 3>(3 * row, 3 * column);
}
}
+}
+
+void Fem2DElementTriangle::computeStiffness(const SurgSim::Math::OdeState& state,
+ SurgSim::Math::Matrix* stiffnessMatrix)
+{
+ computeLocalStiffness(state, &m_KLocal);
// Transformation Local -> Global
const SurgSim::Math::Matrix33d& rotation = m_initialRotation;
@@ -339,13 +263,15 @@ void Fem2DElementTriangle::computeStiffness(const SurgSim::Math::OdeState& state
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);
+ setSubMatrix(rotation * KLocal3x3block * rotationTranspose, rowBlockId, colBlockId, 3, 3, stiffnessMatrix);
}
}
}
-void Fem2DElementTriangle::computeInitialRotation(const SurgSim::Math::OdeState& state)
+SurgSim::Math::Matrix33d Fem2DElementTriangle::computeRotation(const SurgSim::Math::OdeState& state)
{
+ SurgSim::Math::Matrix33d rotation;
+
// Build (A; i, j, k) an orthonormal frame
// such that in the local frame, we have:
// A(0, 0)
@@ -356,29 +282,31 @@ void Fem2DElementTriangle::computeInitialRotation(const SurgSim::Math::OdeState&
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() << ")";
+ << "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() << ")";
+ << "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() << ")";
+ 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;
+ rotation.col(0) = i;
+ rotation.col(1) = j;
+ rotation.col(2) = k;
+
+ return rotation;
}
SurgSim::Math::Vector Fem2DElementTriangle::computeCartesianCoordinate(
- const SurgSim::Math::OdeState& state,
- const SurgSim::Math::Vector& naturalCoordinate) const
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const
{
SURGSIM_ASSERT(isValidCoordinate(naturalCoordinate)) << "naturalCoordinate must be normalized and length 3.";
@@ -395,21 +323,389 @@ SurgSim::Math::Vector Fem2DElementTriangle::computeCartesianCoordinate(
}
SurgSim::Math::Vector Fem2DElementTriangle::computeNaturalCoordinate(
- const SurgSim::Math::OdeState& state,
- const SurgSim::Math::Vector& cartesianCoordinate) const
+ 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::doUpdateFMDK(const Math::OdeState& state, int options)
+{
+ if (options & Math::ODEEQUATIONUPDATE_F)
+ {
+ Eigen::Matrix<double, 18, 1> x;
+
+ // 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);
+ m_f = -m_K * (x - m_x0);
+ }
+}
+
+void Fem2DElementTriangle::computeIntegral_dTd()
+{
+ // Component of the plate mass matrix coming from the displacement w(x,y) = d.U = [d1 d2 d3 d4 d5 d6 d7 d8 d9].U
+ // With d = [N1, N2.y10 + N3.y20, -N2.x10 - N3.x20,
+ // N4, N5.y10 + N6.y20, -N5.x10 - N6.x20,
+ // N7, N8.y10 + N9.y20, -N8.x10 - N9.x20]
+ // Let's define lambda = 1 - xi - eta
+ // N1(xi, eta) = 3.lambda^2 - 2.lambda^3 + 2.xi.eta.lambda
+ // N2(xi, eta) = lambda^2.xi + xi.eta.lambda/2
+ // N3(xi, eta) = lambda^2.eta + xi.eta.lambda/2
+ // N4(xi, eta) = 3.xi^2 - 2.xi^3 + 2.xi.eta.lambda
+ // N5(xi, eta) = xi^2.(xi - 1) - xi.eta.lambda
+ // N6(xi, eta) = xi^2.eta + xi.eta.lambda/2
+ // N7(xi, eta) = 3.eta^2 - 2.eta^3 + 2.xi.eta.lambda
+ // N8(xi, eta) = eta^2.xi + xi.eta.lambda/2
+ // N9(xi, eta) = eta^2.(eta - 1) - xi.eta.lambda
+ // For more details, c.f. "Shell elements: modelizations DKT, DST, DKTG and Q4g", Code_Aster, 2013, Thomas De Soza.
+ // w = N1.w1 + N2.dw1/dxi + N3.dw1/deta
+ // + N4.w2 + N5.dw2/dxi + N6.dw2/deta
+ // + N7.w3 + N8.dw3/dxi + N9.dw3/deta
+ //
+ // Note that J = (dx/dxi=x10 dy/dxi=y10 )
+ // (dx/deta=x20 dy/deta=y20)
+ // And dw/dx = -thetay
+ // And dw/dy = thetax
+
+ // with dwi/dxi = dwi/dx.dx/dxi + dwi/dy.dy/dxi = -thetayi.x10 + thetaxi.y10
+ // with dwi/deta = dwi/dx.dx/deta + dwi/dy.dy/deta = -thetayi.x20 + thetaxi.y20
+
+ // m_integral_dT_d(i, j) = int_0^1 int_0^{1-eta} di.dj dxi deta
+
+ m_integral_dT_d.resize(9, 9);
+
+ // In the local space, the triangle point coordinates are A(x0=0, y0=0) B(x1, y1=0) C(x2, y2)
+ const double x1 = -m_xij[2]; // -(x0 - x1) = x1 (as x0 = 0)
+ const double x2 = m_xij[1]; // x2 - x0 = x2 (as x0 = 0)
+ const double y2 = m_yij[1]; // y2 - y0) = y2 (as y1 = 0)
+
+ const double x1x1 = x1 * x1;
+ const double x2x2 = x2 * x2;
+ const double y2y2 = y2 * y2;
+ const double x1x2 = x1 * x2;
+
+ m_integral_dT_d(0, 0) = 121.0 / 1260.0;
+ m_integral_dT_d(0, 1) = 13.0 / 1260.0 * y2;
+ m_integral_dT_d(0, 2) = 13.0 / 1260.0 * (-x1 - x2);
+ m_integral_dT_d(0, 3) = 89.0 / 2520.0;
+ m_integral_dT_d(0, 4) = 19.0 / 5040.0 * y2;
+ m_integral_dT_d(0, 5) = (53.0 * x1 - 19.0 * x2) / 5040.0;
+ m_integral_dT_d(0, 6) = 89.0 / 2520.0;
+ m_integral_dT_d(0, 7) = 53.0 / 5040.0 * (-y2);
+ m_integral_dT_d(0, 8) = (53.0 * x2 - 19.0 * x1) / 5040.0;
+
+ m_integral_dT_d(1, 0) = m_integral_dT_d(0, 1); // symmetric part
+ m_integral_dT_d(1, 1) = 31.0 / 20160.0 * y2y2;
+ m_integral_dT_d(1, 2) = (-y2) / 20160.0 * (19.0 * x1 + 31.0 * x2);
+ m_integral_dT_d(1, 3) = 19.0 / 5040.0 * y2;
+ m_integral_dT_d(1, 4) = 11.0 / 20160.0 * y2y2;
+ m_integral_dT_d(1, 5) = y2 / 20160.0 * (24.0 * x1 - 11.0 * x2);
+ m_integral_dT_d(1, 6) = 17.0 / 2520.0 * y2;
+ m_integral_dT_d(1, 7) = 19.0 / 10080.0 * (-y2y2);
+ m_integral_dT_d(1, 8) = (-y2) / 20160.0 * (13.0 * x1 - 38.0 * x2);
+
+ m_integral_dT_d(2, 0) = m_integral_dT_d(0, 2); // symmetric part
+ m_integral_dT_d(2, 1) = m_integral_dT_d(1, 2); // symmetric part
+ m_integral_dT_d(2, 2) = (31.0 * (x1x1 + x2x2) + 2.0 * 19.0 * x1x2) / 20160.0;
+ m_integral_dT_d(2, 3) = (-19.0 * x2 - 2.0 * 17.0 * x1) / 5040.0;
+ m_integral_dT_d(2, 4) = (-y2) / 20160.0 * (13.0 * x1 + 11.0 * x2);
+ m_integral_dT_d(2, 5) = (11.0 * x2 * (x2 - x1) - 2.0 * 19.0 * x1x1) / 20160.0;
+ m_integral_dT_d(2, 6) = (-19.0 * x1 - 2.0 * 17.0 * x2) / 5040.0;
+ m_integral_dT_d(2, 7) = y2 / 10080.0 * (12.0 * x1 + 19.0 * x2);
+ m_integral_dT_d(2, 8) = (11.0 * x1 * (x1 - x2) - 2.0 * 19.0 * x2x2) / 20160.0;
+
+ m_integral_dT_d(3, 0) = m_integral_dT_d(0, 3); // symmetric part
+ m_integral_dT_d(3, 1) = m_integral_dT_d(1, 3); // symmetric part
+ m_integral_dT_d(3, 2) = m_integral_dT_d(2, 3); // symmetric part
+ m_integral_dT_d(3, 3) = 121.0 / 1260.0;
+ m_integral_dT_d(3, 4) = 13.0 / 1260.0 * y2;
+ m_integral_dT_d(3, 5) = (-13.0 * x2 + 2.0 * 13.0 * x1) / 1260.0;
+ m_integral_dT_d(3, 6) = 89.0 / 2520.0;
+ m_integral_dT_d(3, 7) = 53.0 / 5040.0 * (-y2);
+ m_integral_dT_d(3, 8) = (53.0 * x2 - 2.0 * 17.0 * x1) / 5040.0;
+
+ m_integral_dT_d(4, 0) = m_integral_dT_d(0, 4); // symmetric part
+ m_integral_dT_d(4, 1) = m_integral_dT_d(1, 4); // symmetric part
+ m_integral_dT_d(4, 2) = m_integral_dT_d(2, 4); // symmetric part
+ m_integral_dT_d(4, 3) = m_integral_dT_d(3, 4); // symmetric part
+ m_integral_dT_d(4, 4) = 31.0 / 20160.0 * y2y2;
+ m_integral_dT_d(4, 5) = y2 / 20160.0 * (50.0 * x1 - 31.0 * x2);
+ m_integral_dT_d(4, 6) = 17.0 / 2520.0 * y2;
+ m_integral_dT_d(4, 7) = 19.0 / 10080.0 * (-y2y2);
+ m_integral_dT_d(4, 8) = (-y2) / 20160.0 * (25.0 * x1 - 38.0 * x2);
+
+ m_integral_dT_d(5, 0) = m_integral_dT_d(0, 5); // symmetric part
+ m_integral_dT_d(5, 1) = m_integral_dT_d(1, 5); // symmetric part
+ m_integral_dT_d(5, 2) = m_integral_dT_d(2, 5); // symmetric part
+ m_integral_dT_d(5, 3) = m_integral_dT_d(3, 5); // symmetric part
+ m_integral_dT_d(5, 4) = m_integral_dT_d(4, 5); // symmetric part
+ m_integral_dT_d(5, 5) = (20.0 * 5.0 * x1 * (x1 - x2) + 31.0 * x2x2) / 20160.0;
+ m_integral_dT_d(5, 6) = (53.0 * x1 - 2.0 * 17.0 * x2) / 5040.0;
+ m_integral_dT_d(5, 7) = (-y2) / 10080.0 * (31.0 * x1 - 19.0 * x2);
+ m_integral_dT_d(5, 8) = 19.0 * (-x1x1 - x2x2) / 10080.0 + 29.0 / 6720.0 * x1x2;
+
+ m_integral_dT_d(6, 0) = m_integral_dT_d(0, 6); // symmetric part
+ m_integral_dT_d(6, 1) = m_integral_dT_d(1, 6); // symmetric part
+ m_integral_dT_d(6, 2) = m_integral_dT_d(2, 6); // symmetric part
+ m_integral_dT_d(6, 3) = m_integral_dT_d(3, 6); // symmetric part
+ m_integral_dT_d(6, 4) = m_integral_dT_d(4, 6); // symmetric part
+ m_integral_dT_d(6, 5) = m_integral_dT_d(5, 6); // symmetric part
+ m_integral_dT_d(6, 6) = 121.0 / 1260.0;
+ m_integral_dT_d(6, 7) = (-y2) * 13.0 / 630.0;
+ m_integral_dT_d(6, 8) = (-13.0 * x1 + 2.0 * 13.0 * x2) / 1260.0;
+
+ m_integral_dT_d(7, 0) = m_integral_dT_d(0, 7); // symmetric part
+ m_integral_dT_d(7, 1) = m_integral_dT_d(1, 7); // symmetric part
+ m_integral_dT_d(7, 2) = m_integral_dT_d(2, 7); // symmetric part
+ m_integral_dT_d(7, 3) = m_integral_dT_d(3, 7); // symmetric part
+ m_integral_dT_d(7, 4) = m_integral_dT_d(4, 7); // symmetric part
+ m_integral_dT_d(7, 5) = m_integral_dT_d(5, 7); // symmetric part
+ m_integral_dT_d(7, 6) = m_integral_dT_d(6, 7); // symmetric part
+ m_integral_dT_d(7, 7) = 5.0 / 1008.0 * y2y2;
+ m_integral_dT_d(7, 8) = 5.0 / 2016.0 * y2 * (x1 - 2.0 * x2);
+
+ m_integral_dT_d(8, 0) = m_integral_dT_d(0, 8); // symmetric part
+ m_integral_dT_d(8, 1) = m_integral_dT_d(1, 8); // symmetric part
+ m_integral_dT_d(8, 2) = m_integral_dT_d(2, 8); // symmetric part
+ m_integral_dT_d(8, 3) = m_integral_dT_d(3, 8); // symmetric part
+ m_integral_dT_d(8, 4) = m_integral_dT_d(4, 8); // symmetric part
+ m_integral_dT_d(8, 5) = m_integral_dT_d(5, 8); // symmetric part
+ m_integral_dT_d(8, 6) = m_integral_dT_d(6, 8); // symmetric part
+ m_integral_dT_d(8, 7) = m_integral_dT_d(7, 8); // symmetric part
+ m_integral_dT_d(8, 8) = 5.0 / 1008.0 * x2 * (x2 - x1) + 31.0 / 20160.0 * x1x1;
+}
+
+void Fem2DElementTriangle::computeIntegral_HxHxT()
+{
+ // Compute the integral terms of Batoz Hx product functions
+ // m_integralHxiHxj(i, j) = int_0^1 int_0^{1-eta} Hxi.Hxj dxi deta
+
+ const double& a4 = m_ak[0];
+ const double& a5 = m_ak[1];
+ const double& a6 = m_ak[2];
+
+ const double& b4 = m_bk[0];
+ const double& b5 = m_bk[1];
+ const double& b6 = m_bk[2];
+
+ const double& c4 = m_ck[0];
+ const double& c5 = m_ck[1];
+ const double& c6 = m_ck[2];
+
+ m_integralHxiHxj.resize(9, 9);
+
+ m_integralHxiHxj(0, 0) = 1. / 5. * (a6 * a6 - a5 * a6 + a5 * a5);
+ m_integralHxiHxj(0, 1) = 1. / 15. * (b6 * (2.0 * a6 - a5) + b5 * (a6 - 2.0 * a5));
+ m_integralHxiHxj(0, 2) = 1. / 15. * (c6 * (-2.0 * a6 + a5) + c5 * (-a6 + 2.0 * a5));
+ m_integralHxiHxj(0, 3) = 1. / 10. * (a6 * (-2.0 * a6 + a5) + a4 * (a6 - a5));
+ m_integralHxiHxj(0, 4) = 1. / 15. * (b6 * (2.0 * a6 - a5) + b4 * (a6 - a5));
+ m_integralHxiHxj(0, 5) = 1. / 60. * a5 + 1. / 15. * (c6 * (-2.0 * a6 + a5) + c4 * (-a6 + a5));
+ m_integralHxiHxj(0, 6) = 1. / 10. * (a5 * (a6 - 2.0 * a5) + a4 * (-a6 + a5));
+ m_integralHxiHxj(0, 7) = 1. / 15. * (b5 * (a6 - 2.0 * a5) + b4 * (a6 - a5));
+ m_integralHxiHxj(0, 8) = -1. / 60. * a6 + 1. / 15. * (c5 * (-a6 + 2.0 * a5) + c4 * (-a6 + a5));
+
+ m_integralHxiHxj(1, 0) = m_integralHxiHxj(0, 1); // symmetric part
+ m_integralHxiHxj(1, 1) = 4. / 45. * (b5 * b5 + b5 * b6 + b6 * b6);
+ m_integralHxiHxj(1, 2) = 2. / 45. * (c5 * (-2.0 * b5 - b6) + c6 * (-b5 - 2.0 * b6));
+ m_integralHxiHxj(1, 3) = 1. / 15. * (a6 * (-2.0 * b6 - b5) + a4 * (b6 + b5));
+ m_integralHxiHxj(1, 4) = 2. / 45. * (b6 * (b5 + 2.0 * b6) + b4 * (b6 + b5));
+ m_integralHxiHxj(1, 5) = -1. / 90. * b5 + 2. / 45. * (c6 * (-2.0 * b6 - b5) - c4 * (b6 + b5));
+ m_integralHxiHxj(1, 6) = 1. / 15. * (a5 * (b6 + 2.0 * b5) - a4 * (b6 + b5));
+ m_integralHxiHxj(1, 7) = 2. / 45. * (b5 * (b6 + 2.0 * b5) + b4 * (b6 + b5));
+ m_integralHxiHxj(1, 8) = -1. / 90. * b6 - 2. / 45. * (c5 * (2.0 * b5 + b6) + c4 * (b6 + b5));
+
+ m_integralHxiHxj(2, 0) = m_integralHxiHxj(0, 2); // symmetric part
+ m_integralHxiHxj(2, 1) = m_integralHxiHxj(1, 2); // symmetric part
+ m_integralHxiHxj(2, 2) = 1. / 60. + 4. / 45. * (c5 * c5 + c6 * c6 + c5 * c6);
+ m_integralHxiHxj(2, 3) = -1. / 60. * a4 + 1. / 15. * (-a4 * (c6 + c5) + a6 * (2.0 * c6 + c5));
+ m_integralHxiHxj(2, 4) = -1. / 90. * b4 - 2. / 45. * (b6 * (c5 + 2.0 * c6) + b4 * (c6 + c5));
+ m_integralHxiHxj(2, 5) = -1. / 360. + 1. / 90. * (c5 + c4) + 2. / 45. * (c6 * (2.0 * c6 + c5) + c4 * (c6 + c5));
+ m_integralHxiHxj(2, 6) = 1. / 60. * a4 + 1. / 15. * (a4 * (c6 + c5) - a5 * (c6 + 2.0 * c5));
+ m_integralHxiHxj(2, 7) = -1. / 90. * b4 - 2. / 45. * (b5 * (2.0 * c5 + c6) + b4 * (c6 + c5));
+ m_integralHxiHxj(2, 8) = -1. / 360. + 1. / 90. * (c6 + c4) + 2. / 45. * (c5 * (2.0 * c5 + c6) + c4 * (c6 + c5));
+
+ m_integralHxiHxj(3, 0) = m_integralHxiHxj(0, 3); // symmetric part
+ m_integralHxiHxj(3, 1) = m_integralHxiHxj(1, 3); // symmetric part
+ m_integralHxiHxj(3, 2) = m_integralHxiHxj(2, 3); // symmetric part
+ m_integralHxiHxj(3, 3) = 1. / 5. * (a6 * a6 - a6 * a4 + a4 * a4);
+ m_integralHxiHxj(3, 4) = 1. / 15. * (b6 * (-2.0 * a6 + a4) + b4 * (-a6 + 2.0 * a4));
+ m_integralHxiHxj(3, 5) = 1. / 15. * (c6 * (-a4 + 2.0 * a6) + c4 * (-2.0 * a4 + a6));
+ m_integralHxiHxj(3, 6) = 1. / 10. * (a6 * (-a5 + a4) + a4 * (a5 - 2.0 * a4));
+ m_integralHxiHxj(3, 7) = 1. / 15. * (-a6 * (b4 + b5) + a4 * (2.0 * b4 + b5));
+ m_integralHxiHxj(3, 8) = 1. / 60. * a6 + 1. / 15. * (a6 * (c4 + c5) - a4 * (2.0 * c4 + c5));
+
+ m_integralHxiHxj(4, 0) = m_integralHxiHxj(0, 4); // symmetric part
+ m_integralHxiHxj(4, 1) = m_integralHxiHxj(1, 4); // symmetric part
+ m_integralHxiHxj(4, 2) = m_integralHxiHxj(2, 4); // symmetric part
+ m_integralHxiHxj(4, 3) = m_integralHxiHxj(3, 4); // symmetric part
+ m_integralHxiHxj(4, 4) = 4. / 45. * (b6 * b6 + b6 * b4 + b4 * b4);
+ m_integralHxiHxj(4, 5) = -2. / 45. * (b6 * (2.0 * b6 + c4) + b4 * (c6 + 2.0 * c4));
+ m_integralHxiHxj(4, 6) = 1. / 15. * (b6 * (a5 - a4) + b4 * (a5 - 2.0 * a4));
+ m_integralHxiHxj(4, 7) = 2. / 45. * (b6 * (b4 + b5) + b4 * (2.0 * b4 + b5));
+ m_integralHxiHxj(4, 8) = -1. / 90. * b6 - 2. / 45. * (b6 * (c4 + c5) + b4 * (2.0 * c4 + c5));
+
+ m_integralHxiHxj(5, 0) = m_integralHxiHxj(0, 5); // symmetric part
+ m_integralHxiHxj(5, 1) = m_integralHxiHxj(1, 5); // symmetric part
+ m_integralHxiHxj(5, 2) = m_integralHxiHxj(2, 5); // symmetric part
+ m_integralHxiHxj(5, 3) = m_integralHxiHxj(3, 5); // symmetric part
+ m_integralHxiHxj(5, 4) = m_integralHxiHxj(4, 5); // symmetric part
+ m_integralHxiHxj(5, 5) = 1. / 60. + 4. / 45. * (c6 * c6 + c6 * c4 + c4 * c4);
+ m_integralHxiHxj(5, 6) = -1. / 60. * a5 + 1. / 15. * (-a5 * (c6 + c4) + a4 * (c6 + 2.0 * c4));
+ m_integralHxiHxj(5, 7) = -1. / 90. * b5 - 2. / 45. * (c6 * (b4 + b5) + c4 * (b5 + 2.0 * b4));
+ m_integralHxiHxj(5, 8) = -1. / 360. + 1. / 90. * (c5 + c6) + 2. / 45. * (c4 * (2.0 * c4 + c5) + c6 * (c5 + c4));
+
+ m_integralHxiHxj(6, 0) = m_integralHxiHxj(0, 6); // symmetric part
+ m_integralHxiHxj(6, 1) = m_integralHxiHxj(1, 6); // symmetric part
+ m_integralHxiHxj(6, 2) = m_integralHxiHxj(2, 6); // symmetric part
+ m_integralHxiHxj(6, 3) = m_integralHxiHxj(3, 6); // symmetric part
+ m_integralHxiHxj(6, 4) = m_integralHxiHxj(4, 6); // symmetric part
+ m_integralHxiHxj(6, 5) = m_integralHxiHxj(5, 6); // symmetric part
+ m_integralHxiHxj(6, 6) = 1. / 5. * (a4 * a4 - a4 * a5 + a5 * a5);
+ m_integralHxiHxj(6, 7) = 1. / 15. * (a5 * (b4 + 2.0 * b5) - a4 * (2.0 * b4 + b5));
+ m_integralHxiHxj(6, 8) = 1. / 15. * (-a5 * (c4 + 2.0 * c5) + a4 * (2.0 * c4 + c5));
+
+ m_integralHxiHxj(7, 0) = m_integralHxiHxj(0, 7); // symmetric part
+ m_integralHxiHxj(7, 1) = m_integralHxiHxj(1, 7); // symmetric part
+ m_integralHxiHxj(7, 2) = m_integralHxiHxj(2, 7); // symmetric part
+ m_integralHxiHxj(7, 3) = m_integralHxiHxj(3, 7); // symmetric part
+ m_integralHxiHxj(7, 4) = m_integralHxiHxj(4, 7); // symmetric part
+ m_integralHxiHxj(7, 5) = m_integralHxiHxj(5, 7); // symmetric part
+ m_integralHxiHxj(7, 6) = m_integralHxiHxj(6, 7); // symmetric part
+ m_integralHxiHxj(7, 7) = 4. / 45. * (b4 * b4 + b4 * b5 + b5 * b5);
+ m_integralHxiHxj(7, 8) = -2. / 45. * (b4 * (2.0 * c4 + c5) + b5 * (c4 + 2.0 * c5));
+
+ m_integralHxiHxj(8, 0) = m_integralHxiHxj(0, 8); // symmetric part
+ m_integralHxiHxj(8, 1) = m_integralHxiHxj(1, 8); // symmetric part
+ m_integralHxiHxj(8, 2) = m_integralHxiHxj(2, 8); // symmetric part
+ m_integralHxiHxj(8, 3) = m_integralHxiHxj(3, 8); // symmetric part
+ m_integralHxiHxj(8, 4) = m_integralHxiHxj(4, 8); // symmetric part
+ m_integralHxiHxj(8, 5) = m_integralHxiHxj(5, 8); // symmetric part
+ m_integralHxiHxj(8, 6) = m_integralHxiHxj(6, 8); // symmetric part
+ m_integralHxiHxj(8, 7) = m_integralHxiHxj(7, 8); // symmetric part
+ m_integralHxiHxj(8, 8) = 1. / 60. + 4. / 45. * (c4 * c4 + c5 * c4 + c5 * c5);
+}
+
+void Fem2DElementTriangle::computeIntegral_HyHyT()
+{
+ // Compute the integral terms of Batoz Hy product functions
+ // m_integralHyiHyj(i, j) = int_0^1 int_0^{1-eta} Hyi.Hyj dxi deta
+
+ const double& b4 = m_bk[0];
+ const double& b5 = m_bk[1];
+ const double& b6 = m_bk[2];
+
+ const double& d4 = m_dk[0];
+ const double& d5 = m_dk[1];
+ const double& d6 = m_dk[2];
+
+ const double& e4 = m_ek[0];
+ const double& e5 = m_ek[1];
+ const double& e6 = m_ek[2];
+
+ m_integralHyiHyj.resize(9, 9);
+
+ m_integralHyiHyj(0, 0) = 1. / 5. * (d6 * d6 - d5 * d6 + d5 * d5);
+ m_integralHyiHyj(0, 1) = 1. / 15. * (d6 * (2.0 * e6 + e5) - d5 * (e6 + 2.0 * e5));
+ m_integralHyiHyj(0, 2) = 1. / 15. * (b6 * (-2.0 * d6 + d5) + b5 * (-d6 + 2.0 * d5));
+ m_integralHyiHyj(0, 3) = 1. / 10. * (d6 * (d4 - 2.0 * d6) + d5 * (-d4 + d6));
+ m_integralHyiHyj(0, 4) = -1. / 60. * d5 + 1. / 15. * (d6 * (2.0 * e6 + e4) - d5 * (e6 + e4));
+ m_integralHyiHyj(0, 5) = 1. / 15. * (b6 * (-2.0 * d6 + d5) + b4 * (-d6 + d5));
+ m_integralHyiHyj(0, 6) = 1. / 10. * (d6 * (d5 - d4) + d5 * (-2.0 * d5 + d4));
+ m_integralHyiHyj(0, 7) = 1. / 60. * d6 + 1. / 15. * (d6 * (e4 + e5) - d5 * (e4 + 2.0 * e5));
+ m_integralHyiHyj(0, 8) = 1. / 15. * (b5 * (-d6 + 2.0 * d5) + b4 * (-d6 + d5));
+
+ m_integralHyiHyj(1, 0) = m_integralHyiHyj(0, 1); // symmetric part
+ m_integralHyiHyj(1, 1) = 1. / 60. + 4. / 45. * (e6 * e6 + e5 * e6 + e5 * e5);
+ m_integralHyiHyj(1, 2) = 2. / 45. * (-e5 * (b6 + 2.0 * b5) - e6 * (b5 + 2.0 * b6));
+ m_integralHyiHyj(1, 3) = 1. / 60. * d4 + 1. / 15. * (e6 * (d4 - 2.0 * d6) + e5 * (-d6 + d4));
+ m_integralHyiHyj(1, 4) = -1. / 360. + 1. / 90. * (e5 + e4) + 2. / 45. * (e6 * (2.0 * e6 + e5) + e4 * (e5 + e6));
+ m_integralHyiHyj(1, 5) = -1. / 90. * b4 - 2. / 45. * (e5 * (b6 + b4) + e6 * (2.0 * b6 + b4));
+ m_integralHyiHyj(1, 6) = -1. / 60. * d4 + 1. / 15. * (e6 * (-d4 + d5) + e5 * (-d4 + 2.0 * d5));
+ m_integralHyiHyj(1, 7) = -1. / 360. + 1. / 90. * (e4 + e6) + 2. / 45. * (e6 * (e5 + e4) + e5 * (e4 + 2.0 * e5));
+ m_integralHyiHyj(1, 8) = -1. / 90. * b4 - 2. / 45. * (e5 * (b4 + 2.0 * b5) + e6 * (b5 + b4));
+
+ m_integralHyiHyj(2, 0) = m_integralHyiHyj(0, 2); // symmetric part
+ m_integralHyiHyj(2, 1) = m_integralHyiHyj(1, 2); // symmetric part
+ m_integralHyiHyj(2, 2) = 4. / 45. * (b5 * b5 + b5 * b6 + b6 * b6);
+ m_integralHyiHyj(2, 3) = 1. / 15. * (b6 * (2.0 * d6 - d4) + b5 * (d6 - d4));
+ m_integralHyiHyj(2, 4) = -1. / 90. * b5 - 2. / 45. * (b6 * (2.0 * e6 + e4) + b5 * (e6 + e4));
+ m_integralHyiHyj(2, 5) = 2. / 45. * (b6 * (2.0 * b6 + b4) + b5 * (b6 + b4));
+ m_integralHyiHyj(2, 6) = 1. / 15. * (b6 * (-d5 + d4) + b5 * (-2.0 * d5 + d4));
+ m_integralHyiHyj(2, 7) = -1. / 90. * b6 - 2. / 45. * (b6 * (e5 + e4) + b5 * (2.0 * e5 + e4));
+ m_integralHyiHyj(2, 8) = 2. / 45. * (b5 * (b4 + 2.0 * b5) + b6 * (b4 + b5));
+
+ m_integralHyiHyj(3, 0) = m_integralHyiHyj(0, 3); // symmetric part
+ m_integralHyiHyj(3, 1) = m_integralHyiHyj(1, 3); // symmetric part
+ m_integralHyiHyj(3, 2) = m_integralHyiHyj(2, 3); // symmetric part
+ m_integralHyiHyj(3, 3) = 1. / 5. * (d6 * d6 - d6 * d4 + d4 * d4);
+ m_integralHyiHyj(3, 4) = 1. / 15. * (d4 * (e6 + 2.0 * e4) - d6 * (2.0 * e6 + e4));
+ m_integralHyiHyj(3, 5) = 1. / 15. * (d6 * (2.0 * b6 + b4) - d4 * (b6 + 2.0 * b4));
+ m_integralHyiHyj(3, 6) = 1. / 10. * (d6 * (-d5 + d4) + d4 * (d5 - 2.0 * d4));
+ m_integralHyiHyj(3, 7) = -1. / 60. * d6 + 1. / 15. * (-d6 * (e4 + e5) + d4 * (2.0 * e4 + e5));
+ m_integralHyiHyj(3, 8) = 1. / 15. * (d6 * (b4 + b5) - d4 * (2.0 * b4 + b5));
+
+ m_integralHyiHyj(4, 0) = m_integralHyiHyj(0, 4); // symmetric part
+ m_integralHyiHyj(4, 1) = m_integralHyiHyj(1, 4); // symmetric part
+ m_integralHyiHyj(4, 2) = m_integralHyiHyj(2, 4); // symmetric part
+ m_integralHyiHyj(4, 3) = m_integralHyiHyj(3, 4); // symmetric part
+ m_integralHyiHyj(4, 4) = 1. / 60. + 4. / 45. * (e6 * e6 + e6 * e4 + e4 * e4);
+ m_integralHyiHyj(4, 5) = -2. / 45. * (e6 * (2.0 * b6 + b4) + e4 * (b6 + 2.0 * b4));
+ m_integralHyiHyj(4, 6) = 1. / 60. * d5 + 1. / 15. * (e6 * (d5 - d4) + e4 * (d5 - 2.0 * d4));
+ m_integralHyiHyj(4, 7) = -1. / 360. + 1. / 90. * (e6 + e5) + 2. / 45. * (e6 * (e5 + e4) + e4 * (e5 + 2.0 * e4));
+ m_integralHyiHyj(4, 8) = -1. / 90. * b5 - 2. / 45. * (e6 * (b5 + b4) + e4 * (b5 + 2.0 * b4));
+
+ m_integralHyiHyj(5, 0) = m_integralHyiHyj(0, 5); // symmetric part
+ m_integralHyiHyj(5, 1) = m_integralHyiHyj(1, 5); // symmetric part
+ m_integralHyiHyj(5, 2) = m_integralHyiHyj(2, 5); // symmetric part
+ m_integralHyiHyj(5, 3) = m_integralHyiHyj(3, 5); // symmetric part
+ m_integralHyiHyj(5, 4) = m_integralHyiHyj(4, 5); // symmetric part
+ m_integralHyiHyj(5, 5) = 4. / 45. * (b6 * b6 + b6 * b4 + b4 * b4);
+ m_integralHyiHyj(5, 6) = 1. / 15. * (b6 * (-d5 + d4) + b4 * (-d5 + 2.0 * d4));
+ m_integralHyiHyj(5, 7) = -1. / 90. * b6 - 2. / 45. * (b6 * (e4 + e5) + b4 * (2.0 * e4 + e5));
+ m_integralHyiHyj(5, 8) = 2. / 45. * (b6 * (b4 + b5) + b4 * (2.0 * b4 + b5));
+
+ m_integralHyiHyj(6, 0) = m_integralHyiHyj(0, 6); // symmetric part
+ m_integralHyiHyj(6, 1) = m_integralHyiHyj(1, 6); // symmetric part
+ m_integralHyiHyj(6, 2) = m_integralHyiHyj(2, 6); // symmetric part
+ m_integralHyiHyj(6, 3) = m_integralHyiHyj(3, 6); // symmetric part
+ m_integralHyiHyj(6, 4) = m_integralHyiHyj(4, 6); // symmetric part
+ m_integralHyiHyj(6, 5) = m_integralHyiHyj(5, 6); // symmetric part
+ m_integralHyiHyj(6, 6) = 1. / 5. * (d4 * d4 - d5 * d4 + d5 * d5);
+ m_integralHyiHyj(6, 7) = 1. / 15. * (d5 * (e4 + 2.0 * e5) - d4 * (2.0 * e4 + e5));
+ m_integralHyiHyj(6, 8) = 1. / 15. * (-d5 * (b4 + 2.0 * b5) + d4 * (2.0 * b4 + b5));
+
+ m_integralHyiHyj(7, 0) = m_integralHyiHyj(0, 7); // symmetric part
+ m_integralHyiHyj(7, 1) = m_integralHyiHyj(1, 7); // symmetric part
+ m_integralHyiHyj(7, 2) = m_integralHyiHyj(2, 7); // symmetric part
+ m_integralHyiHyj(7, 3) = m_integralHyiHyj(3, 7); // symmetric part
+ m_integralHyiHyj(7, 4) = m_integralHyiHyj(4, 7); // symmetric part
+ m_integralHyiHyj(7, 5) = m_integralHyiHyj(5, 7); // symmetric part
+ m_integralHyiHyj(7, 6) = m_integralHyiHyj(6, 7); // symmetric part
+ m_integralHyiHyj(7, 7) = 1. / 60. + 4. / 45. * (e4 * e4 + e4 * e5 + e5 * e5);
+ m_integralHyiHyj(7, 8) = -2. / 45. * (e4 * (2.0 * b4 + b5) + e5 * (b4 + 2.0 * b5));
+
+ m_integralHyiHyj(8, 0) = m_integralHyiHyj(0, 8); // symmetric part
+ m_integralHyiHyj(8, 1) = m_integralHyiHyj(1, 8); // symmetric part
+ m_integralHyiHyj(8, 2) = m_integralHyiHyj(2, 8); // symmetric part
+ m_integralHyiHyj(8, 3) = m_integralHyiHyj(3, 8); // symmetric part
+ m_integralHyiHyj(8, 4) = m_integralHyiHyj(4, 8); // symmetric part
+ m_integralHyiHyj(8, 5) = m_integralHyiHyj(5, 8); // symmetric part
+ m_integralHyiHyj(8, 6) = m_integralHyiHyj(6, 8); // symmetric part
+ m_integralHyiHyj(8, 7) = m_integralHyiHyj(7, 8); // symmetric part
+ m_integralHyiHyj(8, 8) = 4. / 45. * (b4 * b4 + b4 * b5 + b5 * b5);
+}
+
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();
+ ", 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();
+ ", 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();
+ ", 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]);
@@ -433,14 +729,15 @@ void Fem2DElementTriangle::computeShapeFunctionsParameters(const SurgSim::Math::
// 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
+ "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;
+ "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
@@ -448,9 +745,10 @@ void Fem2DElementTriangle::computeShapeFunctionsParameters(const SurgSim::Math::
// 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() << ")";
+ "), 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() << ")";
+ ", 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)
@@ -495,125 +793,132 @@ void Fem2DElementTriangle::computeShapeFunctionsParameters(const SurgSim::Math::
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)
+ 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
+ // 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
+ // 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
}
+
+ // Pre-compute the 3 integral terms useful for the plate mass matrix
+ computeIntegral_dTd(); // associated to the displacement along z (noted w)
+ computeIntegral_HyHyT(); // associated to the displacement along thetax
+ computeIntegral_HxHxT(); // associated to the displacement along thetay
}
-std::array<double, 9> Fem2DElementTriangle::batozDhxDxi(double xi, double neta) const
+std::array<double, 9> Fem2DElementTriangle::batozDhxDxi(double xi, double eta) 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[0] = m_Pk[2] * a + (m_Pk[1] - m_Pk[2]) * eta; // P6(1-2xi) + (P5-P6)eta
+ res[1] = m_qk[2] * a - (m_qk[1] + m_qk[2]) * eta; // q6(1-2xi) - (q5-q6)eta
+ res[2] = -4.0 + 6.0 * (xi + eta) + m_rk[2] * a - // -4 + 6(xi+eta) + r6(1-2xi) - eta(r5+r6)
+ eta * (m_rk[1] + m_rk[2]);
- 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)
+ res[3] = -m_Pk[2] * a + eta * (m_Pk[0] + m_Pk[2]); // -P6(1-2xi) + eta(P4+P6)
+ res[4] = m_qk[2] * a - eta * (m_qk[2] - m_qk[0]); // q6(1-2xi) - eta(q6-q4)
+ res[5] = -2.0 + 6.0 * xi + m_rk[2] * a + eta * (m_rk[0] - m_rk[2]); // -2 + 6xi + r6(1-2xi) + eta(r4-r6)
+
+ res[6] = -eta * (m_Pk[1] + m_Pk[0]); // -eta(P5+P4)
+ res[7] = eta * (m_qk[0] - m_qk[1]); // eta(a4-a5)
+ res[8] = -eta * (m_rk[1] - m_rk[0]); // -eta(r5-r4)
return res;
}
-std::array<double, 9> Fem2DElementTriangle::batozDhxDneta(double xi, double neta) const
+std::array<double, 9> Fem2DElementTriangle::batozDhxDeta(double xi, double eta) const
{
std::array<double, 9> res;
- double a = 1.0 - 2.0 * neta;
+ double a = 1.0 - 2.0 * eta;
+
+ res[0] = -m_Pk[1] * a - xi * (m_Pk[2] - m_Pk[1]); // -P5(1-2eta) - xi(P6-P5)
+ res[1] = m_qk[1] * a - xi * (m_qk[1] + m_qk[2]); // q5(1-2eta) - xi(q5+q6)
+ res[2] = -4.0 + 6.0 * (xi + eta) + m_rk[1] * a - // -4 + 6(xi+eta) + r5(1-2eta) - xi(r5+r6)
+ xi * (m_rk[1] + m_rk[2]);
- 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)
+ res[6] = m_Pk[1] * a - xi * (m_Pk[0] + m_Pk[1]); // P5(1-2eta) - xi(P4+P5)
+ res[7] = m_qk[1] * a + xi * (m_qk[0] - m_qk[1]); // q5(1-2eta) + xi(q4-q5)
+ res[8] = -2.0 + 6.0 * eta + m_rk[1] * a + xi * (m_rk[0] - m_rk[1]); // -2 + 6eta + r5(1-2eta) + xi(r4-r5)
return res;
}
-std::array<double, 9> Fem2DElementTriangle::batozDhyDxi(double xi, double neta) const
+std::array<double, 9> Fem2DElementTriangle::batozDhyDxi(double xi, double eta) 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[0] = m_tk[2] * a + eta * (m_tk[1] - m_tk[2]); // t6(1-2xi) + eta(t5-t6)
+ res[1] = 1.0 + m_rk[2] * a - eta * (m_rk[1] + m_rk[2]); // 1+r6(1-2xi) - eta(r5+r6)
+ res[2] = -m_qk[2] * a + eta * (m_qk[1] + m_qk[2]); // -q6(1-2xi) + eta(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[3] = -m_tk[2] * a + eta * (m_tk[0] + m_tk[2]); // -t6(1-2xi) + eta(t4+t6)
+ res[4] = -1.0 + m_rk[2] * a + eta * (m_rk[0] - m_rk[2]); // -1 + r6(1-2xi) + eta(r4-r6)
+ res[5] = -m_qk[2] * a - eta * (m_qk[0] - m_qk[2]); // -q6(1-2xi) - eta(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)
+ res[6] = -eta * (m_tk[0] + m_tk[1]); // -eta(t4+t5)
+ res[7] = eta * (m_rk[0] - m_rk[1]); // eta(r4-r5)
+ res[8] = -eta * (m_qk[0] - m_qk[1]); // -eta(q4-q5)
return res;
}
-std::array<double, 9> Fem2DElementTriangle::batozDhyDneta(double xi, double neta) const
+std::array<double, 9> Fem2DElementTriangle::batozDhyDeta(double xi, double eta) const
{
std::array<double, 9> res;
- double a = 1.0 - 2.0 * neta;
+ double a = 1.0 - 2.0 * eta;
- 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[0] = -m_tk[1] * a - xi * (m_tk[2] - m_tk[1]); // -t5(1-2eta) - xi(t6-t5)
+ res[1] = 1.0 + m_rk[1] * a - xi * (m_rk[1] + m_rk[2]); // 1+r5(1-2eta) - xi(r5+r6)
+ res[2] = -m_qk[1] * a + xi * (m_qk[1] + m_qk[2]); // -q5(1-2eta) + 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)
+ res[6] = m_tk[1] * a - xi * (m_tk[0] + m_tk[1]); // t5(1-2eta) - xi(t4+t5)
+ res[7] = -1.0 + m_rk[1] * a + xi * (m_rk[0] - m_rk[1]); // -1 + r5(1-2eta) + xi (r4-r5)
+ res[8] = -m_qk[1] * a - xi * (m_qk[0] - m_qk[1]); // -q5(1-2eta) - xi(q4-q5)
return res;
}
-Fem2DElementTriangle::Matrix39Type Fem2DElementTriangle::batozStrainDisplacement(double xi, double neta)const
+Fem2DElementTriangle::Matrix39Type Fem2DElementTriangle::batozStrainDisplacement(double xi, double eta)const
{
Matrix39Type res;
- std::array<double, 9> dHx_dxi, dHx_dneta, dHy_dxi, dHy_dneta;
+ std::array<double, 9> dHx_dxi, dHx_deta, dHy_dxi, dHy_deta;
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);
+ dHx_dxi = batozDhxDxi(xi, eta);
+ dHx_deta = batozDhxDeta(xi, eta);
+ dHy_dxi = batozDhyDxi(xi, eta);
+ dHy_deta = batozDhyDeta(xi, eta);
- for(size_t i = 0; i < 9; ++i)
+ 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(0, i) = coefficient * (m_yij[1] * dHx_dxi[i] + m_yij[2] * dHx_deta[i]);
+ res(1, i) = coefficient * (-m_xij[1] * dHy_dxi[i] - m_xij[2] * dHy_deta[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]);
+ (-m_xij[1] * dHx_dxi[i] - m_xij[2] * dHx_deta[i] + m_yij[1] * dHy_dxi[i] + m_yij[2] * dHy_deta[i]);
}
return res;
diff --git a/SurgSim/Physics/Fem2DElementTriangle.h b/SurgSim/Physics/Fem2DElementTriangle.h
index e86ade2..c69622f 100644
--- a/SurgSim/Physics/Fem2DElementTriangle.h
+++ b/SurgSim/Physics/Fem2DElementTriangle.h
@@ -18,6 +18,7 @@
#include <array>
+#include "SurgSim/Physics/Fem.h"
#include "SurgSim/Physics/FemElement.h"
namespace SurgSim
@@ -25,23 +26,28 @@ namespace SurgSim
namespace Physics
{
+SURGSIM_STATIC_REGISTRATION(Fem2DElementTriangle);
/// 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 triangle is modeled as a shell (6DOF) which is decomposed into a membrane (in-plane 2DOF \f$(x, y)\f$) and a
+/// plate (bending/twisting 3DOF \f$(z, \theta_x, \theta_y)\f$). The thin-plate assumption does not consider the
+/// drilling dof \f$\theta_z\f$.
+/// The system includes the drilling 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.
+/// Numerical Methods in Engineering, vol 15, 1771-1812 (1980). <br>
+/// The plate mass matrix is not detailed in the above paper, but the analytical equations
+/// have been derived from it. Moreover, to account for contribution of the displacement
+/// along z to the plate mass matrix, we used a cubic expression of this displacement given in:
+/// "Shell elements: modelizations DKT, DST, DKTG and Q4g", Code_Aster, 2013, Thomas De Soza.
///
/// \note The element is considered to have a constant thickness.
+/// \note The element uses linear elasticity (not visco-elasticity), so it does not have any damping.
class Fem2DElementTriangle : public FemElement
{
typedef Eigen::Matrix<double, 3, 3> Matrix33Type;
@@ -54,10 +60,21 @@ class Fem2DElementTriangle : public FemElement
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.
+ Fem2DElementTriangle();
+
+ /// Constructor
+ /// \param nodeIds An array of 3 node ids defining this triangle element with respect to a
+ /// DeformableRepresentationState which is passed to the initialize method.
explicit Fem2DElementTriangle(std::array<size_t, 3> nodeIds);
+ /// Constructor for FemElement object factory
+ /// \param elementData A FemElement struct defining this triangle element with respect to a
+ /// DeformableRepresentationState which is passed to the initialize method.
+ /// \exception SurgSim::Framework::AssertionFailure if nodeIds has a size different than 3
+ explicit Fem2DElementTriangle(std::shared_ptr<FemElementStructs::FemElementParameter> elementData);
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::Fem2DElementTriangle)
+
/// Sets the triangle's thickness
/// \param thickness The thickness of the triangle
void setThickness(double thickness);
@@ -66,108 +83,48 @@ public:
/// \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;
+ void initialize(const SurgSim::Math::OdeState& state) override;
+
+ double getVolume(const SurgSim::Math::OdeState& state) const override;
+
+ SurgSim::Math::Vector computeCartesianCoordinate(const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const override;
+
+ SurgSim::Math::Vector computeNaturalCoordinate(const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& cartesianCoordinate) const override;
protected:
- /// Computes the triangle element's initial rotation
+ /// Initializes variables needed before Initialize() is called
+ void initializeMembers();
+
+ /// Computes the triangle element's rotation given a state
/// \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);
+ /// \return The rotation matrix of the element in the given state
+ SurgSim::Math::Matrix33d computeRotation(const SurgSim::Math::OdeState& state);
+
+ /// Computes the triangle's local stiffness matrix
+ /// \param state The state to compute the local stiffness matrix from
+ /// \param[out] localStiffnessMatrix The local stiffness matrix to store the result into
+ virtual void computeLocalStiffness(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 18, 18>* localStiffnessMatrix);
/// 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);
+ /// \param[out] stiffnessMatrix The stiffness matrix to store the result into
+ void computeStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* stiffnessMatrix);
+
+ /// Computes the triangle's local mass matrix
+ /// \param state The state to compute the local mass matrix from
+ /// \param[out] localMassMatrix The local mass matrix to store the result into
+ virtual void computeLocalMass(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 18, 18>* localMassMatrix);
/// 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);
+ /// \param state The state to compute the mass matrix from
+ /// \param[out] massMatrix The mass matrix to store the result into
+ void computeMass(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* massMatrix);
+
+ void doUpdateFMDK(const Math::OdeState& state, int options) override;
/// The element's rest state
Eigen::Matrix<double, 18, 1> m_x0;
@@ -175,12 +132,8 @@ protected:
/// 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;
@@ -191,55 +144,100 @@ protected:
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
+ /// \param restState the rest state to compute the shape functions parameters 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
+ /// Membrane (in-plane) deformation. DOF simulated: (x, y).
+ /// "Theory of Matrix Structural Analysis" from J.S. Przemieniecki.
+ /// Shape functions are \f$f_i(x, y) = a_i + b_i.x + c_i.y\f$, here we store \f$(a_i, b_i, c_i)\f$ on each row.
+ SurgSim::Math::Matrix33d m_membraneShapeFunctionsParameters;
+
+ // 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; ///< Batoz variable \f$x_{ij} = x_i - x_j\f$
+ SurgSim::Math::Vector3d m_yij; ///< Batoz variable \f$y_{ij} = y_i - y_j\f$
+ SurgSim::Math::Vector3d m_lij_sqr; ///< Batoz variable \f$l_{ij}^2 = x_{ij}^2 + y_{ij}^2\f$
+ SurgSim::Math::Vector3d m_ak; ///< Batoz variable \f$a_{k} = -x_{ij}/l_i^2\f$
+ SurgSim::Math::Vector3d m_bk; ///< Batoz variable \f$b_{k} = 3/4 x_{ij} y_{ij}/l_{ij}2\f$
+ SurgSim::Math::Vector3d m_ck; ///< Batoz variable \f$c_{k} = (1/4 x_{ij}^2 - 1/2 y_{ij}^2)/l_{ij}^2\f$
+ SurgSim::Math::Vector3d m_dk; ///< Batoz variable \f$d_{k} = -y_{ij}/l_{ij}^2\f$
+ SurgSim::Math::Vector3d m_ek; ///< Batoz variable \f$e_{k} = (1/4 y_{ij}^2 - 1/2 x_{ij}^2)/l_{ij}^2\f$
+ //@}
+
+ //@{
+ SurgSim::Math::Vector3d m_Pk; ///< Batoz variable \f$P_{k} = -6x_{ij}/l_{ij}^2 = 6.\textrm{m_a}_k\f$
+ SurgSim::Math::Vector3d m_qk; ///< Batoz variable \f$q_{k} = 3x_{ij}y_{ij}/l_{ij}^2 = 4.\textrm{m_b}_k\f$
+ SurgSim::Math::Vector3d m_tk; ///< Batoz variable \f$t_{k} = -6y_{ij}/l_{ij}^2 = 6.\textrm{m_d}_k\f$
+ SurgSim::Math::Vector3d m_rk; ///< Batoz variable \f$r_{k} = 3y_{ij}^2/l_{ij}^2\f$
+ //@}
+
+ //@{
+ SurgSim::Math::Matrix m_integral_dT_d; ///< Plate mass matrix: integral terms related to the dof \f$(z)\f$
+ SurgSim::Math::Matrix m_integralHyiHyj; ///< Plate mass matrix: integral terms related to the dof \f$(\theta_x)\f$
+ SurgSim::Math::Matrix m_integralHxiHxj; ///< Plate mass matrix: integral terms related to the dof \f$(\theta_y)\f$
+ //@}
+
/// 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;
+ /// \param xi, eta The parametric coordinate (in [0 1] and xi+eta<1.0)
+ /// \return The vector dHx/dxi evaluated at (xi, eta)
+ std::array<double, 9> batozDhxDxi(double xi, double eta) const;
+ /// Batoz derivative dHx/deta
+ /// \param xi, eta The parametric coordinate (in [0 1] and xi+eta<1.0)
+ /// \return The vector dHx/deta evaluated at (xi, eta)
+ std::array<double, 9> batozDhxDeta(double xi, double eta) 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;
+ /// \param xi, eta The parametric coordinate (in [0 1] and xi+eta<1.0)
+ /// \return The vector dHy/dxi evaluated at (xi, eta)
+ std::array<double, 9> batozDhyDxi(double xi, double eta) const;
+ /// Batoz derivative dHy/deta
+ /// \param xi, eta The parametric coordinate (in [0 1] and xi+eta<1.0)
+ /// \return The vector dHy/deta evaluated at (xi, eta)
+ std::array<double, 9> batozDhyDeta(double xi, double eta) 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;
+ /// \param xi, eta The parametric coordinate (in [0 1] and xi+eta<1.0)
+ /// \return The 3x9 strain displacement matrix evaluated at (xi, eta)
+ Matrix39Type batozStrainDisplacement(double xi, double eta) const;
+
+private:
+ /// Computes the triangle's local membrane part mass matrix
+ /// \param state The state to compute the local membrane part mass matrix from
+ /// \param[out] localMassMatrix The local mass matrix to store the result into
+ void computeLocalMembraneMass(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 18, 18>* localMassMatrix);
+
+ /// Computes the triangle's local plate part mass matrix
+ /// \param state The state to compute the local plate part mass matrix from
+ /// \param[out] localMassMatrix The local mass matrix to store the result into
+ /// \note The plate mass matrix is composed of 3 matrices associated respectively to
+ /// \note displacements in direction (z, thetax, thetay).
+ void computeLocalPlateMass(const SurgSim::Math::OdeState& state,
+ Eigen::Matrix<double, 18, 18>* localMassMatrix);
+
+ /// Computes the integral terms d^T.d over the parametrized triangle area.
+ /// This integral is required in the plate mass matrix computation.
+ /// The displacement along z is w(x, y) = [d1 d2 d3 d4 d5 d6 d7 d8 d9].U = d.U
+ /// with di cubic shape functions and U nodal plate displacements.
+ void computeIntegral_dTd();
+
+ /// Computes the integral terms Hy.Hy^T over the parametrized triangle area.
+ /// This integral is required in the plate mass matrix computation.
+ /// The displacement along thetay is Thetay(x, y) = -dw/dx = betax = Hx^T.U
+ /// with Hxi quadratic shape functions and U nodal plate displacements.
+ void computeIntegral_HxHxT();
+
+ /// Computes the integral terms Hx.Hx^T over the parametrized triangle area.
+ /// This integral is required in the plate mass matrix computation.
+ /// The displacement along thetax is Thetax(x, y) = dw/dy = -betay = -Hy^T.U
+ /// with Hyi quadratic shape functions and U nodal plate displacements.
+ void computeIntegral_HyHyT();
};
} // namespace Physics
diff --git a/SurgSim/Physics/Fem2DElementTriangle_computeLocalMembraneMass.dox b/SurgSim/Physics/Fem2DElementTriangle_computeLocalMembraneMass.dox
new file mode 100644
index 0000000..530f8db
--- /dev/null
+++ b/SurgSim/Physics/Fem2DElementTriangle_computeLocalMembraneMass.dox
@@ -0,0 +1,141 @@
+/*!
+ \fn void SurgSim::Physics::Fem2DElementTriangle::computeLocalMembraneMass(const SurgSim::Math::OdeState& state, Eigen::Matrix<double, 18, 18>* localMassMatrix)
+ <br>
+
+ The mass matrix is derived from the kinetic energy, which depends on the displacements \f$\mathbf{u}\f$.<br>
+ In the case of triangle membrane, the displacements are a subset of the full shell displacements:
+ \f[ \displaystyle
+ \mathbf{u}(x, y)
+ =
+ \left(
+ \begin{array}{c}
+ u_x(x, y) \\
+ u_y(x, y)
+ \end{array}
+ \right)
+ =
+ \left(
+ \begin{array}{c}
+ \sum_{i=0}^2 N_i(x, y).u_x^i \\
+ \sum_{i=0}^2 N_i(x, y).u_y^i
+ \end{array}
+ \right)
+ =
+ \left(
+ \begin{array}{cccccc}
+ N_0(x, y) & 0 & N_1(x, y) & 0 & N_2(x, y) & 0 \\
+ 0 & N_0(x, y) & 0 & N_1(x, y) & 0 & N_2(x, y)
+ \end{array}
+ \right)
+ \left(
+ \begin{array}{c}
+ u_x^0 \\
+ u_y^0 \\
+ u_x^1 \\
+ u_y^1 \\
+ u_x^2 \\
+ u_y^2
+ \end{array}
+ \right)
+ =
+ a.\mathbf{U}
+ \f]
+ with \f$\mathbf{U}\f$ the triangle membrane nodal displacements vector (subset of the full shell nodal displacements)
+ and \f$N_i(x, y) = a_i + b_i.x + c_i.y\f$ the triangle linear shape functions.
+
+ Considering a constant mass density \f$\rho\f$ and a constant thickness \f$t\f$, the kinetic energy of the triangle membrane is
+ \f$T = \int_V \frac{1}{2} \rho(x, y, z) \mathbf{\dot{u}}^T.\mathbf{\dot{u}} \, dV =
+ \frac{1}{2}.\rho.t.\int_A \left(\dot{\mathbf{U}}^T.a^T\right).\left(a.\dot{\mathbf{U}}\right) \, dA\f$.
+
+ The triangle membrane mass matrix can be derived from the kinetic energy using the Lagrange equation of motion:
+ \f[
+ \displaystyle
+ \frac{d \frac{\partial T}{\partial \dot{\mathbf{U}}}}{d t}
+ =
+ \frac{1}{2}.\rho.t.\frac{d \frac{\partial \displaystyle \int_A \left(\dot{\mathbf{U}}^T.a^T\right).\left(a.\dot{\mathbf{U}}\right) \, dA}{\partial \dot{\mathbf{U}}}}{d t}
+ =
+ \frac{1}{2}.\rho.t.\frac{d \displaystyle \int_A 2.a^T.a.\dot{\mathbf{U}} \, dA}{d t}
+ =
+ \left( \rho.t.\int_A a^T.a \, dA \right).\ddot{\mathbf{U}}
+ \f]
+ Therefore, the mass matrix is:
+ \f[
+ M = \rho.t.\int_A a^T.a \, dA
+ \f]
+ To integrate over the triangle area, we operate the following variables change:
+ \f[
+ \left\{
+ \begin{array}{ccc}
+ x(\xi, \eta) & = & x_0 + \xi.(x_1 - x_0) + \eta.(x_2 - x_0) \\
+ y(\xi, \eta) & = & y_0 + \xi.(y_1 - y_0) + \eta.(y_2 - y_0)
+ \end{array}
+ \right.
+ \f]
+ Therefore, we have
+ \f$
+ J
+ =
+ \left(
+ \begin{array}{cc}
+ \frac{\partial x}{\partial \xi} & \frac{\partial x}{\partial \eta} \\
+ \frac{\partial y}{\partial \xi} & \frac{\partial y}{\partial \eta}
+ \end{array}
+ \right)
+ =
+ \left(
+ \begin{array}{cc}
+ (x_1 - x_0) & (x_2 - x_0) \\
+ (y_1 - y_0) & (y_2 - y_0) \\
+ \end{array}
+ \right)
+ \f$
+ and \f$|J| = 2A\f$. Therefore the mass matrix becomes
+ \f[
+ M
+ =
+ \rho.t.\int_0^1 \int_0^{1-\eta} a^T.a |J| \, d\xi \, d\eta
+ =
+ 2.A.\rho.t.\int_0^1 \int_0^{1-\eta}
+ \left(
+ \begin{array}{cccccc}
+ N0.N0 & 0 & N0.N1 & 0 & N0.N2 & 0 \\
+ 0 & N0.N0 & 0 & N0.N1 & 0 & N0.N2 \\
+ N1.N0 & 0 & N1.N1 & 0 & N1.N2 & 0 \\
+ 0 & N1.N0 & 0 & N1.N1 & 0 & N1.N2 \\
+ N2.N0 & 0 & N2.N1 & 0 & N2.N2 & 0 \\
+ 0 & N2.N0 & 0 & N2.N1 & 0 & N2.N2
+ \end{array}
+ \right)
+ \, d\xi \, d\eta
+ \f]
+
+ Considering the triangle in its local space (i.e. \f$x_0=y_0=y_1=0\f$, \f$x_1>0\f$, \f$y_2>0\f$), we get:
+ \f[
+ M
+ =
+ 2.A.\rho.t.
+ \left(
+ \begin{array}{cccccc}
+ \frac{1.0}{12.0} & 0 & \frac{1.0}{24.0} & 0 & \frac{1.0}{24.0} & 0 \\
+ 0 & \frac{1.0}{12.0} & 0 & \frac{1.0}{24.0} & 0 & \frac{1.0}{24.0} \\
+ \frac{1.0}{24.0} & 0 & \frac{1.0}{12.0} & 0 & \frac{1.0}{24.0} & 0 \\
+ 0 & \frac{1.0}{24.0} & 0 & \frac{1.0}{12.0} & 0 & \frac{1.0}{24.0} \\
+ \frac{1.0}{24.0} & 0 & \frac{1.0}{24.0} & 0 & \frac{1.0}{12.0} & 0 \\
+ 0 & \frac{1.0}{24.0} & 0 & \frac{1.0}{24.0} & 0 & \frac{1.0}{12.0}
+ \end{array}
+ \right)
+ =
+ \rho.A.t.
+ \left(
+ \begin{array}{cccccc}
+ \frac{1.0}{6.0} & 0 & \frac{1.0}{12.0} & 0 & \frac{1.0}{12.0} & 0 \\
+ 0 & \frac{1.0}{6.0} & 0 & \frac{1.0}{12.0} & 0 & \frac{1.0}{12.0} \\
+ \frac{1.0}{12.0} & 0 & \frac{1.0}{6.0} & 0 & \frac{1.0}{12.0} & 0 \\
+ 0 & \frac{1.0}{12.0} & 0 & \frac{1.0}{6.0} & 0 & \frac{1.0}{12.0} \\
+ \frac{1.0}{12.0} & 0 & \frac{1.0}{12.0} & 0 & \frac{1.0}{6.0} & 0 \\
+ 0 & \frac{1.0}{12.0} & 0 & \frac{1.0}{12.0} & 0 & \frac{1.0}{6.0}
+ \end{array}
+ \right)
+ \f]
+ which needs to be assembled properly in the full shell element mass matrix.
+*/
\ No newline at end of file
diff --git a/SurgSim/Physics/Fem2DElementTriangle_computeLocalPlateMass.dox b/SurgSim/Physics/Fem2DElementTriangle_computeLocalPlateMass.dox
new file mode 100644
index 0000000..6d64f78
--- /dev/null
+++ b/SurgSim/Physics/Fem2DElementTriangle_computeLocalPlateMass.dox
@@ -0,0 +1,165 @@
+/*!
+ \fn void SurgSim::Physics::Fem2DElementTriangle::computeLocalPlateMass(const SurgSim::Math::OdeState& state, Eigen::Matrix<double, 18, 18>* localMassMatrix)
+ <br>
+
+ The mass matrix is derived from the kinetic energy, which depends on the displacements \f$\mathbf{u}\f$.<br>
+ In the case of triangle plate, the displacements are a subset of the full shell displacements:
+ \f[ \displaystyle
+ \mathbf{u}(x, y, z)
+ =
+ \left(
+ \begin{array}{c}
+ u(x, y, z) = z.\beta_x(x, y) \\
+ v(x, y, z) = z.\beta_y(x, y) \\
+ w(x, y)
+ \end{array}
+ \right)
+ \f]
+
+ Note that we use the plate formulation DKT (Discrete Kirchhoff Theory) developped by Batoz in 1980.
+ In this formulation, the displacement \f$w\f$ is never formaly expressed, we only know that it is a
+ cubic polynomial function of \f$(x, y)\f$.
+ To complete the plate mass matrix calculation, we tried to use the expression of \f$w\f$ from
+ Przemieniecki book "Theory of Matrix Structural Analysis" Chapter 11.10, equation 11.57
+ (i.e. \f$w(x, y) = \mathbf{d}.C^{-1}.\mathbf{U}\f$).
+ Unfortunately, it can occur that the matrix \f$C\f$ becomes singular (example of a right isoceles
+ triangle, i.e. \f$x_0=y_0=y_1=x_2=0\f$ and \f$x_1=y_2=1\f$).
+ To overcome this issue, we prefered the Code_Aster approach developped in
+ "Shell elements: modelizations DKT, DST, DKTG and Q4g", Code_Aster, 2013, Thomas De Soza,
+ where a cubic well defined expression of \f$w(x, y)\f$ is given as:
+ \f$w(x, y) = \mathbf{d}.\mathbf{U}\f$ where \f$d_i\f$ are cubic shape functions.
+
+ Therefore, it leads to the following plate displacements:
+ \f[
+ \mathbf{u}(x, y, z)
+ =
+ \left(
+ \begin{array}{c}
+ u(x, y, z) = z.\beta_x(x, y) = z.\mathbf{H_x}^T(\xi, \eta).\mathbf{U} \\
+ v(x, y, z) = z.\beta_y(x, y) = z.\mathbf{H_y}^T(\xi, \eta).\mathbf{U} \\
+ w(x, y) = \mathbf{d}.\mathbf{U}
+ \end{array}
+ \right)
+ =
+ \left(
+ \begin{array}{c}
+ z.\mathbf{H_x}^T(\xi, \eta) \\
+ z.\mathbf{H_y}^T(\xi, \eta) \\
+ \mathbf{d}
+ \end{array}
+ \right)
+ .
+ \mathbf{U}
+ =
+ a.\mathbf{U}
+ \f]
+ with \f$\mathbf{U}\f$ the triangle plate nodal displacements vector (subset of the full shell nodal displacements)
+ and \f$\mathbf{H_x}\f$ and \f$\mathbf{H_y}\f$ are the Batoz shape functions.
+
+ Considering a constant mass density \f$\rho\f$ and a constant thickness \f$t\f$, the kinetic energy of the triangle plate is
+ \f$T = \int_V \frac{1}{2} \rho(x, y, z) \mathbf{\dot{u}}^T.\mathbf{\dot{u}} \, dV =
+ \frac{1}{2}.\rho.\int_{-t/2}^{+t/2} \int_A \left(\dot{\mathbf{U}}^T.a^T\right).\left(a.\dot{\mathbf{U}}\right) \, dA \, dz \f$.
+
+ The triangle plate mass matrix can be derived from the kinetic energy using the Lagrange equation of motion:
+ \f[
+ \displaystyle
+ \frac{d \frac{\partial T}{\partial \dot{\mathbf{U}}}}{d t}
+ =
+ \frac{1}{2}.\rho.\frac{d \frac{\partial \displaystyle \int_{-t/2}^{+t/2} \int_A \left(\dot{\mathbf{U}}^T.a^T\right).\left(a.\dot{\mathbf{U}}\right) \, dA \, dz}{\partial \dot{\mathbf{U}}}}{d t}
+ =
+ \frac{1}{2}.\rho.\frac{d \displaystyle \int_{-t/2}^{+t/2} \int_A 2.a^T.a.\dot{\mathbf{U}} \, dA \, dz}{d t}
+ \\
+ =
+ \left( \rho.\int_{-t/2}^{+t/2} \int_A a^T.a \, dA \, dz \right).\ddot{\mathbf{U}}
+ =
+ \left( \rho.\int_{-t/2}^{+t/2} \int_A
+ (z.\mathbf{H_x}).(z.\mathbf{H_x}^T) +
+ (z.\mathbf{H_y}).(z.\mathbf{H_y}^T) +
+ \mathbf{d}^T).\mathbf{d}
+ \, dA \, dz \right).\ddot{\mathbf{U}}
+ \f]
+ Therefore, the mass matrix is:
+ \f[
+ M
+ = \rho.\int_{-t/2}^{+t/2} \int_A (z.\mathbf{H_x}).(z.\mathbf{H_x}^T) \, dA \, dz
+ + \rho.\int_{-t/2}^{+t/2} \int_A (z.\mathbf{H_y}).(z.\mathbf{H_y}^T) \, dA \, dz
+ + \rho.\int_{-t/2}^{+t/2} \int_A \mathbf{d}^T.\mathbf{d} \, dA \, dz
+ \f]
+ It clearly appears that the triangle plate mass matrix is composed of 3 matrices,
+ each providing the contribution of a displacement (i.e. \f$(u, v, w)\f$) associated to a
+ given degree of freedom (i.e. respectively \f$(\theta_y, \theta_x, z)\f$). Let's note these
+ 3 matrices \f$M_u\f$, \f$M_v\f$ and \f$M_w\f$.
+
+ To integrate over the triangle area, we operate the following variables change:
+ \f[
+ \left\{
+ \begin{array}{ccc}
+ x(\xi, \eta) & = & x_0 + \xi.(x_1 - x_0) + \eta.(x_2 - x_0) \\
+ y(\xi, \eta) & = & y_0 + \xi.(y_1 - y_0) + \eta.(y_2 - y_0)
+ \end{array}
+ \right.
+ \f]
+ Therefore, we have
+ \f$
+ J
+ =
+ \left(
+ \begin{array}{cc}
+ \frac{\partial x}{\partial \xi} & \frac{\partial x}{\partial \eta} \\
+ \frac{\partial y}{\partial \xi} & \frac{\partial y}{\partial \eta}
+ \end{array}
+ \right)
+ =
+ \left(
+ \begin{array}{cc}
+ (x_1 - x_0) & (x_2 - x_0) \\
+ (y_1 - y_0) & (y_2 - y_0) \\
+ \end{array}
+ \right)
+ \f$
+ and \f$|J| = 2A\f$. Therefore the mass matrices become (noting that the shape functions \f$d_i\f$ do not depend on \f$z\f$):
+ \f[
+ \begin{array}{ccc}
+ M_u
+ & = &
+ \rho.\int_{-t/2}^{+t/2} \int_A (z.\mathbf{H_x}).(z.\mathbf{H_x}^T) \, dA \, dz
+ =
+ \rho.\int_{-t/2}^{+t/2} z^2 \, dz.\int_A \mathbf{H_x}.\mathbf{H_x}^T \, dA
+ \\
+ & = &
+ \rho.\frac{t^3}{12}.\int_0^1 \int_0^{1-\eta} \mathbf{H_x}.\mathbf{H_x}^T |J| \, d\xi d\eta
+ =
+ 2.A.\rho.\frac{t^3}{12}.\int_0^1 \int_0^{1-\eta} \mathbf{H_x}.\mathbf{H_x}^T \, d\xi d\eta
+ \\
+ M_v
+ & = &
+ \rho.\int_{-t/2}^{+t/2} \int_A (z.\mathbf{H_y}).(z.\mathbf{H_y}^T) \, dA \, dz
+ =
+ \rho.\int_{-t/2}^{+t/2} z^2 \, dz.\int_A \mathbf{H_y}.\mathbf{H_y}^T \, dA
+ \\
+ & = &
+ \rho.\frac{t^3}{12}.\int_0^1 \int_0^{1-\eta} \mathbf{H_y}.\mathbf{H_y}^T |J| \, d\xi d\eta
+ =
+ 2.A.\rho.\frac{t^3}{12}.\int_0^1 \int_0^{1-\eta} \mathbf{H_y}.\mathbf{H_y}^T \, d\xi d\eta
+ \\
+ M_w
+ & = &
+ \rho.\int_{-t/2}^{+t/2} \int_A \mathbf{d}^T.\mathbf{d} \, dA \, dz
+ =
+ 2.A.\rho.t.\int_0^1 \int_0^{1-\eta} \mathbf{d}^T.\mathbf{d} \, d\xi d\eta
+ \end{array}
+ \f]
+
+ To evaluate the 3 integral terms
+ \f[
+ \begin{array}{ccccc}
+ \int_0^1 \int_0^{1-\eta} \mathbf{H_x}.\mathbf{H_x}^T \, d\xi d\eta & , &
+ \int_0^1 \int_0^{1-\eta} \mathbf{H_y}.\mathbf{H_y}^T \, d\xi d\eta & , &
+ \int_0^1 \int_0^{1-\eta} \mathbf{d}^T.\mathbf{d} \, d\xi d\eta
+ \end{array}
+ \f]
+ we used a formal mathematical package and tested the corresponding code against a numerically exact evaluation
+ using the appropriate Gauss quadrature on the triangle.
+
+ The resulting triangle plate mass matrix needs to be assembled properly in the full shell element mass matrix.
+*/
\ No newline at end of file
diff --git a/SurgSim/Physics/Fem2DLocalization.cpp b/SurgSim/Physics/Fem2DLocalization.cpp
new file mode 100644
index 0000000..d523b58
--- /dev/null
+++ b/SurgSim/Physics/Fem2DLocalization.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/Physics/Fem2DLocalization.h"
+
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/FemElement.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+Fem2DLocalization::Fem2DLocalization(
+ std::shared_ptr<Representation> representation,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition) :
+ FemLocalization(representation, localPosition)
+{
+}
+
+Fem2DLocalization::~Fem2DLocalization()
+{
+}
+
+bool Fem2DLocalization::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);
+}
+
+Math::RigidTransform3d Fem2DLocalization::getElementPose()
+{
+ auto femRepresentation = std::static_pointer_cast<Fem2DRepresentation>(getRepresentation());
+ auto position = getLocalPosition();
+ auto femElement = femRepresentation->getFemElement(getLocalPosition().index);
+ const auto& nodeIds = femElement->getNodeIds();
+ std::array<Math::Vector3d, 3> nodePositions =
+ {femRepresentation->getCurrentState()->getPosition(nodeIds[0]),
+ femRepresentation->getCurrentState()->getPosition(nodeIds[1]),
+ femRepresentation->getCurrentState()->getPosition(nodeIds[2])};
+
+ Math::Vector3d edge, normal, binormal;
+ edge = (nodePositions[1] - nodePositions[0]).normalized();
+ normal = (nodePositions[2] - nodePositions[0]).cross(edge).normalized();
+ binormal = edge.cross(normal);
+ Math::Matrix33d rotation;
+ rotation << edge, normal, binormal;
+
+ return Math::makeRigidTransform(rotation, (nodePositions[0] + nodePositions[1] + nodePositions[2]) / 3.0);
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem2DLocalization.h b/SurgSim/Physics/Fem2DLocalization.h
new file mode 100644
index 0000000..77a7ce3
--- /dev/null
+++ b/SurgSim/Physics/Fem2DLocalization.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_FEM2DLOCALIZATION_H
+#define SURGSIM_PHYSICS_FEM2DLOCALIZATION_H
+
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/Physics/FemLocalization.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Implementation of Localization for Fem2DRepresentation
+///
+/// Fem2DLocalization tracks the global coordinates of an IndexedLocalCoordinate associated with an
+/// Fem2DRepresentation. The IndexedLocalCoordinate must be related to an FemElement (the index is an FemElement id and
+/// the local coordinates are the barycentric coordinates of the nodes in this FemElement).
+class Fem2DLocalization : public FemLocalization
+{
+public:
+ /// Constructor
+ /// \param representation The representation to assign to this localization.
+ /// \param localCoordinate The indexed local coordinate relative to the representation.
+ Fem2DLocalization(std::shared_ptr<Representation> representation,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localCoordinate);
+
+ /// Destructor
+ virtual ~Fem2DLocalization();
+
+ /// Query if 'representation' is valid representation.
+ /// \param representation The representation.
+ /// \return true if valid representation, false if not.
+ bool isValidRepresentation(std::shared_ptr<Representation> representation) override;
+
+ Math::RigidTransform3d getElementPose() override;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM2DLOCALIZATION_H
diff --git a/SurgSim/Physics/Fem2DPlyReaderDelegate.cpp b/SurgSim/Physics/Fem2DPlyReaderDelegate.cpp
index 213ef88..8f14ca7 100644
--- a/SurgSim/Physics/Fem2DPlyReaderDelegate.cpp
+++ b/SurgSim/Physics/Fem2DPlyReaderDelegate.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2014, SimQuest Solutions Inc.
+// Copyright 2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -13,24 +13,26 @@
// 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"
+
+using SurgSim::DataStructures::PlyReader;
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())
+Fem2DPlyReaderDelegate::Fem2DPlyReaderDelegate()
+{
+
+}
+
+Fem2DPlyReaderDelegate::Fem2DPlyReaderDelegate(std::shared_ptr<Fem2D> mesh) :
+ m_mesh(mesh)
{
+ SURGSIM_ASSERT(mesh != nullptr) << "The mesh cannot be null.";
+ mesh->clear();
}
std::string Fem2DPlyReaderDelegate::getElementName() const
@@ -42,11 +44,11 @@ bool Fem2DPlyReaderDelegate::registerDelegate(PlyReader* reader)
{
FemPlyReaderDelegate::registerDelegate(reader);
- // Thickness Processing
+ // Thickness processing
reader->requestElement(
"thickness",
std::bind(
- &Fem2DPlyReaderDelegate::beginThickness, this, std::placeholders::_1, std::placeholders::_2),
+ &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);
@@ -57,19 +59,60 @@ bool Fem2DPlyReaderDelegate::registerDelegate(PlyReader* reader)
bool Fem2DPlyReaderDelegate::fileIsAcceptable(const PlyReader& reader)
{
bool result = FemPlyReaderDelegate::fileIsAcceptable(reader);
+
result = result && reader.hasProperty("thickness", "value");
return result;
}
+void Fem2DPlyReaderDelegate::endParseFile()
+{
+ if (!m_hasPerElementMaterial && m_hasMaterial)
+ {
+ for (auto element : m_mesh->getElements())
+ {
+ element->thickness = m_thickness;
+ element->massDensity = m_materialData.massDensity;
+ element->poissonRatio = m_materialData.poissonRatio;
+ element->youngModulus = m_materialData.youngModulus;
+ }
+ }
+ m_mesh->update();
+}
+
+void Fem2DPlyReaderDelegate::processVertex(const std::string& elementName)
+{
+ FemElementStructs::RotationVectorData data;
+
+ if (m_hasRotationDOF)
+ {
+ data.thetaX = m_vertexData.thetaX;
+ data.thetaY = m_vertexData.thetaY;
+ data.thetaZ = m_vertexData.thetaZ;
+ }
+
+ Fem2D::VertexType vertex(SurgSim::Math::Vector3d(m_vertexData.x, m_vertexData.y, m_vertexData.z), data);
+
+ m_mesh->addVertex(vertex);
+}
+
void Fem2DPlyReaderDelegate::processFemElement(const std::string& elementName)
{
- SURGSIM_ASSERT(m_femData.vertexCount == 3) << "Cannot process 2D Element with "
- << m_femData.vertexCount << " vertices.";
+ SURGSIM_ASSERT(m_elementData.vertexCount == 3) << "Cannot process 2D Element with "
+ << m_elementData.vertexCount << " vertices.";
+
+ auto femElement = std::make_shared<FemElementStructs::FemElement2DParameter>();
+ femElement->nodeIds.resize(m_elementData.vertexCount);
+ std::copy(m_elementData.indices, m_elementData.indices + m_elementData.vertexCount, femElement->nodeIds.data());
- 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));
+ if (m_hasPerElementMaterial)
+ {
+ femElement->massDensity = m_elementData.massDensity;
+ femElement->poissonRatio = m_elementData.poissonRatio;
+ femElement->youngModulus = m_elementData.youngModulus;
+ }
+
+ m_mesh->addElement(femElement);
}
void* Fem2DPlyReaderDelegate::beginThickness(const std::string& elementName, size_t thicknessCount)
@@ -79,18 +122,13 @@ void* Fem2DPlyReaderDelegate::beginThickness(const std::string& elementName, siz
void Fem2DPlyReaderDelegate::endThickness(const std::string& elementName)
{
- SURGSIM_ASSERT(SurgSim::Math::isValid(m_thickness)) << "No thickness information processed.";
+ SURGSIM_ASSERT(SurgSim::Math::isValid(m_thickness)) << "No radius information processed.";
}
-void Fem2DPlyReaderDelegate::endParseFile()
+void Fem2DPlyReaderDelegate::processBoundaryCondition(const std::string& elementName)
{
- for (size_t i = 0; i < m_fem->getNumFemElements(); ++i)
- {
- std::static_pointer_cast<Fem2DElementTriangle>(m_fem->getFemElement(i))->setThickness(m_thickness);
- }
-
- FemPlyReaderDelegate::endParseFile();
+ m_mesh->addBoundaryCondition(static_cast<size_t>(m_boundaryConditionData));
}
-}; // namespace Physics
-}; // namespace SurgSim
\ No newline at end of file
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem2DPlyReaderDelegate.h b/SurgSim/Physics/Fem2DPlyReaderDelegate.h
index a0c6db9..d4f2f84 100644
--- a/SurgSim/Physics/Fem2DPlyReaderDelegate.h
+++ b/SurgSim/Physics/Fem2DPlyReaderDelegate.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2014, SimQuest Solutions Inc.
+// Copyright 2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,32 +16,41 @@
#ifndef SURGSIM_PHYSICS_FEM2DPLYREADERDELEGATE_H
#define SURGSIM_PHYSICS_FEM2DPLYREADERDELEGATE_H
+#include <array>
#include <memory>
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Physics/Fem2D.h"
#include "SurgSim/Physics/FemPlyReaderDelegate.h"
namespace SurgSim
{
namespace Physics
{
-class Fem2DRepresentation;
-/// Implementation of PlyReaderDelegate for Fem2DRepresentation
-class Fem2DPlyReaderDelegate : public SurgSim::Physics::FemPlyReaderDelegate
+class Fem2DPlyReaderDelegate : public FemPlyReaderDelegate
{
public:
- /// Constructor
- /// \param fem The object that is updated when PlyReader::parseFile is called.
- explicit Fem2DPlyReaderDelegate(std::shared_ptr<Fem2DRepresentation> fem);
+ /// Default constructor.
+ Fem2DPlyReaderDelegate();
+
+ /// Constructor.
+ /// \param mesh The mesh to be used, it will be cleared by the constructor.
+ explicit Fem2DPlyReaderDelegate(std::shared_ptr<Fem2D> mesh);
protected:
- virtual std::string getElementName() const override;
+ std::string getElementName() const override;
+
+ bool registerDelegate(PlyReader* reader) override;
+
+ bool fileIsAcceptable(const PlyReader& reader) override;
+
+ void endParseFile() override;
- virtual bool registerDelegate(SurgSim::DataStructures::PlyReader* reader) override;
- virtual bool fileIsAcceptable(const SurgSim::DataStructures::PlyReader& reader) override;
+ void processVertex(const std::string& elementName) override;
- virtual void endParseFile() override;
- virtual void processFemElement(const std::string& elementName) override;
+ void processFemElement(const std::string& elementName) override;
/// Callback function, begin the processing of thickness.
/// \param elementName Name of the element.
@@ -49,15 +58,22 @@ protected:
/// \return memory for thickness data to the reader.
void* beginThickness(const std::string& elementName, size_t thicknessCount);
- /// Callback function, end the processing of thickness.
+ /// Callback function, end the processing of radius.
/// \param elementName Name of the element.
void endThickness(const std::string& elementName);
+ void processBoundaryCondition(const std::string& elementName) override;
+
private:
+
+ /// Element's thickness information
double m_thickness;
+
+ /// Fem2D mesh asset to contain the ply file information
+ std::shared_ptr<Fem2D> m_mesh;
};
} // namespace Physics
} // namespace SurgSim
-#endif // SURGSIM_PHYSICS_FEM2DPLYREADERDELEGATE_H
\ No newline at end of file
+#endif // SURGSIM_PHYSICS_FEM2DPLYREADERDELEGATE_H
diff --git a/SurgSim/Physics/Fem2DRepresentation.cpp b/SurgSim/Physics/Fem2DRepresentation.cpp
index 77ced0e..4c8bbcd 100644
--- a/SurgSim/Physics/Fem2DRepresentation.cpp
+++ b/SurgSim/Physics/Fem2DRepresentation.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -13,14 +13,17 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/DataStructures/Location.h"
#include "SurgSim/Framework/Assert.h"
-#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/Asset.h"
#include "SurgSim/Math/OdeState.h"
-#include "SurgSim/Physics/Fem2DPlyReaderDelegate.h"
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Physics/Fem2DElementTriangle.h"
+#include "SurgSim/Physics/Fem2DLocalization.h"
#include "SurgSim/Physics/Fem2DRepresentation.h"
-#include "SurgSim/Physics/Fem2DRepresentationLocalization.h"
#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/Localization.h"
namespace
{
@@ -32,13 +35,12 @@ void transformVectorByBlockOf3(const SurgSim::Math::RigidTransform3d& transform,
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)";
+ << "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;
}
}
@@ -46,45 +48,109 @@ void transformVectorByBlockOf3(const SurgSim::Math::RigidTransform3d& transform,
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
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Fem2DRepresentation, std::shared_ptr<SurgSim::Framework::Asset>, Fem, getFem,
+ setFem);
+ SURGSIM_ADD_SETTER(Fem2DRepresentation, std::string, FemFileName, loadFem)
+ // Reminder: m_numDofPerNode is held by DeformableRepresentation but needs to be set by all
+ // concrete derived classes
m_numDofPerNode = 6;
+ m_fem = std::make_shared<Fem2D>();
}
Fem2DRepresentation::~Fem2DRepresentation()
{
}
+void Fem2DRepresentation::loadFem(const std::string& fileName)
+{
+ auto mesh = std::make_shared<Fem2D>();
+ mesh->load(fileName);
+ setFem(mesh);
+}
+
+void Fem2DRepresentation::setFem(std::shared_ptr<Framework::Asset> mesh)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "The Fem cannot be set after initialization";
+
+ SURGSIM_ASSERT(mesh != nullptr) << "Mesh for Fem2DRepresentation cannot be a nullptr";
+ auto femMesh = std::dynamic_pointer_cast<Fem2D>(mesh);
+ SURGSIM_ASSERT(femMesh != nullptr)
+ << "Mesh for Fem2DRepresentation needs to be a SurgSim::Physics::Fem2D";
+ m_fem = femMesh;
+ auto state = std::make_shared<Math::OdeState>();
+
+ state->setNumDof(getNumDofPerNode(), m_fem->getNumVertices());
+ for (size_t i = 0; i < m_fem->getNumVertices(); i++)
+ {
+ state->getPositions().segment<3>(getNumDofPerNode() * i) = m_fem->getVertexPosition(i);
+ }
+ for (auto boundaryCondition : m_fem->getBoundaryConditions())
+ {
+ state->addBoundaryCondition(boundaryCondition);
+ }
+
+ // If we have elements, ensure that they are all of the same nature
+ if (femMesh->getNumElements() > 0)
+ {
+ const auto& e0 = femMesh->getElement(0);
+ for (auto const& e : femMesh->getElements())
+ {
+ SURGSIM_ASSERT(e->nodeIds.size() == e0->nodeIds.size()) <<
+ "Cannot mix and match elements of different nature." <<
+ " Found an element with " << e->nodeIds.size() << " nodes but was expecting " << e0->nodeIds.size();
+ }
+
+ // If the FemElement types hasn't been registered yet, let's set a default one
+ if (getFemElementType().empty())
+ {
+ if (e0->nodeIds.size() == 3)
+ {
+ Fem2DElementTriangle triangle;
+ setFemElementType(triangle.getClassName());
+ }
+ }
+ }
+
+ setInitialState(state);
+}
+
+std::shared_ptr<Fem2D> Fem2DRepresentation::getFem() const
+{
+ return m_fem;
+}
+
void Fem2DRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
- SurgSim::Math::Vector& generalizedForce,
- const SurgSim::Math::Matrix& K,
- const SurgSim::Math::Matrix& D)
+ const Math::Vector& generalizedForce,
+ const Math::Matrix& K,
+ const Math::Matrix& D)
{
+ using SurgSim::Math::SparseMatrix;
+
const size_t dofPerNode = getNumDofPerNode();
- const SurgSim::Math::Matrix::Index expectedSize = static_cast<const SurgSim::Math::Matrix::Index>(dofPerNode);
+ const Math::Matrix::Index expectedSize = static_cast<const 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;
+ "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;
+ "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;
+ "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)";
+ std::shared_ptr<Fem2DLocalization> localization2D =
+ std::dynamic_pointer_cast<Fem2DLocalization>(localization);
+ SURGSIM_ASSERT(localization2D != nullptr) << "Invalid localization type (not a Fem2DLocalization)";
const size_t elementId = localization2D->getLocalPosition().index;
- const SurgSim::Math::Vector& coordinate = localization2D->getLocalPosition().coordinate;
+ const Math::Vector& coordinate = localization2D->getLocalPosition().coordinate;
std::shared_ptr<FemElement> element = getFemElement(elementId);
size_t index = 0;
@@ -104,13 +170,17 @@ void Fem2DRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localizati
{
if (K.size() != 0)
{
- m_externalGeneralizedStiffness.block(dofPerNode * nodeId1, dofPerNode * nodeId2,
- dofPerNode, dofPerNode) += coordinate[index1] * coordinate[index2] * K;
+ Math::addSubMatrix(coordinate[index1] * coordinate[index2] * K,
+ static_cast<SparseMatrix::Index>(nodeId1),
+ static_cast<SparseMatrix::Index>(nodeId2),
+ &m_externalGeneralizedStiffness, true);
}
if (D.size() != 0)
{
- m_externalGeneralizedDamping.block(dofPerNode * nodeId1, dofPerNode * nodeId2,
- dofPerNode, dofPerNode) += coordinate[index1] * coordinate[index2] * D;
+ Math::addSubMatrix(coordinate[index1] * coordinate[index2] * D,
+ static_cast<SparseMatrix::Index>(nodeId1),
+ static_cast<SparseMatrix::Index>(nodeId2),
+ &m_externalGeneralizedDamping, true);
}
index2++;
}
@@ -118,28 +188,89 @@ void Fem2DRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localizati
index1++;
}
}
+ m_externalGeneralizedStiffness.makeCompressed();
+ m_externalGeneralizedDamping.makeCompressed();
+ m_hasExternalGeneralizedForce = true;
}
-RepresentationType Fem2DRepresentation::getType() const
+
+std::shared_ptr<Localization> Fem2DRepresentation::createNodeLocalization(size_t nodeId)
{
- return REPRESENTATION_TYPE_FEM2D;
+ DataStructures::IndexedLocalCoordinate coordinate;
+
+ SURGSIM_ASSERT(nodeId >= 0 && nodeId < getCurrentState()->getNumNodes()) << "Invalid node id";
+
+ // Look for any element that contains this node
+ bool foundNodeId = false;
+ for (size_t elementId = 0; elementId < getNumFemElements(); elementId++)
+ {
+ auto element = getFemElement(elementId);
+ auto found = std::find(element->getNodeIds().begin(), element->getNodeIds().end(), nodeId);
+ if (found != element->getNodeIds().end())
+ {
+ coordinate.index = elementId;
+ coordinate.coordinate.setZero(element->getNumNodes());
+ coordinate.coordinate[found - element->getNodeIds().begin()] = 1.0;
+ foundNodeId = true;
+ break;
+ }
+ }
+ SURGSIM_ASSERT(foundNodeId) << "Could not find any element containing the node " << nodeId;
+
+ // Fem2DLocalization will verify the coordinate (2nd parameter) based on
+ // the Fem2DRepresentation passed as 1st parameter.
+ return std::make_shared<Fem2DLocalization>(
+ std::static_pointer_cast<Physics::Representation>(getSharedPtr()), coordinate);
}
-std::shared_ptr<FemPlyReaderDelegate> Fem2DRepresentation::getDelegate()
+std::shared_ptr<Localization> Fem2DRepresentation::createElementLocalization(
+ const DataStructures::IndexedLocalCoordinate& location)
{
- auto thisAsSharedPtr = std::static_pointer_cast<Fem2DRepresentation>(shared_from_this());
- auto readerDelegate = std::make_shared<Fem2DPlyReaderDelegate>(thisAsSharedPtr);
+ return std::make_shared<Fem2DLocalization>(
+ std::static_pointer_cast<Physics::Representation>(getSharedPtr()), location);
+}
- return readerDelegate;
+std::shared_ptr<Localization> Fem2DRepresentation::createLocalization(const DataStructures::Location& location)
+{
+ if (location.index.hasValue())
+ {
+ return createNodeLocalization(*location.index);
+ }
+ else if (location.triangleMeshLocalCoordinate.hasValue())
+ {
+ // In the 2d case, elements are triangles, so Locations of type triangleMesh and elementMesh refer to the same
+ // mesh. The distinction between an element and a triangle is mostly useful in the 3d case to separate the
+ // surface from the volume.
+ return createElementLocalization(*location.triangleMeshLocalCoordinate);
+ }
+ else if (location.elementMeshLocalCoordinate.hasValue())
+ {
+ return createElementLocalization(*location.elementMeshLocalCoordinate);
+ }
+
+ SURGSIM_FAILURE() << "Localization cannot be created without a mesh-based location (node, triangle or element).";
+
+ return nullptr;
}
-void Fem2DRepresentation::transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
- const SurgSim::Math::RigidTransform3d& transform)
+void Fem2DRepresentation::transformState(std::shared_ptr<Math::OdeState> state,
+ const Math::RigidTransform3d& transform)
{
transformVectorByBlockOf3(transform, &state->getPositions());
transformVectorByBlockOf3(transform, &state->getVelocities(), true);
}
-} // namespace Physics
+bool Fem2DRepresentation::doInitialize()
+{
+ for (auto& element : m_fem->getElements())
+ {
+ std::shared_ptr<FemElement> femElement;
+ femElement = FemElement::getFactory().create(getFemElementType(), element);
+ m_femElements.push_back(femElement);
+ }
+ return FemRepresentation::doInitialize();
+}
+
+} // namespace Physics
} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem2DRepresentation.h b/SurgSim/Physics/Fem2DRepresentation.h
index ddb3517..1b8d765 100644
--- a/SurgSim/Physics/Fem2DRepresentation.h
+++ b/SurgSim/Physics/Fem2DRepresentation.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -20,18 +20,33 @@
#include <string>
#include "SurgSim/Framework/FrameworkConvert.h"
-#include "SurgSim/Framework/ObjectFactory.h"
#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem2D.h"
#include "SurgSim/Physics/FemRepresentation.h"
namespace SurgSim
{
+namespace DataStructures
+{
+struct IndexedLocalCoordinate;
+struct Location;
+}
+namespace Framework
+{
+class Asset;
+}
+namespace Math
+{
+class OdeState;
+}
namespace Physics
{
-SURGSIM_STATIC_REGISTRATION(Fem2DRepresentation);
+class Localization;
-class Fem2DPlyReaderDelegate;
+SURGSIM_STATIC_REGISTRATION(Fem2DRepresentation);
/// Finite Element Model 2D is a fem built with 2D FemElement
class Fem2DRepresentation : public FemRepresentation
@@ -46,19 +61,43 @@ public:
SURGSIM_CLASSNAME(SurgSim::Physics::Fem2DRepresentation);
- virtual void addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
- SurgSim::Math::Vector& generalizedForce,
+ void loadFem(const std::string& fileName) override;
+
+ /// Sets the fem mesh asset
+ /// \param mesh The fem mesh to assign to this representation
+ /// \exception SurgSim::Framework::AssertionFailure if mesh is nullptr or it's actual type is not Fem2D
+ void setFem(std::shared_ptr<SurgSim::Framework::Asset> mesh);
+
+ /// \return The fem mesh asset as a Fem2D
+ std::shared_ptr<Fem2D> getFem() const;
+
+ void addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ const 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;
+ std::shared_ptr<Localization> createLocalization(const SurgSim::DataStructures::Location& location) override;
protected:
- virtual void transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
- const SurgSim::Math::RigidTransform3d& transform) override;
+ void transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
+ const SurgSim::Math::RigidTransform3d& transform) override;
+
+ bool doInitialize() override;
private:
- virtual std::shared_ptr<FemPlyReaderDelegate> getDelegate() override;
+ /// Helper method: create a localization for a node
+ /// \param nodeId The node index
+ /// \return Localization of the node for this representation
+ std::shared_ptr<Localization> createNodeLocalization(size_t nodeId);
+
+ /// Helper method: create a localization for an element-based IndexedLocalCoordinate (triangle)
+ /// \param location The IndexedLocalCoordinate defining a point on the element mesh
+ /// \return Localization of the point for this representation
+ std::shared_ptr<Localization> createElementLocalization(
+ const SurgSim::DataStructures::IndexedLocalCoordinate& location);
+
+ /// The Fem2DRepresentation's asset as a Fem2D
+ std::shared_ptr<Fem2D> m_fem;
};
} // namespace Physics
diff --git a/SurgSim/Physics/Fem2DRepresentationLocalization.cpp b/SurgSim/Physics/Fem2DRepresentationLocalization.cpp
deleted file mode 100644
index 6807610..0000000
--- a/SurgSim/Physics/Fem2DRepresentationLocalization.cpp
+++ /dev/null
@@ -1,98 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index a43b427..0000000
--- a/SurgSim/Physics/Fem2DRepresentationLocalization.h
+++ /dev/null
@@ -1,72 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/Fem3D.cpp b/SurgSim/Physics/Fem3D.cpp
new file mode 100644
index 0000000..cf8fbb2
--- /dev/null
+++ b/SurgSim/Physics/Fem3D.cpp
@@ -0,0 +1,37 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Fem3D.h"
+#include "SurgSim/Physics/Fem3DPlyReaderDelegate.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+SURGSIM_REGISTER(SurgSim::Framework::Asset, SurgSim::Physics::Fem3D, Fem3D)
+
+Fem3D::Fem3D()
+{
+}
+
+bool Fem3D::doLoad(const std::string& filePath)
+{
+ return loadFemFile<Fem3DPlyReaderDelegate, Fem3D>(filePath);
+}
+
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem3D.h b/SurgSim/Physics/Fem3D.h
new file mode 100644
index 0000000..bf3232a
--- /dev/null
+++ b/SurgSim/Physics/Fem3D.h
@@ -0,0 +1,47 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_FEM3D_H
+#define SURGSIM_PHYSICS_FEM3D_H
+
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/Physics/Fem.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+SURGSIM_STATIC_REGISTRATION(Fem3D);
+
+/// Fem class data structure implementation for 3-Dimensional FEMs
+/// \sa Fem
+class Fem3D : public Fem<SurgSim::DataStructures::EmptyData, FemElementStructs::FemElement3DParameter>
+{
+public:
+ /// Default constructor
+ Fem3D();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::Fem3D);
+
+protected:
+ // Asset API override
+ bool doLoad(const std::string& filePath) override;
+};
+
+} // namespace Physics
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM3D_H
diff --git a/SurgSim/Physics/Fem3DElementCorotationalTetrahedron.cpp b/SurgSim/Physics/Fem3DElementCorotationalTetrahedron.cpp
index 69be499..9cda073 100644
--- a/SurgSim/Physics/Fem3DElementCorotationalTetrahedron.cpp
+++ b/SurgSim/Physics/Fem3DElementCorotationalTetrahedron.cpp
@@ -14,10 +14,10 @@
// limitations under the License.
#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/OdeEquation.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;
@@ -27,9 +27,22 @@ namespace SurgSim
namespace Physics
{
+SURGSIM_REGISTER(SurgSim::Physics::FemElement, SurgSim::Physics::Fem3DElementCorotationalTetrahedron,
+ Fem3DElementCorotationalTetrahedron)
-Fem3DElementCorotationalTetrahedron::Fem3DElementCorotationalTetrahedron(std::array<size_t, 4> nodeIds)
- : Fem3DElementTetrahedron(nodeIds)
+Fem3DElementCorotationalTetrahedron::Fem3DElementCorotationalTetrahedron() :
+ Fem3DElementTetrahedron()
+{
+}
+
+Fem3DElementCorotationalTetrahedron::Fem3DElementCorotationalTetrahedron(std::array<size_t, 4> nodeIds) :
+ Fem3DElementTetrahedron(nodeIds)
+{
+}
+
+Fem3DElementCorotationalTetrahedron::Fem3DElementCorotationalTetrahedron(
+ std::shared_ptr<FemElementStructs::FemElementParameter> elementData) :
+ Fem3DElementTetrahedron(elementData)
{
}
@@ -38,91 +51,85 @@ void Fem3DElementCorotationalTetrahedron::initialize(const SurgSim::Math::OdeSta
// 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;
+ m_MLinear = m_M;
+ m_KLinear = 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();
+ SurgSim::Math::Matrix44d V;
+ V.col(0).segment<3>(0) = state.getPosition(m_nodeIds[0]);
+ V.col(1).segment<3>(0) = state.getPosition(m_nodeIds[1]);
+ V.col(2).segment<3>(0) = state.getPosition(m_nodeIds[2]);
+ V.col(3).segment<3>(0) = state.getPosition(m_nodeIds[3]);
+ V.row(3).setOnes();
double determinant;
bool invertible;
- m_V.computeInverseAndDetWithCheck(m_Vinverse, determinant, invertible);
+ 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;
+ " 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;
+
+ updateFMDK(state, Math::ODEEQUATIONUPDATE_FMDK);
}
-void Fem3DElementCorotationalTetrahedron::addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
- double scale)
+void Fem3DElementCorotationalTetrahedron::doUpdateFMDK(const Math::OdeState& state, int options)
{
- Eigen::Matrix<double, 12, 1> x, R_x0, f;
+ SurgSim::Math::Matrix33d* rotation = nullptr;
+ Math::Matrix* M = nullptr;
+ Math::Matrix* K = nullptr;
- // 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)
+ if (options & Math::ODEEQUATIONUPDATE_F)
{
- getSubVector(R_x0, nodeId, 3) = m_rotation * getSubVector(m_x0, nodeId, 3);
+ rotation = &m_R;
+ K = &m_K;
}
- 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)
+ if (options & Math::ODEEQUATIONUPDATE_M)
{
- return;
+ M = &m_M;
}
- Eigen::Matrix<double, 12, 1> vectorLocal, resultLocal;
- getSubVector(vector, m_nodeIds, 3, &vectorLocal);
-
- // Adds the mass contribution
- if (alphaM != 0.0)
+ if (options & Math::ODEEQUATIONUPDATE_K)
{
- resultLocal = alphaM * (m_M * vectorLocal);
- addSubVector(resultLocal, m_nodeIds, 3, result);
+ K = &m_K;
}
- // Adds the stiffness contribution
- if (alphaK != 0.0)
+ computeRotationMassAndStiffness(state, rotation, M, K);
+
+ if (options & Math::ODEEQUATIONUPDATE_F)
{
- resultLocal = alphaK * (m_corotationalStiffnessMatrix * vectorLocal);
- addSubVector(resultLocal, m_nodeIds, 3, result);
+ Eigen::Matrix<double, 12, 1> x, R_x0;
+
+ // 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_R * getSubVector(m_x0, nodeId, 3);
+ }
+ m_f = -m_K * (x - R_x0);
}
}
-bool Fem3DElementCorotationalTetrahedron::update(const SurgSim::Math::OdeState& state)
+void Fem3DElementCorotationalTetrahedron::computeRotationMassAndStiffness(const SurgSim::Math::OdeState& state,
+ SurgSim::Math::Matrix33d* rotation,
+ Math::Matrix* Me,
+ Math::Matrix* Ke) const
{
using SurgSim::Math::makeSkewSymmetricMatrix;
using SurgSim::Math::skew;
using SurgSim::Math::Vector3d;
- // The update does two things:
- // 1) Recompute the element's rotation
+ SurgSim::Math::Matrix33d R;
+
+ // This method does two things:
+ // 1) Recompute the element's rotation R
// 2) Update the element's stiffness matrix based on the new rotation
- // 1) Recompute the element's rotation
+ // 1) Recompute the element's rotation R
Eigen::Matrix<double, 12, 1> x;
getSubVector(state.getPositions(), m_nodeIds, 3, &x);
@@ -134,7 +141,7 @@ bool Fem3DElementCorotationalTetrahedron::update(const SurgSim::Math::OdeState&
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.
+ // Matrix V is the matrix of the undeformed tetrahedron points in homogeneous 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);
@@ -145,23 +152,41 @@ bool Fem3DElementCorotationalTetrahedron::update(const SurgSim::Math::OdeState&
// 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);
+ F.computeRotationScaling(&R, &scaling);
- if (std::abs(m_rotation.determinant() - 1.0) > 1e-8)
+ if (rotation != nullptr)
+ {
+ *rotation = R;
+ }
+
+ if (std::abs(R.determinant() - 1.0) > 1e-8)
{
SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) <<
- "Rotation has an invalid determinant of " << m_rotation.determinant();
- return false;
+ "Rotation has an invalid determinant of " << R.determinant();
+ return;
+ }
+
+ Eigen::Matrix<double, 12, 12> R12x12;
+ if (Me != nullptr || Ke != nullptr)
+ {
+ // Build a 12x12 rotation matrix, duplicating the 3x3 rotation on the diagonal blocks
+ // This is useful only in the mass and stiffness matrices calculation.
+ R12x12.setZero();
+ for (size_t nodeId = 0; nodeId < 4; ++nodeId)
+ {
+ getSubMatrix(R12x12, nodeId, nodeId, 3, 3) = R;
+ }
}
- // 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)
+ // 2) Update the element's mass matrix based on the new rotation
+ // M = R.Me.R^t
+ if (Me != nullptr)
{
- getSubMatrix(R12x12, nodeId, nodeId, 3, 3) = m_rotation;
+ Eigen::Matrix<double, 12, 12> RM = R12x12 * m_MLinear;
+ *Me = RM * R12x12.transpose();
}
- // 2) Update the element's stiffness matrix based on the new rotation
+ // 3) 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
@@ -172,95 +197,95 @@ bool Fem3DElementCorotationalTetrahedron::update(const SurgSim::Math::OdeState&
// 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)
+ if (Ke != nullptr)
{
- SURGSIM_LOG(SurgSim::Framework::Logger::getDefaultLogger(), WARNING) <<
- "Corotational element has a singular G matrix";
- return false;
- }
+ // Here is the rotated element stiffness matrix part
+ Eigen::Matrix<double, 12, 12> RK = R12x12 * m_KLinear;
+ *Ke = RK * R12x12.transpose();
+
+ // Now we compute some useful matrices for the next step
+ double determinant;
+ bool invertible;
+ Math::Matrix33d G = (scaling.trace() * Math::Matrix33d::Identity() - scaling) * R.transpose();
+ Math::Matrix33d Ginv = Math::Matrix33d::Zero();
+ G.computeInverseAndDetWithCheck(Ginv, determinant, invertible);
+ if (!invertible)
+ {
+ SURGSIM_LOG(SurgSim::Framework::Logger::getDefaultLogger(), WARNING) <<
+ "Corotational element has a singular G matrix";
+ return;
+ }
- // 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)
+ // 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)
{
- // 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());
+ 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((R.transpose() *
+ (Vector3d::Unit(i) * Vector3d::Unit(j).transpose())).eval());
- // Compute dR/dFij = [wij].R
- dRdF[i][j] = makeSkewSymmetricMatrix(wij) * m_rotation;
+ // Compute dR/dFij = [wij].R
+ dRdF[i][j] = makeSkewSymmetricMatrix(wij) * R;
+ }
}
- }
-
- // 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)
+ // 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)
{
- size_t dofId = 3 * nodeId + axis;
+ Vector3d ni(m_Vinverse.row(nodeId).segment<3>(0));
- // Let's define the 3x3 matrix dR/d(x[dofId])
- for (int i = 0; i < 3; ++i)
+ for (int axis = 0; axis < 3; ++axis)
{
- for (int j = 0; j < 3; ++j)
+ size_t dofId = 3 * nodeId + axis;
+
+ // Let's define the 3x3 matrix dR/d(x[dofId])
+ for (int i = 0; i < 3; ++i)
{
- dRdX[dofId](i, j) = dRdF[i][j].row(axis).dot(ni);
+ 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)
+ // 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)
{
- getSubMatrix(dRdxl12x12, nodeId, nodeId, 3, 3) = dRdX[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;
+ Ke->col(dofId) += dRdxl12x12 * KTimesRx_x0 + (RK * dRdxl12x12.transpose()) * x;
+ }
}
-
- return true;
}
}; // namespace Physics
diff --git a/SurgSim/Physics/Fem3DElementCorotationalTetrahedron.h b/SurgSim/Physics/Fem3DElementCorotationalTetrahedron.h
index 7cd764d..9e2bb49 100644
--- a/SurgSim/Physics/Fem3DElementCorotationalTetrahedron.h
+++ b/SurgSim/Physics/Fem3DElementCorotationalTetrahedron.h
@@ -24,6 +24,7 @@ namespace SurgSim
namespace Physics
{
+SURGSIM_STATIC_REGISTRATION(Fem3DElementCorotationalTetrahedron);
/// 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
@@ -40,40 +41,52 @@ 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
+ Fem3DElementCorotationalTetrahedron();
+
+ /// Constructor
+ /// \param nodeIds A vector of node 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;
+ /// Constructor for FemElement object factory
+ /// \param elementData A FemElement3D struct 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.
+ /// \exception SurgSim::Framework::AssertionFailure if nodeIds has a size different than 4
+ explicit Fem3DElementCorotationalTetrahedron(std::shared_ptr<FemElementStructs::FemElementParameter> elementData);
- virtual void addMatVec(const SurgSim::Math::OdeState& state,
- double alphaM, double alphaD, double alphaK,
- const SurgSim::Math::Vector& vector, SurgSim::Math::Vector* result) override;
+ SURGSIM_CLASSNAME(SurgSim::Physics::Fem3DElementCorotationalTetrahedron);
- /// 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;
+ void initialize(const SurgSim::Math::OdeState& state) override;
protected:
- /// The element rigid rotation
- SurgSim::Math::Matrix33d m_rotation;
+ /// Compute the rotation, mass and stiffness matrices of the element from the given state
+ /// \param state The state to compute the rotation and jacobians from
+ /// \param [out] R rotation matrix of the element in the given state (can be nullptr if not needed)
+ /// \param [out] Me, Ke Respectively the mass and stiffness matrices (Me and/or Ke be nullptr if not needed)
+ /// \note The model is not viscoelastic but purely elastic, so there is no damping matrix here.
+ void computeRotationMassAndStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix33d* R,
+ Math::Matrix* Me, Math::Matrix* Ke) const;
- /// The co-rotational stiffness matrix
- Eigen::Matrix<double, 12, 12> m_corotationalStiffnessMatrix;
+ void doUpdateFMDK(const Math::OdeState& state, int options) override;
/// 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;
+
+ // The mass matrix of the linear tetrahedron
+ Eigen::Matrix<double, 12, 12> m_MLinear;
+
+ // The stiffness matrix of the linear tetrahedron
+ Eigen::Matrix<double, 12, 12> m_KLinear;
+
+ // The rotation matrix
+ SurgSim::Math::Matrix33d m_R;
};
} // namespace Physics
diff --git a/SurgSim/Physics/Fem3DElementCube.cpp b/SurgSim/Physics/Fem3DElementCube.cpp
index f12dd2f..8dfa9ce 100644
--- a/SurgSim/Physics/Fem3DElementCube.cpp
+++ b/SurgSim/Physics/Fem3DElementCube.cpp
@@ -15,10 +15,10 @@
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/OdeEquation.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;
@@ -30,21 +30,35 @@ namespace SurgSim
namespace Physics
{
+SURGSIM_REGISTER(SurgSim::Physics::FemElement, SurgSim::Physics::Fem3DElementCube, Fem3DElementCube)
+
+Fem3DElementCube::Fem3DElementCube()
+{
+ initializeMembers();
+}
+
Fem3DElementCube::Fem3DElementCube(std::array<size_t, 8> nodeIds)
{
- // Set the number of dof per node (3 in this case)
- setNumDofPerNode(3);
+ initializeMembers();
+ m_nodeIds.assign(nodeIds.cbegin(), nodeIds.cend());
+}
- // 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;
+Fem3DElementCube::Fem3DElementCube(std::shared_ptr<FemElementStructs::FemElementParameter> elementData)
+{
+ initializeMembers();
+ auto element3DData = std::dynamic_pointer_cast<FemElementStructs::FemElement3DParameter>(elementData);
+ SURGSIM_ASSERT(element3DData != nullptr) << "Incorrect struct type passed";
+ SURGSIM_ASSERT(element3DData->nodeIds.size() == 8) << "Incorrect number of nodes for Fem3D cube";
+ m_nodeIds.assign(element3DData->nodeIds.begin(), element3DData->nodeIds.end());
+ setMassDensity(element3DData->massDensity);
+ setPoissonRatio(element3DData->poissonRatio);
+ setYoungModulus(element3DData->youngModulus);
+}
- m_nodeIds.assign( nodeIds.cbegin(), nodeIds.cend());
+void Fem3DElementCube::initializeMembers()
+{
+ // Set the number of dof per node (3 in this case)
+ setNumDofPerNode(3);
}
void Fem3DElementCube::initialize(const SurgSim::Math::OdeState& state)
@@ -52,11 +66,20 @@ void Fem3DElementCube::initialize(const SurgSim::Math::OdeState& state)
// Test the validity of the physical parameters
FemElement::initialize(state);
+ // 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;
+
// 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 << "]";
+ "Invalid nodeId " << *nodeId << " expected in range [0.." << state.getNumNodes() - 1 << "]";
}
// Store the rest state for this cube in m_elementRestPosition
@@ -66,30 +89,12 @@ void Fem3DElementCube::initialize(const SurgSim::Math::OdeState& state)
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);
+ computeMass(state, &m_M);
+ computeStiffness(state, &m_strain, &m_stress, &m_K);
}
void Fem3DElementCube::computeMass(const SurgSim::Math::OdeState& state,
- Eigen::Matrix<double, 24, 24>* M)
+ SurgSim::Math::Matrix* M)
{
using SurgSim::Math::gaussQuadrature2Points;
@@ -113,25 +118,31 @@ void Fem3DElementCube::computeMass(const SurgSim::Math::OdeState& state,
for (int k = 0; k < 2; ++k)
{
addMassMatrixAtPoint(state,
- gaussQuadrature2Points[i], gaussQuadrature2Points[j], gaussQuadrature2Points[k], M);
+ gaussQuadrature2Points[i], gaussQuadrature2Points[j],
+ gaussQuadrature2Points[k], M);
}
}
}
}
-void Fem3DElementCube::addMass(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* M, double scale)
+void Fem3DElementCube::doUpdateFMDK(const Math::OdeState& state, int options)
{
- addSubMatrix(m_mass * scale, m_nodeIds, 3, M);
-}
+ if (options & Math::ODEEQUATIONUPDATE_F)
+ {
+ Eigen::Matrix<double, 24, 1> x;
-void Fem3DElementCube::addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* D, double scale)
-{
+ // K.U = Fext
+ // K.(x - x0) = Fext
+ // 0 = Fext + Fint with Fint = -K.(x - x0)
+ getSubVector(state.getPositions(), m_nodeIds, 3, &x);
+ m_f = -m_K * (x - m_elementRestPosition);
+ }
}
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)
+ SurgSim::Math::Matrix* stiffness)
{
using SurgSim::Math::gaussQuadrature2Points;
@@ -146,17 +157,18 @@ void Fem3DElementCube::computeStiffness(const SurgSim::Math::OdeState& state,
for (int k = 0; k < 2; ++k)
{
addStrainStressStiffnessAtPoint(state,
- gaussQuadrature2Points[i], gaussQuadrature2Points[j], gaussQuadrature2Points[k],
- strain, stress, stiffness);
+ 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
+ SurgSim::Math::Matrix33d* J,
+ SurgSim::Math::Matrix33d* Jinv,
+ double* detJ) const
{
using SurgSim::Framework::Logger;
@@ -175,11 +187,11 @@ void Fem3DElementCube::evaluateJ(const SurgSim::Math::OdeState& state, double ep
// 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)
+ 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);
+ (*J)(1, axis) += p[index][axis] * dShapeFunctiondeta(index, epsilon, eta, mu);
+ (*J)(2, axis) += p[index][axis] * dShapeFunctiondmu(index, epsilon, eta, mu);
}
}
@@ -189,17 +201,17 @@ void Fem3DElementCube::evaluateJ(const SurgSim::Math::OdeState& state, double ep
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";
+ "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";
+ "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
+ Eigen::Matrix<double, 6, 24>* B) const
{
SURGSIM_ASSERT(B != nullptr) << "Trying to evaluate the strain-displacmenet with a nullptr";
@@ -212,10 +224,10 @@ void Fem3DElementCube::evaluateStrainDisplacement(double epsilon, double eta, do
// 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)
- );
+ dShapeFunctiondepsilon(index, epsilon, eta, mu),
+ dShapeFunctiondeta(index, epsilon, eta, mu),
+ dShapeFunctiondmu(index, epsilon, eta, mu)
+ );
Vector3d dNidxyz = Jinv * dNidEpsilonEtaMu;
// B = (dNi/dx 0 0)
@@ -225,20 +237,20 @@ void Fem3DElementCube::evaluateStrainDisplacement(double epsilon, double eta, do
// ( 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)(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) = 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) = dNidxyz[2];
(*B)(5, getNumDofPerNode() * index + 2) = dNidxyz[0];
}
}
void Fem3DElementCube::buildConstitutiveMaterialMatrix(
- Eigen::Matrix<double, 6, 6>* constitutiveMatrix)
+ 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):
@@ -252,12 +264,12 @@ void Fem3DElementCube::buildConstitutiveMaterialMatrix(
}
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)
+ 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,
+ SurgSim::Math::Matrix* k)
{
SurgSim::Math::Matrix33d J, Jinv;
double detJ;
@@ -275,10 +287,10 @@ void Fem3DElementCube::addStrainStressStiffnessAtPoint(const SurgSim::Math::OdeS
}
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)
+ const SurgSim::Math::gaussQuadraturePoint& epsilon,
+ const SurgSim::Math::gaussQuadraturePoint& eta,
+ const SurgSim::Math::gaussQuadraturePoint& mu,
+ SurgSim::Math::Matrix* m)
{
// This helper method hels to compute:
// M = rho * integration{over volume} {phi^T.phi} dV
@@ -297,7 +309,7 @@ void Fem3DElementCube::addMassMatrixAtPoint(const SurgSim::Math::OdeState& state
for (size_t index = 0; index < 8; ++index)
{
double weightPerIndex = shapeFunction(index, epsilon.point, eta.point, mu.point);
- phi(0, getNumDofPerNode() * index ) += weightPerIndex;
+ phi(0, getNumDofPerNode() * index) += weightPerIndex;
phi(1, getNumDofPerNode() * index + 1) += weightPerIndex;
phi(2, getNumDofPerNode() * index + 2) += weightPerIndex;
}
@@ -305,61 +317,6 @@ void Fem3DElementCube::addMassMatrixAtPoint(const SurgSim::Math::OdeState& state
*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;
@@ -373,18 +330,18 @@ double Fem3DElementCube::getVolume(const SurgSim::Math::OdeState& state) const
// 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;
+ 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;
+ 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;
+ const double& mu = gaussQuadrature2Points[k].point;
+ const double& weightMu = gaussQuadrature2Points[k].weight;
SurgSim::Math::Matrix33d J, Jinv;
double detJ;
@@ -397,10 +354,12 @@ double Fem3DElementCube::getVolume(const SurgSim::Math::OdeState& state) const
}
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;
+ "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;
}
@@ -408,41 +367,41 @@ double Fem3DElementCube::getVolume(const SurgSim::Math::OdeState& state) const
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]);
+ (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]);
+ 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]);
+ (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];
+ (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
+ 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].";
+ << "naturalCoordinate must be normalized and length 8 within [0 1].";
Vector3d cartesianCoordinate(0.0, 0.0, 0.0);
@@ -457,8 +416,8 @@ SurgSim::Math::Vector Fem3DElementCube::computeCartesianCoordinate(
}
SurgSim::Math::Vector Fem3DElementCube::computeNaturalCoordinate(
- const SurgSim::Math::OdeState& state,
- const SurgSim::Math::Vector& cartesianCoordinate) const
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& cartesianCoordinate) const
{
SURGSIM_FAILURE() << "Function " << __FUNCTION__ << " not yet implemented.";
return SurgSim::Math::Vector3d::Zero();
diff --git a/SurgSim/Physics/Fem3DElementCube.h b/SurgSim/Physics/Fem3DElementCube.h
index 6a0e8bf..0963dc1 100644
--- a/SurgSim/Physics/Fem3DElementCube.h
+++ b/SurgSim/Physics/Fem3DElementCube.h
@@ -26,6 +26,7 @@ namespace SurgSim
namespace Physics
{
+SURGSIM_STATIC_REGISTRATION(Fem3DElementCube);
/// Class for Fem Element 3D based on a cube volume discretization
/// \note The stiffness property of the cube is derived from
@@ -37,10 +38,14 @@ namespace Physics
/// \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.
+/// \note Fem3DElementCube uses linear elasticity (not visco-elasticity), so it does not give any damping.
class Fem3DElementCube : public FemElement
{
public:
/// Constructor
+ Fem3DElementCube();
+
+ /// 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):
@@ -50,6 +55,19 @@ public:
/// \note simulation will keep running. Behavior will be undefined because of possible negative volume terms.
explicit Fem3DElementCube(std::array<size_t, 8> nodeIds);
+ /// Constructor for FemElement object factory
+ /// \param elementData A FemElement3D struct 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.
+ /// \exception SurgSim::Framework::AssertionFailure if nodeIds has a size different than 8
+ explicit Fem3DElementCube(std::shared_ptr<FemElementStructs::FemElementParameter> elementData);
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::Fem3DElementCube)
+
/// 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
@@ -60,93 +78,20 @@ public:
/// \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;
+ void initialize(const SurgSim::Math::OdeState& state) override;
+
+ double getVolume(const SurgSim::Math::OdeState& state) const override;
+
+ SurgSim::Math::Vector computeCartesianCoordinate(const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const override;
+
+ SurgSim::Math::Vector computeNaturalCoordinate(const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& cartesianCoordinate) const override;
protected:
+ /// Initializes variables needed before Initialize() is called
+ void initializeMembers();
+
/// Build the constitutive material 6x6 matrix
/// \param[out] constitutiveMatrix The 6x6 constitutive material matrix
void buildConstitutiveMaterialMatrix(Eigen::Matrix<double, 6, 6>* constitutiveMatrix);
@@ -155,26 +100,16 @@ protected:
/// \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);
+ Eigen::Matrix<double, 6, 24>* strain,
+ Eigen::Matrix<double, 6, 24>* stress,
+ SurgSim::Math::Matrix* 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);
+ void computeMass(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* m);
+
+ void doUpdateFMDK(const Math::OdeState& state, int options) override;
/// Helper method to evaluate strain-stress and stiffness integral terms with a discrete sum using
/// a Gauss quadrature rule
@@ -182,22 +117,22 @@ protected:
/// \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);
+ 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,
+ SurgSim::Math::Matrix* 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);
+ const SurgSim::Math::gaussQuadraturePoint& epsilon,
+ const SurgSim::Math::gaussQuadraturePoint& eta,
+ const SurgSim::Math::gaussQuadraturePoint& mu,
+ SurgSim::Math::Matrix* 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
@@ -205,9 +140,9 @@ protected:
/// \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;
+ 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
@@ -215,7 +150,7 @@ protected:
/// \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;
+ Eigen::Matrix<double, 6, 24>* B) const;
/// Cube rest volume
double m_restVolume;
@@ -294,11 +229,6 @@ protected:
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
diff --git a/SurgSim/Physics/Fem3DElementTetrahedron.cpp b/SurgSim/Physics/Fem3DElementTetrahedron.cpp
index c756674..22c8f50 100644
--- a/SurgSim/Physics/Fem3DElementTetrahedron.cpp
+++ b/SurgSim/Physics/Fem3DElementTetrahedron.cpp
@@ -15,13 +15,13 @@
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Math/OdeEquation.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;
@@ -34,7 +34,8 @@ namespace
/// \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];
+ 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];
}
};
@@ -44,12 +45,48 @@ namespace SurgSim
namespace Physics
{
+SURGSIM_REGISTER(SurgSim::Physics::FemElement, SurgSim::Physics::Fem3DElementTetrahedron, Fem3DElementTetrahedron)
+
+Fem3DElementTetrahedron::Fem3DElementTetrahedron()
+{
+ initializeMembers();
+}
Fem3DElementTetrahedron::Fem3DElementTetrahedron(std::array<size_t, 4> nodeIds)
{
+ initializeMembers();
+ m_nodeIds.assign(std::begin(nodeIds), std::end(nodeIds));
+}
+
+Fem3DElementTetrahedron::Fem3DElementTetrahedron(std::shared_ptr<FemElementStructs::FemElementParameter> elementData)
+{
+ initializeMembers();
+ auto element3DData = std::dynamic_pointer_cast<FemElementStructs::FemElement3DParameter>(elementData);
+ SURGSIM_ASSERT(element3DData != nullptr) << "Incorrect struct type passed";
+ SURGSIM_ASSERT(element3DData->nodeIds.size() == 4) << "Incorrect number of nodes for Fem3D Tetrahedron";
+ m_nodeIds.assign(element3DData->nodeIds.begin(), element3DData->nodeIds.end());
+ setMassDensity(element3DData->massDensity);
+ setPoissonRatio(element3DData->poissonRatio);
+ setYoungModulus(element3DData->youngModulus);
+}
+
+void Fem3DElementTetrahedron::initializeMembers()
+{
setNumDofPerNode(3); // 3 dof per node (x, y, z)
+}
- m_nodeIds.assign(std::begin(nodeIds), std::end(nodeIds));
+void Fem3DElementTetrahedron::doUpdateFMDK(const Math::OdeState& state, int options)
+{
+ if (options & Math::ODEEQUATIONUPDATE_F)
+ {
+ Eigen::Matrix<double, 12, 1> x;
+
+ // K.U = Fext
+ // K.(x - x0) = Fext
+ // 0 = Fext + Fint with Fint = -K.(x - x0)
+ getSubVector(state.getPositions(), m_nodeIds, 3, &x);
+ m_f = -m_K * (x - m_x0);
+ }
}
void Fem3DElementTetrahedron::initialize(const SurgSim::Math::OdeState& state)
@@ -60,7 +97,7 @@ void Fem3DElementTetrahedron::initialize(const SurgSim::Math::OdeState& 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 << "]";
+ << "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 )
@@ -78,35 +115,16 @@ void Fem3DElementTetrahedron::initialize(const SurgSim::Math::OdeState& state)
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] << "]";
+ << "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)
+ SurgSim::Math::Matrix* M)
{
// From Przemieniecki book
// -> section 11 "Inertia properties of structural elements
@@ -150,34 +168,24 @@ void Fem3DElementTetrahedron::computeMass(const SurgSim::Math::OdeState& state,
}
}
-
-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)
+ SurgSim::Math::Matrix* 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++)
+ for (int i = 0; i < 4; i++)
{
- m_strain(0, 3 * i ) = coef * m_bi[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) = 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 + 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) = coef * m_di[i];
m_strain(5, 3 * i + 2) = coef * m_bi[i];
}
@@ -199,62 +207,6 @@ void Fem3DElementTetrahedron::computeStiffness(const SurgSim::Math::OdeState& st
*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 |
@@ -399,11 +351,11 @@ void Fem3DElementTetrahedron::computeShapeFunctions(const SurgSim::Math::OdeStat
}
SurgSim::Math::Vector Fem3DElementTetrahedron::computeCartesianCoordinate(
- const SurgSim::Math::OdeState& state,
- const SurgSim::Math::Vector& naturalCoordinate) const
+ const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const
{
SURGSIM_ASSERT(isValidCoordinate(naturalCoordinate))
- << "naturalCoordinate must be normalized and length 4.";
+ << "naturalCoordinate must be normalized and length 4.";
const Vector& x = state.getPositions();
Vector3d p0 = getSubVector(x, m_nodeIds[0], 3);
@@ -412,13 +364,13 @@ SurgSim::Math::Vector Fem3DElementTetrahedron::computeCartesianCoordinate(
Vector3d p3 = getSubVector(x, m_nodeIds[3], 3);
return naturalCoordinate(0) * p0
- + naturalCoordinate(1) * p1
- + naturalCoordinate(2) * p2
- + naturalCoordinate(3) * p3;
+ + 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
+ const SurgSim::Math::OdeState& state, const SurgSim::Math::Vector& cartesianCoordinate) const
{
SURGSIM_ASSERT(cartesianCoordinate.size() == 3) << "globalCoordinate must be length 3.";
@@ -434,8 +386,8 @@ SurgSim::Math::Vector Fem3DElementTetrahedron::computeNaturalCoordinate(
for (size_t i = 0; i < 4; ++i)
{
result[i] = ai[i] + bi[i] * cartesianCoordinate[0]
- + ci[i] * cartesianCoordinate[1]
- + di[i] * cartesianCoordinate[2];
+ + ci[i] * cartesianCoordinate[1]
+ + di[i] * cartesianCoordinate[2];
}
result /= 6.0 * volume;
diff --git a/SurgSim/Physics/Fem3DElementTetrahedron.h b/SurgSim/Physics/Fem3DElementTetrahedron.h
index f7fe767..ae9a0e4 100644
--- a/SurgSim/Physics/Fem3DElementTetrahedron.h
+++ b/SurgSim/Physics/Fem3DElementTetrahedron.h
@@ -20,6 +20,7 @@
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem.h"
#include "SurgSim/Physics/FemElement.h"
namespace SurgSim
@@ -27,6 +28,7 @@ namespace SurgSim
namespace Physics
{
+SURGSIM_STATIC_REGISTRATION(Fem3DElementTetrahedron);
/// Class for Fem Element 3D based on a tetrahedron volume discretization
/// \note The inertia property (mass) of the tetrahedron is derived from
@@ -35,111 +37,48 @@ namespace Physics
/// \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.
+/// \note Fem3DElementTetrahedron uses linear elasticity (not visco-elasticity), so it does not give any damping.
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
+ Fem3DElementTetrahedron();
+
+ /// Constructor
+ /// \param nodeIds An array of 4 node 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
+ /// Constructor for FemElement object factory
+ /// \param elementData A FemElement3D struct 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 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;
+ /// \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.
+ /// \exception SurgSim::Framework::AssertionFailure if nodeIds has a size different than 4
+ explicit Fem3DElementTetrahedron(std::shared_ptr<FemElementStructs::FemElementParameter> elementData);
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::Fem3DElementTetrahedron)
+
+ void initialize(const SurgSim::Math::OdeState& state) override;
+
+ double getVolume(const SurgSim::Math::OdeState& state) const override;
+
+ SurgSim::Math::Vector computeCartesianCoordinate(const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& naturalCoordinate) const override;
+
+ SurgSim::Math::Vector computeNaturalCoordinate(const SurgSim::Math::OdeState& state,
+ const SurgSim::Math::Vector& cartesianCoordinate) const override;
protected:
+ /// Initializes variables needed before Initialize() is called
+ void initializeMembers();
+
+ void doUpdateFMDK(const Math::OdeState& state, int options) override;
+
/// 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
@@ -148,35 +87,23 @@ protected:
/// \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;
+ 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);
+ SurgSim::Math::Matrix* 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);
+ SurgSim::Math::Matrix* m);
/// Shape functions: Tetrahedron rest volume
double m_restVolume;
@@ -192,11 +119,6 @@ protected:
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
diff --git a/SurgSim/Physics/Fem3DLocalization.cpp b/SurgSim/Physics/Fem3DLocalization.cpp
new file mode 100644
index 0000000..bad6840
--- /dev/null
+++ b/SurgSim/Physics/Fem3DLocalization.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/Fem3DLocalization.h"
+
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/FemElement.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+Fem3DLocalization::Fem3DLocalization(
+ std::shared_ptr<Representation> representation,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition) :
+ FemLocalization(representation, localPosition)
+{
+}
+
+Fem3DLocalization::~Fem3DLocalization()
+{
+}
+
+bool Fem3DLocalization::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/Fem3DLocalization.h b/SurgSim/Physics/Fem3DLocalization.h
new file mode 100644
index 0000000..855baa8
--- /dev/null
+++ b/SurgSim/Physics/Fem3DLocalization.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_FEM3DLOCALIZATION_H
+#define SURGSIM_PHYSICS_FEM3DLOCALIZATION_H
+
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/Physics/FemLocalization.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Implementation of Localization for Fem3DRepresentation
+///
+/// Fem3DLocalization tracks the global coordinates of an IndexedLocalCoordinate associated with an
+/// Fem3DRepresentation. The IndexedLocalCoordinate must be related to an FemElement (the index is an FemElement id and
+/// the local coordinates are the barycentric coordinates of the nodes in this FemElement).
+/// 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 Fem3DLocalization : public FemLocalization
+{
+public:
+ /// Constructor
+ /// \param representation The representation to assign to this localization.
+ /// \param localPosition The local position to set the localization at.
+ Fem3DLocalization(std::shared_ptr<Representation> representation,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition);
+
+ /// Destructor
+ virtual ~Fem3DLocalization();
+
+ /// Query if 'representation' is valid representation.
+ /// \param representation The representation.
+ /// \return true if valid representation, false if not.
+ bool isValidRepresentation(std::shared_ptr<Representation> representation) override;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEM3DLOCALIZATION_H
diff --git a/SurgSim/Physics/Fem3DPlyReaderDelegate.cpp b/SurgSim/Physics/Fem3DPlyReaderDelegate.cpp
index f249c6e..3b33d2c 100644
--- a/SurgSim/Physics/Fem3DPlyReaderDelegate.cpp
+++ b/SurgSim/Physics/Fem3DPlyReaderDelegate.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2014, SimQuest Solutions Inc.
+// Copyright 2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -13,13 +13,8 @@
// 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/Math/Valid.h"
#include "SurgSim/Physics/Fem3DPlyReaderDelegate.h"
-#include "SurgSim/Physics/Fem3DRepresentation.h"
using SurgSim::DataStructures::PlyReader;
@@ -28,9 +23,15 @@ namespace SurgSim
namespace Physics
{
-Fem3DPlyReaderDelegate::Fem3DPlyReaderDelegate(std::shared_ptr<Fem3DRepresentation> fem)
- : FemPlyReaderDelegate(fem)
+Fem3DPlyReaderDelegate::Fem3DPlyReaderDelegate()
+{
+}
+
+Fem3DPlyReaderDelegate::Fem3DPlyReaderDelegate(std::shared_ptr<Fem3D> mesh) :
+ m_mesh(mesh)
{
+ SURGSIM_ASSERT(mesh != nullptr) << "The mesh cannot be null.";
+ mesh->clear();
}
std::string Fem3DPlyReaderDelegate::getElementName() const
@@ -38,24 +39,62 @@ std::string Fem3DPlyReaderDelegate::getElementName() const
return "3d_element";
}
-void Fem3DPlyReaderDelegate::processFemElement(const std::string& elementName)
+bool Fem3DPlyReaderDelegate::fileIsAcceptable(const PlyReader& reader)
{
- SURGSIM_ASSERT(4 == m_femData.vertexCount || 8 == m_femData.vertexCount) <<
- "Cannot process 3D element with " << m_femData.vertexCount << " vertices.";
+ bool result = FemPlyReaderDelegate::fileIsAcceptable(reader);
+
+ SURGSIM_ASSERT(!m_hasRotationDOF)
+ << "Fem3DPlyReaderDelegate does not support rotational DOF, data will be ignored.";
- if (4 == m_femData.vertexCount)
+ return result;
+}
+
+void Fem3DPlyReaderDelegate::endParseFile()
+{
+ if (!m_hasPerElementMaterial && m_hasMaterial)
{
- 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));
+ for (auto element : m_mesh->getElements())
+ {
+ element->massDensity = m_materialData.massDensity;
+ element->poissonRatio = m_materialData.poissonRatio;
+ element->youngModulus = m_materialData.youngModulus;
+ }
}
- else
+
+ m_mesh->update();
+}
+
+void Fem3DPlyReaderDelegate::processVertex(const std::string& elementName)
+{
+ Fem3D::VertexType vertex(SurgSim::Math::Vector3d(m_vertexData.x, m_vertexData.y, m_vertexData.z));
+
+ m_mesh->addVertex(vertex);
+}
+
+void Fem3DPlyReaderDelegate::processFemElement(const std::string& elementName)
+{
+ SURGSIM_ASSERT(m_elementData.vertexCount == 4 || m_elementData.vertexCount == 8) <<
+ "Cannot process 3D Element with " << m_elementData.vertexCount << " vertices.";
+
+ auto femElement = std::make_shared<FemElementStructs::FemElement3DParameter>();
+ femElement->nodeIds.resize(m_elementData.vertexCount);
+ std::copy(m_elementData.indices, m_elementData.indices + m_elementData.vertexCount, femElement->nodeIds.data());
+
+ if (m_hasPerElementMaterial)
{
- 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));
+ femElement->massDensity = m_elementData.massDensity;
+ femElement->poissonRatio = m_elementData.poissonRatio;
+ femElement->youngModulus = m_elementData.youngModulus;
}
+
+ m_mesh->addElement(femElement);
+}
+
+void Fem3DPlyReaderDelegate::processBoundaryCondition(const std::string& elementName)
+{
+ m_mesh->addBoundaryCondition(static_cast<size_t>(m_boundaryConditionData));
}
-}; // namespace Physics
-}; // namespace SurgSim
\ No newline at end of file
+
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Fem3DPlyReaderDelegate.h b/SurgSim/Physics/Fem3DPlyReaderDelegate.h
index 4e3ceb4..ef1bb91 100644
--- a/SurgSim/Physics/Fem3DPlyReaderDelegate.h
+++ b/SurgSim/Physics/Fem3DPlyReaderDelegate.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2014, SimQuest Solutions Inc.
+// Copyright 2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,31 +16,51 @@
#ifndef SURGSIM_PHYSICS_FEM3DPLYREADERDELEGATE_H
#define SURGSIM_PHYSICS_FEM3DPLYREADERDELEGATE_H
+#include <array>
#include <memory>
+#include "SurgSim/DataStructures/EmptyData.h"
+#include "SurgSim/DataStructures/PlyReader.h"
+#include "SurgSim/Physics/Fem3D.h"
#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);
+ /// Default constructor.
+ Fem3DPlyReaderDelegate();
+
+ /// Constructor.
+ /// \param mesh The mesh to be used, it will be cleared by the constructor.
+ explicit Fem3DPlyReaderDelegate(std::shared_ptr<Fem3D> mesh);
protected:
- virtual std::string getElementName() const override;
+ std::string getElementName() const override;
+
+ bool fileIsAcceptable(const PlyReader& reader) override;
+
+ void endParseFile() override;
+
+ void processVertex(const std::string& elementName) override;
+
+ void processFemElement(const std::string& elementName) override;
+
+ void processBoundaryCondition(const std::string& elementName) override;
+
+ /// End file callback
+ void endFile();
- virtual void processFemElement(const std::string& elementName) override;
+private:
+ /// Fem3D mesh asset to contain the ply file information
+ std::shared_ptr<Fem3D> m_mesh;
};
} // namespace Physics
} // namespace SurgSim
-#endif // SURGSIM_PHYSICS_FEM3DPLYREADERDELEGATE_H
\ No newline at end of file
+#endif // SURGSIM_PHYSICS_FEM3DPLYREADERDELEGATE_H
diff --git a/SurgSim/Physics/Fem3DRepresentation.cpp b/SurgSim/Physics/Fem3DRepresentation.cpp
index 9987cfe..cc5d3f6 100644
--- a/SurgSim/Physics/Fem3DRepresentation.cpp
+++ b/SurgSim/Physics/Fem3DRepresentation.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -13,46 +13,36 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
#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/Framework/Assert.h"
+#include "SurgSim/Framework/Asset.h"
+#include "SurgSim/Math/MeshShape.h"
#include "SurgSim/Math/OdeState.h"
-#include "SurgSim/Math/Valid.h"
+#include "SurgSim/Math/SparseMatrix.h"
#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
-#include "SurgSim/Physics/Fem3DPlyReaderDelegate.h"
+#include "SurgSim/Physics/Fem3DElementCube.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
+#include "SurgSim/Physics/Fem3DLocalization.h"
#include "SurgSim/Physics/Fem3DRepresentation.h"
-#include "SurgSim/Physics/Fem3DRepresentationLocalization.h"
#include "SurgSim/Physics/FemElement.h"
-
-using SurgSim::Framework::Logger;
+#include "SurgSim/Physics/Localization.h"
namespace
{
-void transformVectorByBlockOf3(const SurgSim::Math::RigidTransform3d& transform,
- SurgSim::Math::Vector* x, bool rotationOnly = false)
+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)";
+ 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);
+ SurgSim::Math::Vector3d xi = x->segment<3>(3 * nodeId);
+ x->segment<3>(3 * nodeId) = (rotationOnly) ? transform.linear() * xi : transform * xi;
}
}
}
@@ -66,44 +56,108 @@ SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Physics::Fem3DRepresent
Fem3DRepresentation::Fem3DRepresentation(const std::string& name) :
FemRepresentation(name)
{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(Fem3DRepresentation, std::shared_ptr<SurgSim::Framework::Asset>, Fem, getFem,
+ setFem);
+ SURGSIM_ADD_SETTER(Fem3DRepresentation, std::string, FemFileName, loadFem)
// Reminder: m_numDofPerNode is held by DeformableRepresentation
// but needs to be set by all concrete derived classes
m_numDofPerNode = 3;
+ m_fem = std::make_shared<Fem3D>();
}
Fem3DRepresentation::~Fem3DRepresentation()
{
}
-RepresentationType Fem3DRepresentation::getType() const
+void Fem3DRepresentation::loadFem(const std::string& fileName)
{
- return REPRESENTATION_TYPE_FEM3D;
+ auto mesh = std::make_shared<Fem3D>();
+ mesh->load(fileName);
+ setFem(mesh);
+}
+
+void Fem3DRepresentation::setFem(std::shared_ptr<Framework::Asset> mesh)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "The Fem cannot be set after initialization";
+
+ SURGSIM_ASSERT(mesh != nullptr) << "Mesh for Fem3DRepresentation cannot be a nullptr";
+ auto femMesh = std::dynamic_pointer_cast<Fem3D>(mesh);
+ SURGSIM_ASSERT(femMesh != nullptr)
+ << "Mesh for Fem3DRepresentation needs to be a SurgSim::Physics::Fem3D";
+ m_fem = femMesh;
+ auto state = std::make_shared<Math::OdeState>();
+
+ state->setNumDof(getNumDofPerNode(), m_fem->getNumVertices());
+ for (size_t i = 0; i < m_fem->getNumVertices(); i++)
+ {
+ state->getPositions().segment<3>(getNumDofPerNode() * i) = m_fem->getVertexPosition(i);
+ }
+ for (auto boundaryCondition : m_fem->getBoundaryConditions())
+ {
+ state->addBoundaryCondition(boundaryCondition);
+ }
+
+ // If we have elements, ensure that they are all of the same nature
+ if (femMesh->getNumElements() > 0)
+ {
+ const auto& e0 = femMesh->getElement(0);
+ for (auto const& e : femMesh->getElements())
+ {
+ SURGSIM_ASSERT(e->nodeIds.size() == e0->nodeIds.size()) <<
+ "Cannot mix and match elements of different nature." <<
+ " Found an element with " << e->nodeIds.size() << " nodes but was expecting " << e0->nodeIds.size();
+ }
+
+ // If the FemElement types hasn't been registered yet, let's set a default one
+ if (getFemElementType().empty())
+ {
+ if (e0->nodeIds.size() == 4)
+ {
+ Fem3DElementTetrahedron tetrahdron;
+ setFemElementType(tetrahdron.getClassName());
+ }
+ else if (e0->nodeIds.size() == 8)
+ {
+ Fem3DElementCube cube;
+ setFemElementType(cube.getClassName());
+ }
+ }
+ }
+
+ setInitialState(state);
+}
+
+std::shared_ptr<Fem3D> Fem3DRepresentation::getFem() const
+{
+ return m_fem;
}
void Fem3DRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
- SurgSim::Math::Vector& generalizedForce,
- const SurgSim::Math::Matrix& K,
- const SurgSim::Math::Matrix& D)
+ const Math::Vector& generalizedForce,
+ const Math::Matrix& K,
+ const Math::Matrix& D)
{
+ using Math::SparseMatrix;
+
const size_t dofPerNode = getNumDofPerNode();
- const SurgSim::Math::Matrix::Index expectedSize = static_cast<const SurgSim::Math::Matrix::Index>(dofPerNode);
+ const Math::Matrix::Index expectedSize = static_cast<const 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;
+ "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;
+ "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;
+ "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)";
+ std::shared_ptr<Fem3DLocalization> localization3D =
+ std::dynamic_pointer_cast<Fem3DLocalization>(localization);
+ SURGSIM_ASSERT(localization3D != nullptr) << "Invalid localization type (not a Fem3DLocalization)";
const size_t elementId = localization3D->getLocalPosition().index;
- const SurgSim::Math::Vector& coordinate = localization3D->getLocalPosition().coordinate;
+ const Math::Vector& coordinate = localization3D->getLocalPosition().coordinate;
std::shared_ptr<FemElement> element = getFemElement(elementId);
size_t index = 0;
@@ -123,17 +177,17 @@ void Fem3DRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localizati
{
if (K.size() != 0)
{
- m_externalGeneralizedStiffness.block(dofPerNode * nodeId1,
- dofPerNode * nodeId2,
- dofPerNode, dofPerNode)
- += coordinate[index1] * coordinate[index2] * K;
+ Math::addSubMatrix(coordinate[index1] * coordinate[index2] * K,
+ static_cast<SparseMatrix::Index>(nodeId1),
+ static_cast<SparseMatrix::Index>(nodeId2),
+ &m_externalGeneralizedStiffness, true);
}
if (D.size() != 0)
{
- m_externalGeneralizedDamping.block(dofPerNode * nodeId1,
- dofPerNode * nodeId2,
- dofPerNode, dofPerNode)
- += coordinate[index1] * coordinate[index2] * D;
+ Math::addSubMatrix(coordinate[index1] * coordinate[index2] * D,
+ static_cast<SparseMatrix::Index>(nodeId1),
+ static_cast<SparseMatrix::Index>(nodeId2),
+ &m_externalGeneralizedDamping, true);
}
index2++;
}
@@ -141,18 +195,13 @@ void Fem3DRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localizati
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;
+ m_externalGeneralizedStiffness.makeCompressed();
+ m_externalGeneralizedDamping.makeCompressed();
+ m_hasExternalGeneralizedForce = true;
}
std::unordered_map<size_t, size_t> Fem3DRepresentation::createTriangleIdToElementIdMap(
- const SurgSim::DataStructures::TriangleMesh& mesh)
+ std::shared_ptr<const Math::MeshShape> mesh)
{
std::unordered_map<size_t, size_t> result;
@@ -180,7 +229,7 @@ std::unordered_map<size_t, size_t> Fem3DRepresentation::createTriangleIdToElemen
triangleSorted.begin(), triangleSorted.end());
};
- auto& meshTriangles = mesh.getTriangles();
+ auto& meshTriangles = mesh->getTriangles();
for (auto triangle = meshTriangles.cbegin(); triangle != meshTriangles.cend(); ++triangle)
{
if (! triangle->isValid)
@@ -192,7 +241,7 @@ std::unordered_map<size_t, size_t> Fem3DRepresentation::createTriangleIdToElemen
// 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);
+ 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())
@@ -215,38 +264,77 @@ bool Fem3DRepresentation::doWakeUp()
return false;
}
- auto deformableCollisionRepresentation
- = std::dynamic_pointer_cast<DeformableCollisionRepresentation>(m_collisionRepresentation);
-
- if (deformableCollisionRepresentation != nullptr)
+ auto deformableCollision = std::dynamic_pointer_cast<DeformableCollisionRepresentation>(m_collisionRepresentation);
+ if (deformableCollision != nullptr)
{
- m_triangleIdToElementIdMap = createTriangleIdToElementIdMap(*deformableCollisionRepresentation->getMesh());
+ auto mesh = std::dynamic_pointer_cast<Math::MeshShape>(deformableCollision->getShape());
+ m_triangleIdToElementIdMap = createTriangleIdToElementIdMap(mesh);
}
return true;
}
-std::shared_ptr<Localization> Fem3DRepresentation::createLocalization(const SurgSim::DataStructures::Location& location)
+bool Fem3DRepresentation::doInitialize()
+{
+ for (auto& element : m_fem->getElements())
+ {
+ std::shared_ptr<FemElement> femElement;
+ femElement = FemElement::getFactory().create(getFemElementType(), element);
+ m_femElements.push_back(femElement);
+ }
+
+ return FemRepresentation::doInitialize();
+}
+
+std::shared_ptr<Localization> Fem3DRepresentation::createNodeLocalization(size_t nodeId)
{
- SURGSIM_ASSERT(location.meshLocalCoordinate.hasValue())
- << "Localization cannot be created if the triangle ID is not available.";
+ DataStructures::IndexedLocalCoordinate coordinate;
+
+ SURGSIM_ASSERT(nodeId >= 0 && nodeId < getCurrentState()->getNumNodes()) << "Invalid node id";
+
+ // Look for any element that contains this node
+ bool foundNodeId = false;
+ for (size_t elementId = 0; elementId < getNumFemElements(); elementId++)
+ {
+ auto element = getFemElement(elementId);
+ auto found = std::find(element->getNodeIds().begin(), element->getNodeIds().end(), nodeId);
+ if (found != element->getNodeIds().end())
+ {
+ coordinate.index = elementId;
+ coordinate.coordinate.setZero(element->getNumNodes());
+ coordinate.coordinate[found - element->getNodeIds().begin()] = 1.0;
+ foundNodeId = true;
+ break;
+ }
+ }
+ SURGSIM_ASSERT(foundNodeId) << "Could not find any element containing the node " << nodeId;
- SURGSIM_ASSERT(location.meshLocalCoordinate.getValue().coordinate.size() == 3)
- << "Localization has incorrect size for the barycentric coordinates.";
+ // Fem3DLocalization will verify the coordinate (2nd parameter) based on
+ // the Fem3DRepresentation passed as 1st parameter.
+ return std::make_shared<Fem3DLocalization>(
+ std::static_pointer_cast<Physics::Representation>(getSharedPtr()), coordinate);
+}
- auto deformableCollisionRepresentation
- = std::dynamic_pointer_cast<DeformableCollisionRepresentation>(m_collisionRepresentation);
+std::shared_ptr<Localization> Fem3DRepresentation::createTriangleLocalization(
+ const DataStructures::IndexedLocalCoordinate& location)
+{
+ DataStructures::IndexedLocalCoordinate coordinate;
+ size_t triangleId = location.index;
+ const Math::Vector& triangleCoord = location.coordinate;
- SURGSIM_ASSERT(deformableCollisionRepresentation != nullptr)
- << "Localization cannot be created if the DeformableCollisionRepresentation is not correctly set.";
+ auto deformableCollision =
+ std::dynamic_pointer_cast<DeformableCollisionRepresentation>(m_collisionRepresentation);
+ SURGSIM_ASSERT(deformableCollision != nullptr)
+ << "Triangle 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;
+ auto mesh = std::dynamic_pointer_cast<Math::MeshShape>(deformableCollision->getShape());
+ auto triangleVertices = mesh->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.";
+ 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);
@@ -270,11 +358,7 @@ std::shared_ptr<Localization> Fem3DRepresentation::createLocalization(const Surg
}
// 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;
+ Math::Vector4d barycentricCoordinate(triangleCoord[0], triangleCoord[1], triangleCoord[2], 0.0);
coordinate.index = elementId;
coordinate.coordinate.resize(elementVertices.size());
for (size_t i = 0; i < elementVertices.size(); ++i)
@@ -282,16 +366,41 @@ std::shared_ptr<Localization> Fem3DRepresentation::createLocalization(const Surg
coordinate.coordinate[i] = barycentricCoordinate[indices[i]];
}
- // Fem3DRepresentationLocalization will verify the coordinate (2nd parameter) based on
+ // Fem3DLocalization 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 std::make_shared<Fem3DLocalization>(
+ std::static_pointer_cast<Physics::Representation>(getSharedPtr()), coordinate);
+}
- return result;
+std::shared_ptr<Localization> Fem3DRepresentation::createElementLocalization(
+ const DataStructures::IndexedLocalCoordinate& location)
+{
+ return std::make_shared<Fem3DLocalization>(
+ std::static_pointer_cast<Physics::Representation>(getSharedPtr()), location);
+}
+
+std::shared_ptr<Localization> Fem3DRepresentation::createLocalization(const DataStructures::Location& location)
+{
+ if (location.index.hasValue())
+ {
+ return createNodeLocalization(*location.index);
+ }
+ else if (location.triangleMeshLocalCoordinate.hasValue())
+ {
+ return createTriangleLocalization(*location.triangleMeshLocalCoordinate);
+ }
+ else if (location.elementMeshLocalCoordinate.hasValue())
+ {
+ return createElementLocalization(*location.elementMeshLocalCoordinate);
+ }
+
+ SURGSIM_FAILURE() << "Localization cannot be created without a mesh-based location (node, triangle or element).";
+
+ return nullptr;
}
-void Fem3DRepresentation::transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
- const SurgSim::Math::RigidTransform3d& transform)
+void Fem3DRepresentation::transformState(std::shared_ptr<Math::OdeState> state,
+ const Math::RigidTransform3d& transform)
{
transformVectorByBlockOf3(transform, &state->getPositions());
transformVectorByBlockOf3(transform, &state->getVelocities(), true);
diff --git a/SurgSim/Physics/Fem3DRepresentation.h b/SurgSim/Physics/Fem3DRepresentation.h
index c59350d..ebf8bc6 100644
--- a/SurgSim/Physics/Fem3DRepresentation.h
+++ b/SurgSim/Physics/Fem3DRepresentation.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -21,23 +21,34 @@
#include <unordered_map>
#include "SurgSim/Framework/FrameworkConvert.h"
-#include "SurgSim/Framework/ObjectFactory.h"
#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3D.h"
#include "SurgSim/Physics/FemRepresentation.h"
namespace SurgSim
{
-
namespace DataStructures
{
-class TriangleMesh;
+struct IndexedLocalCoordinate;
+struct Location;
+}
+namespace Framework
+{
+class Asset;
+}
+namespace Math
+{
+class MeshShape;
+class OdeState;
}
namespace Physics
{
-SURGSIM_STATIC_REGISTRATION(Fem3DRepresentation);
+class Localization;
-class FemPlyReaderDelegate;
+SURGSIM_STATIC_REGISTRATION(Fem3DRepresentation);
/// Finite Element Model 3D is a fem built with 3D FemElement
class Fem3DRepresentation : public FemRepresentation
@@ -52,38 +63,61 @@ public:
SURGSIM_CLASSNAME(SurgSim::Physics::Fem3DRepresentation);
- /// Query the representation type
- /// \return the RepresentationType for this representation
- virtual RepresentationType getType() const override;
+ void loadFem(const std::string& fileName) override;
+
+ /// Sets the fem mesh asset
+ /// \param mesh The fem mesh to assign to this representation
+ /// \exception SurgSim::Framework::AssertionFailure if mesh is nullptr or it's actual type is not Fem3D
+ void setFem(std::shared_ptr<SurgSim::Framework::Asset> mesh);
- 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;
+ /// \return The fem mesh asset as a Fem3D
+ std::shared_ptr<Fem3D> getFem() const;
- virtual std::shared_ptr<Localization> createLocalization(const SurgSim::DataStructures::Location&) override;
+ void addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ const SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K = SurgSim::Math::Matrix(),
+ const SurgSim::Math::Matrix& D = SurgSim::Math::Matrix()) override;
+
+ std::shared_ptr<Localization> createLocalization(const SurgSim::DataStructures::Location& location) override;
protected:
- virtual bool doWakeUp() override;
+ 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;
+ void transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
+ const SurgSim::Math::RigidTransform3d& transform) override;
-private:
- virtual std::shared_ptr<FemPlyReaderDelegate> getDelegate() override;
+ bool doInitialize() override;
+private:
/// 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);
+ std::shared_ptr<const SurgSim::Math::MeshShape> mesh);
+
+ /// Helper method: create a localization for a node
+ /// \param nodeId The node index
+ /// \return Localization of the node for this representation
+ std::shared_ptr<Localization> createNodeLocalization(size_t nodeId);
+
+ /// Helper method: create a localization for an triangle-based IndexedLocalCoordinate
+ /// \param location The IndexedLocalCoordinate defining a point on the triangle mesh
+ /// \return Localization of the point for this representation
+ std::shared_ptr<Localization> createTriangleLocalization(
+ const SurgSim::DataStructures::IndexedLocalCoordinate& location);
+
+ /// Helper method: create a localization for an element-based IndexedLocalCoordinate (tetrahedron or cube)
+ /// \param location The IndexedLocalCoordinate defining a point on the element mesh
+ /// \return Localization of the point for this representation
+ std::shared_ptr<Localization> createElementLocalization(
+ const SurgSim::DataStructures::IndexedLocalCoordinate& location);
/// Mapping from collision triangle's id to fem element id.
std::unordered_map<size_t, size_t> m_triangleIdToElementIdMap;
+
+ /// The Fem3DRepresentation's asset as a Fem3D
+ std::shared_ptr<Fem3D> m_fem;
};
} // namespace Physics
diff --git a/SurgSim/Physics/Fem3DRepresentationBilateral3D.cpp b/SurgSim/Physics/Fem3DRepresentationBilateral3D.cpp
deleted file mode 100644
index af64409..0000000
--- a/SurgSim/Physics/Fem3DRepresentationBilateral3D.cpp
+++ /dev/null
@@ -1,128 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index a4dfa27..0000000
--- a/SurgSim/Physics/Fem3DRepresentationBilateral3D.h
+++ /dev/null
@@ -1,74 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
deleted file mode 100644
index 5412c24..0000000
--- a/SurgSim/Physics/Fem3DRepresentationContact.cpp
+++ /dev/null
@@ -1,130 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 79e5c45..0000000
--- a/SurgSim/Physics/Fem3DRepresentationContact.h
+++ /dev/null
@@ -1,73 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
deleted file mode 100644
index 469c49d..0000000
--- a/SurgSim/Physics/Fem3DRepresentationLocalization.cpp
+++ /dev/null
@@ -1,98 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 5c26fc6..0000000
--- a/SurgSim/Physics/Fem3DRepresentationLocalization.h
+++ /dev/null
@@ -1,73 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/FemConstraintFixedPoint.cpp b/SurgSim/Physics/FemConstraintFixedPoint.cpp
new file mode 100644
index 0000000..bfe7cf6
--- /dev/null
+++ b/SurgSim/Physics/FemConstraintFixedPoint.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 "SurgSim/Physics/FemConstraintFixedPoint.h"
+#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/FemLocalization.h"
+#include "SurgSim/Physics/FemRepresentation.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+FemConstraintFixedPoint::FemConstraintFixedPoint()
+{
+}
+
+FemConstraintFixedPoint::~FemConstraintFixedPoint()
+{
+}
+
+void FemConstraintFixedPoint::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<FemRepresentation> fem
+ = std::static_pointer_cast<FemRepresentation>(localization->getRepresentation());
+ const size_t numDofPerNode = fem->getNumDofPerNode();
+
+ if (!fem->isActive())
+ {
+ return;
+ }
+
+ const double scale = (sign == CONSTRAINT_POSITIVE_SIDE) ? 1.0 : -1.0;
+ const SurgSim::DataStructures::IndexedLocalCoordinate& coord
+ = std::static_pointer_cast<FemLocalization>(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 RigidRepresentationFixedPoint 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(fem->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 = fem->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(numDofPerNode * nodeIndex + axis) = coord.coordinate[index] * (dt * scale);
+ }
+ }
+ mlcp->updateConstraint(m_newH, fem->getComplianceMatrix() * m_newH.transpose(),
+ indexOfRepresentation, indexOfConstraint + axis);
+ }
+}
+
+SurgSim::Physics::ConstraintType FemConstraintFixedPoint::getConstraintType() const
+{
+ return SurgSim::Physics::FIXED_3DPOINT;
+}
+
+size_t FemConstraintFixedPoint::doGetNumDof() const
+{
+ return 3;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/FemConstraintFixedPoint.h b/SurgSim/Physics/FemConstraintFixedPoint.h
new file mode 100644
index 0000000..e57f55f
--- /dev/null
+++ b/SurgSim/Physics/FemConstraintFixedPoint.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_FEMCONSTRAINTFIXEDPOINT_H
+#define SURGSIM_PHYSICS_FEMCONSTRAINTFIXEDPOINT_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Base class for all FemRepresentation fixed point constraint implementation.
+class FemConstraintFixedPoint : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ FemConstraintFixedPoint();
+
+ /// Destructor
+ virtual ~FemConstraintFixedPoint();
+
+ SurgSim::Physics::ConstraintType getConstraintType() const override;
+
+private:
+ size_t doGetNumDof() const override;
+
+ 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_FEMCONSTRAINTFIXEDPOINT_H
diff --git a/SurgSim/Physics/FemConstraintFixedRotationVector.cpp b/SurgSim/Physics/FemConstraintFixedRotationVector.cpp
new file mode 100644
index 0000000..7a493ad
--- /dev/null
+++ b/SurgSim/Physics/FemConstraintFixedRotationVector.cpp
@@ -0,0 +1,133 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/FemConstraintFixedRotationVector.h"
+#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/FemLocalization.h"
+#include "SurgSim/Physics/FemRepresentation.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+FemConstraintFixedRotationVector::FemConstraintFixedRotationVector()
+{
+}
+
+FemConstraintFixedRotationVector::~FemConstraintFixedRotationVector()
+{
+}
+
+void FemConstraintFixedRotationVector::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<FemRepresentation> fem
+ = std::static_pointer_cast<FemRepresentation>(localization->getRepresentation());
+ const size_t numDofPerNode = fem->getNumDofPerNode();
+
+ SURGSIM_ASSERT(numDofPerNode >= 6) << "The rotation vector constraint needs rotational dof (6DOF/node minimum)"
+ << " but " << fem->getFullName() << " only has " << numDofPerNode << " DOF";
+
+ if (!fem->isActive())
+ {
+ return;
+ }
+
+ const double scale = (sign == CONSTRAINT_POSITIVE_SIDE) ? 1.0 : -1.0;
+ const SurgSim::DataStructures::IndexedLocalCoordinate& coord
+ = std::static_pointer_cast<FemLocalization>(localization)->getLocalPosition();
+
+ auto nodeIds = fem->getFemElement(coord.index)->getNodeIds();
+ Vector3d currentRotationVector = Vector3d::Zero();
+ for (size_t i = 0; i < nodeIds.size(); i++)
+ {
+ currentRotationVector +=
+ fem->getCurrentState()->getPositions().segment<3>(6 * nodeIds[i] + 3) * coord.coordinate[i];
+ }
+
+ // Fixed rotation vector constraint in MCLP
+ // p(t) is defined as the rotation vector before motion
+ // s is defined as the rotation vector 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 RigidRepresentationFixedPoint for more implementation details.
+
+ // Update b with new violation: P(free motion)
+ mlcp->b.segment<3>(indexOfConstraint) += currentRotationVector * scale;
+
+ // m_newH is a SparseVector, so resizing is cheap. The object's memory also gets cleared.
+ m_newH.resize(fem->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 = fem->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(numDofPerNode * nodeIndex + axis + 3) = coord.coordinate[index] * (dt * scale);
+ }
+ }
+ mlcp->updateConstraint(m_newH, fem->getComplianceMatrix() * m_newH.transpose(),
+ indexOfRepresentation, indexOfConstraint + axis);
+ }
+}
+
+SurgSim::Physics::ConstraintType FemConstraintFixedRotationVector::getConstraintType() const
+{
+ return SurgSim::Physics::FIXED_3DROTATION_VECTOR;
+}
+
+size_t FemConstraintFixedRotationVector::doGetNumDof() const
+{
+ return 3;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/FemConstraintFixedRotationVector.h b/SurgSim/Physics/FemConstraintFixedRotationVector.h
new file mode 100644
index 0000000..b1d8fe4
--- /dev/null
+++ b/SurgSim/Physics/FemConstraintFixedRotationVector.h
@@ -0,0 +1,60 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_FEMCONSTRAINTFIXEDROTATIONVECTOR_H
+#define SURGSIM_PHYSICS_FEMCONSTRAINTFIXEDROTATIONVECTOR_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Base class for all FemRepresentation fixed rotation vector constraint implementation.
+class FemConstraintFixedRotationVector : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ FemConstraintFixedRotationVector();
+
+ /// Destructor
+ virtual ~FemConstraintFixedRotationVector();
+
+ SurgSim::Physics::ConstraintType getConstraintType() const override;
+
+private:
+ size_t doGetNumDof() const override;
+
+ /// \note The constraint violation being calculated based on a quaternion interpolation (slerp), and this
+ /// type of interpolation being highly non-linear, the classical way of using the implementation one after the
+ /// other one won't work.
+ /// Therefore, the RotationVectorConstraint will use the vector mlcp->b to retrieve both representation's
+ /// rotation vector, then calculate the proper slerp and set the violation back in mlcp->b
+ 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_FEMCONSTRAINTFIXEDROTATIONVECTOR_H
diff --git a/SurgSim/Physics/FemConstraintFrictionalSliding.cpp b/SurgSim/Physics/FemConstraintFrictionalSliding.cpp
new file mode 100644
index 0000000..82b6e85
--- /dev/null
+++ b/SurgSim/Physics/FemConstraintFrictionalSliding.cpp
@@ -0,0 +1,107 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/FemConstraintFrictionalSliding.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/SlidingConstraintData.h"
+#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/FemLocalization.h"
+#include "SurgSim/Physics/FemRepresentation.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+FemConstraintFrictionalSliding::FemConstraintFrictionalSliding()
+{
+}
+
+FemConstraintFrictionalSliding::~FemConstraintFrictionalSliding()
+{
+}
+
+void FemConstraintFrictionalSliding::doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign)
+{
+ auto fem = std::static_pointer_cast<FemRepresentation>(localization->getRepresentation());
+ if (!fem->isActive())
+ {
+ return;
+ }
+
+ const size_t numDofPerNode = fem->getNumDofPerNode();
+ const double scale = (sign == CONSTRAINT_POSITIVE_SIDE) ? 1.0 : -1.0;
+ const SlidingConstraintData& constraintData = static_cast<const SlidingConstraintData&>(data);
+
+ std::array<Math::Vector3d, 3> directions;
+ directions[0] = constraintData.getNormals()[0];
+ directions[1] = constraintData.getNormals()[1];
+ directions[2] = constraintData.getTangent();
+
+ const DataStructures::IndexedLocalCoordinate& coord
+ = std::static_pointer_cast<FemLocalization>(localization)->getLocalPosition();
+ Vector3d globalPosition = localization->calculatePosition();
+
+ m_newH.resize(fem->getNumDof());
+ auto femElement = fem->getFemElement(coord.index);
+ auto numNodes = fem->getFemElement(coord.index)->getNumNodes();
+ m_newH.reserve(numNodes * 3);
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ // Update b with new violation
+ double violation = directions[i].dot(globalPosition);
+ mlcp->b[indexOfConstraint + i] += violation * scale;
+
+ // Fill the new H.
+ m_newH.setZero();
+ for (size_t j = 0; j < numNodes; ++j)
+ {
+ auto nodeId = femElement->getNodeId(j);
+ m_newH.insert(numDofPerNode * nodeId + 0) = coord.coordinate[j] * directions[i][0] * scale * dt;
+ m_newH.insert(numDofPerNode * nodeId + 1) = coord.coordinate[j] * directions[i][1] * scale * dt;
+ m_newH.insert(numDofPerNode * nodeId + 2) = coord.coordinate[j] * directions[i][2] * scale * dt;
+ }
+
+ mlcp->updateConstraint(m_newH, fem->getComplianceMatrix() * m_newH.transpose(), indexOfRepresentation,
+ indexOfConstraint + i);
+ }
+
+ mlcp->mu[indexOfConstraint] = constraintData.getFrictionCoefficient();
+}
+
+SurgSim::Physics::ConstraintType FemConstraintFrictionalSliding::getConstraintType() const
+{
+ return SurgSim::Physics::FRICTIONAL_SLIDING;
+}
+
+size_t FemConstraintFrictionalSliding::doGetNumDof() const
+{
+ return 3;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/FemConstraintFrictionalSliding.h b/SurgSim/Physics/FemConstraintFrictionalSliding.h
new file mode 100644
index 0000000..53fe9c3
--- /dev/null
+++ b/SurgSim/Physics/FemConstraintFrictionalSliding.h
@@ -0,0 +1,55 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_FEMCONSTRAINTFRICTIONALSLIDING_H
+#define SURGSIM_PHYSICS_FEMCONSTRAINTFRICTIONALSLIDING_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Base class for all FemRepresentation frictional sliding constraint implementation.
+class FemConstraintFrictionalSliding: public ConstraintImplementation
+{
+public:
+ /// Constructor
+ FemConstraintFrictionalSliding();
+
+ /// Destructor
+ virtual ~FemConstraintFrictionalSliding();
+
+ SurgSim::Physics::ConstraintType getConstraintType() const override;
+
+private:
+ size_t doGetNumDof() const override;
+
+ 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_FEMCONSTRAINTFRICTIONALSLIDING_H
diff --git a/SurgSim/Physics/FemConstraintFrictionlessContact.cpp b/SurgSim/Physics/FemConstraintFrictionlessContact.cpp
new file mode 100644
index 0000000..c985692
--- /dev/null
+++ b/SurgSim/Physics/FemConstraintFrictionlessContact.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 "SurgSim/Physics/FemConstraintFrictionlessContact.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/SegmentMeshShape.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/FemLocalization.h"
+#include "SurgSim/Physics/FemRepresentation.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+FemConstraintFrictionlessContact::FemConstraintFrictionlessContact(): m_mlcpNumericalPrecision(1.0e-04)
+{
+}
+
+FemConstraintFrictionlessContact::~FemConstraintFrictionlessContact()
+{
+}
+
+void FemConstraintFrictionlessContact::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<FemRepresentation> fem
+ = std::static_pointer_cast<FemRepresentation>(localization->getRepresentation());
+ const size_t numDofPerNode = fem->getNumDofPerNode();
+
+ if (!fem->isActive())
+ {
+ return;
+ }
+
+ const double scale = (sign == CONSTRAINT_POSITIVE_SIDE) ? 1.0 : -1.0;
+ const SurgSim::Math::Vector3d& n = static_cast<const ContactConstraintData&>(data).getNormal();
+ const SurgSim::DataStructures::IndexedLocalCoordinate& coord
+ = std::static_pointer_cast<FemLocalization>(localization)->getLocalPosition();
+ Vector3d globalPosition = localization->calculatePosition();
+
+ double radius = 0.0;
+ {
+ auto rep = std::dynamic_pointer_cast<Collision::Representation>(fem->getCollisionRepresentation());
+ if (rep != nullptr)
+ {
+ auto segmentShape =
+ std::dynamic_pointer_cast<Math::SegmentMeshShape>(rep->getShape());
+ if (segmentShape != nullptr)
+ {
+ radius = segmentShape->getRadius();
+ }
+ }
+ }
+
+ // Update b with new violation
+ double violation = n.dot(globalPosition);
+ mlcp->b[indexOfConstraint] += violation * scale - radius - m_mlcpNumericalPrecision;
+
+ // m_newH is a SparseVector, so resizing is cheap. The object's memory also gets cleared.
+ m_newH.resize(fem->getNumDof());
+ // m_newH is a member variable, so 'reserve' only needs to allocate memory on the first run.
+ std::shared_ptr<FemElement> femElement = fem->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(numDofPerNode * nodeIndex + 0) = coord.coordinate[index] * n[0] * scale * dt;
+ m_newH.insert(numDofPerNode * nodeIndex + 1) = coord.coordinate[index] * n[1] * scale * dt;
+ m_newH.insert(numDofPerNode * nodeIndex + 2) = coord.coordinate[index] * n[2] * scale * dt;
+ }
+ }
+
+ mlcp->updateConstraint(m_newH, fem->getComplianceMatrix() * m_newH.transpose(), indexOfRepresentation,
+ indexOfConstraint);
+}
+
+SurgSim::Physics::ConstraintType FemConstraintFrictionlessContact::getConstraintType() const
+{
+ return SurgSim::Physics::FRICTIONLESS_3DCONTACT;
+}
+
+size_t FemConstraintFrictionlessContact::doGetNumDof() const
+{
+ return 1;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/FemConstraintFrictionlessContact.h b/SurgSim/Physics/FemConstraintFrictionlessContact.h
new file mode 100644
index 0000000..412563a
--- /dev/null
+++ b/SurgSim/Physics/FemConstraintFrictionlessContact.h
@@ -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.
+
+#ifndef SURGSIM_PHYSICS_FEMCONSTRAINTFRICTIONLESSCONTACT_H
+#define SURGSIM_PHYSICS_FEMCONSTRAINTFRICTIONLESSCONTACT_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Base class for all FemRepresentation frictionless contact constraint implementation.
+class FemConstraintFrictionlessContact: public ConstraintImplementation
+{
+public:
+
+ /// Constructor
+ FemConstraintFrictionlessContact();
+
+ /// Destructor
+ virtual ~FemConstraintFrictionlessContact();
+
+ SurgSim::Physics::ConstraintType getConstraintType() const override;
+
+private:
+ size_t doGetNumDof() const override;
+
+ void doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign) override;
+
+ /// \note accounts for the mlcp precision, so that the contact point
+ /// is not floating around the solution plane (with a precision of +-e due to the
+ /// mlcp, but floating above this plane, so the contact is actually verified at the end)
+ double m_mlcpNumericalPrecision;
+
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEMCONSTRAINTFRICTIONLESSCONTACT_H
diff --git a/SurgSim/Physics/FemConstraintFrictionlessSliding.cpp b/SurgSim/Physics/FemConstraintFrictionlessSliding.cpp
new file mode 100644
index 0000000..c677ff6
--- /dev/null
+++ b/SurgSim/Physics/FemConstraintFrictionlessSliding.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.
+
+#include "SurgSim/Physics/FemConstraintFrictionlessSliding.h"
+
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/SlidingConstraintData.h"
+#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/FemLocalization.h"
+#include "SurgSim/Physics/FemRepresentation.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+FemConstraintFrictionlessSliding::FemConstraintFrictionlessSliding()
+{
+}
+
+FemConstraintFrictionlessSliding::~FemConstraintFrictionlessSliding()
+{
+}
+
+void FemConstraintFrictionlessSliding::doBuild(double dt,
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign)
+{
+ auto fem = std::static_pointer_cast<FemRepresentation>(localization->getRepresentation());
+ if (!fem->isActive())
+ {
+ return;
+ }
+
+ const size_t numDofPerNode = fem->getNumDofPerNode();
+ const double scale = (sign == CONSTRAINT_POSITIVE_SIDE) ? 1.0 : -1.0;
+ const SlidingConstraintData& constraintData = static_cast<const SlidingConstraintData&>(data);
+ const auto normals = constraintData.getNormals();
+
+ const DataStructures::IndexedLocalCoordinate& coord
+ = std::static_pointer_cast<FemLocalization>(localization)->getLocalPosition();
+ Vector3d globalPosition = localization->calculatePosition();
+
+ m_newH.resize(fem->getNumDof());
+ auto femElement = fem->getFemElement(coord.index);
+ auto numNodes = fem->getFemElement(coord.index)->getNumNodes();
+ m_newH.reserve(numNodes * 3);
+
+ for (size_t i = 0; i < 2; ++i)
+ {
+ // Update b with new violation
+ double violation = normals[i].dot(globalPosition);
+ mlcp->b[indexOfConstraint + i] += violation * scale;
+
+ // Fill the new H.
+ m_newH.setZero();
+ for (size_t j = 0; j < numNodes; ++j)
+ {
+ auto nodeId = femElement->getNodeId(j);
+ m_newH.insert(numDofPerNode * nodeId + 0) = coord.coordinate[j] * normals[i][0] * scale * dt;
+ m_newH.insert(numDofPerNode * nodeId + 1) = coord.coordinate[j] * normals[i][1] * scale * dt;
+ m_newH.insert(numDofPerNode * nodeId + 2) = coord.coordinate[j] * normals[i][2] * scale * dt;
+ }
+
+ mlcp->updateConstraint(m_newH, fem->getComplianceMatrix() * m_newH.transpose(), indexOfRepresentation,
+ indexOfConstraint + i);
+ }
+}
+
+SurgSim::Physics::ConstraintType FemConstraintFrictionlessSliding::getConstraintType() const
+{
+ return SurgSim::Physics::FRICTIONLESS_SLIDING;
+}
+
+size_t FemConstraintFrictionlessSliding::doGetNumDof() const
+{
+ return 2;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/FemConstraintFrictionlessSliding.h b/SurgSim/Physics/FemConstraintFrictionlessSliding.h
new file mode 100644
index 0000000..543dd1d
--- /dev/null
+++ b/SurgSim/Physics/FemConstraintFrictionlessSliding.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_FEMCONSTRAINTFRICTIONLESSSLIDING_H
+#define SURGSIM_PHYSICS_FEMCONSTRAINTFRICTIONLESSSLIDING_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Base class for all FemRepresentation frictionless sliding constraint implementation.
+class FemConstraintFrictionlessSliding: public ConstraintImplementation
+{
+public:
+ /// Constructor
+ FemConstraintFrictionlessSliding();
+
+ /// Destructor
+ virtual ~FemConstraintFrictionlessSliding();
+
+ SurgSim::Physics::ConstraintType getConstraintType() const override;
+
+private:
+ size_t doGetNumDof() const override;
+
+ 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_FEMCONSTRAINTFRICTIONLESSSLIDING_H
diff --git a/SurgSim/Physics/FemElement-inl.h b/SurgSim/Physics/FemElement-inl.h
new file mode 100644
index 0000000..a0e6a3c
--- /dev/null
+++ b/SurgSim/Physics/FemElement-inl.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_FEMELEMENT_INL_H
+#define SURGSIM_PHYSICS_FEMELEMENT_INL_H
+
+#include <vector>
+
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/SparseMatrix.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+template <typename DerivedSub, typename T, int Opt, typename Index>
+void FemElement::assembleMatrixBlocks(const DerivedSub& subMatrix, const std::vector<size_t> blockIds,
+ size_t blockSize, Eigen::SparseMatrix<T, Opt, Index>* matrix,
+ bool initialize) const
+{
+ using SurgSim::Math::addSubMatrix;
+
+ const Index numBlocks = static_cast<Index>(blockIds.size());
+ for (Index block0 = 0; block0 < numBlocks; block0++)
+ {
+ Index subRow = static_cast<Index>(blockSize * block0);
+ for (Index block1 = 0; block1 < numBlocks; block1++)
+ {
+ Index subCol = static_cast<Index>(blockSize * block1);
+ addSubMatrix(subMatrix.block(subRow, subCol, blockSize, blockSize),
+ static_cast<Index>(blockIds[block0]),
+ static_cast<Index>(blockIds[block1]),
+ matrix, initialize);
+ }
+ }
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEMELEMENT_INL_H
diff --git a/SurgSim/Physics/FemElement.cpp b/SurgSim/Physics/FemElement.cpp
index 408d89d..278c7d6 100644
--- a/SurgSim/Physics/FemElement.cpp
+++ b/SurgSim/Physics/FemElement.cpp
@@ -24,7 +24,8 @@ namespace SurgSim
namespace Physics
{
-FemElement::FemElement() : m_numDofPerNode(0), m_rho(0.0), m_E(0.0), m_nu(0.0)
+FemElement::FemElement() : m_numDofPerNode(0), m_rho(0.0), m_E(0.0), m_nu(0.0),
+ m_useDamping(false), m_initializedFMDK(false)
{}
FemElement::~FemElement()
@@ -32,6 +33,7 @@ FemElement::~FemElement()
void FemElement::initialize(const SurgSim::Math::OdeState& state)
{
+ initializeFMDK();
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() ?";
@@ -40,6 +42,12 @@ void FemElement::initialize(const SurgSim::Math::OdeState& state)
SURGSIM_ASSERT(m_E > 0.0) << "Young modulus ("<<m_E<<") is invalid, it should be positive";
}
+FemElement::FactoryType& FemElement::getFactory()
+{
+ static FactoryType factory;
+ return factory;
+}
+
size_t FemElement::getNumDofPerNode() const
{
return m_numDofPerNode;
@@ -100,9 +108,78 @@ double FemElement::getMass(const SurgSim::Math::OdeState& state) const
return getVolume(state) * m_rho;
}
-bool FemElement::update(const SurgSim::Math::OdeState& state)
+void FemElement::addForce(SurgSim::Math::Vector* F, double scale) const
+{
+ Math::addSubVector(m_f * scale, m_nodeIds, m_numDofPerNode, F);
+}
+
+void FemElement::addMass(SurgSim::Math::SparseMatrix* M, double scale) const
+{
+ assembleMatrixBlocks(m_M * scale, m_nodeIds, static_cast<int>(m_numDofPerNode), M, false);
+}
+
+void FemElement::addDamping(SurgSim::Math::SparseMatrix* D, double scale) const
+{
+ if (m_useDamping)
+ {
+ assembleMatrixBlocks(m_D * scale, m_nodeIds, static_cast<int>(m_numDofPerNode), D, false);
+ }
+}
+
+void FemElement::addStiffness(SurgSim::Math::SparseMatrix* K, double scale) const
+{
+ assembleMatrixBlocks(m_K * scale, m_nodeIds, static_cast<int>(m_numDofPerNode), K, false);
+}
+
+void FemElement::addFMDK(SurgSim::Math::Vector* F,
+ SurgSim::Math::SparseMatrix* M,
+ SurgSim::Math::SparseMatrix* D,
+ SurgSim::Math::SparseMatrix* K) const
+{
+ addForce(F);
+ addMass(M);
+ addDamping(D);
+ addStiffness(K);
+}
+
+void FemElement::addMatVec(double alphaM, double alphaD, double alphaK, const SurgSim::Math::Vector& x,
+ SurgSim::Math::Vector* F) const
{
- return true;
+ using Math::addSubVector;
+ using Math::getSubVector;
+
+ if (alphaM == 0.0 && (!m_useDamping || alphaD == 0.0) && alphaK == 0.0)
+ {
+ return;
+ }
+
+ size_t size = getNumNodes() * getNumDofPerNode();
+ Math::Vector extractedX(size);
+ getSubVector(x, m_nodeIds, getNumDofPerNode(), &extractedX);
+
+ // Accumulate the mass/damping/stiffness contribution
+ Math::Vector extractedResult;
+ extractedResult.setZero(size);
+
+ // Adds the mass contribution
+ if (alphaM != 0.0)
+ {
+ extractedResult += alphaM * (m_M * extractedX);
+ }
+
+ // Adds the damping contribution
+ if (m_useDamping && alphaD != 0.0)
+ {
+ extractedResult += alphaD * (m_D * extractedX);
+ }
+
+ // Adds the stiffness contribution
+ if (alphaK != 0.0)
+ {
+ extractedResult += alphaK * (m_K * extractedX);
+ }
+
+ addSubVector(extractedResult, m_nodeIds, m_numDofPerNode, F);
}
bool FemElement::isValidCoordinate(const SurgSim::Math::Vector& naturalCoordinate) const
@@ -114,6 +191,32 @@ bool FemElement::isValidCoordinate(const SurgSim::Math::Vector& naturalCoordinat
naturalCoordinate.maxCoeff() <= 1.0 + SurgSim::Math::Geometry::ScalarEpsilon);
}
+void FemElement::updateFMDK(const Math::OdeState& state, int options)
+{
+ doUpdateFMDK(state, options);
+}
+
+void FemElement::initializeFMDK()
+{
+ if (!m_initializedFMDK)
+ {
+ m_initializedFMDK = true;
+ doInitializeFMDK();
+ }
+}
+
+void FemElement::doInitializeFMDK()
+{
+ size_t size = getNumNodes() * getNumDofPerNode();
+ m_f.setZero(size);
+ m_M.setZero(size, size);
+ if (m_useDamping)
+ {
+ m_D.setZero(size, size);
+ }
+ m_K.setZero(size, size);
+}
+
} // namespace Physics
} // namespace SurgSim
diff --git a/SurgSim/Physics/FemElement.h b/SurgSim/Physics/FemElement.h
index a19f835..84b91a5 100644
--- a/SurgSim/Physics/FemElement.h
+++ b/SurgSim/Physics/FemElement.h
@@ -18,8 +18,11 @@
#include <vector>
+#include "SurgSim/Framework/ObjectFactory.h"
#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/SparseMatrix.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem.h"
namespace SurgSim
{
@@ -52,6 +55,11 @@ public:
/// \param state The state to initialize the FemElement with
virtual void initialize(const SurgSim::Math::OdeState& state);
+ typedef SurgSim::Framework::ObjectFactory1<FemElement, std::shared_ptr<FemElementStructs::FemElementParameter>>
+ FactoryType;
+
+ static FactoryType& getFactory();
+
/// Gets the number of degree of freedom per node
/// \return The number of dof per node
size_t getNumDofPerNode() const;
@@ -100,63 +108,54 @@ public:
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;
+ virtual void addForce(SurgSim::Math::Vector* F, double scale = 1.0) const;
/// 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;
+ virtual void addMass(SurgSim::Math::SparseMatrix* M, double scale = 1.0) const;
/// 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;
+ virtual void addDamping(SurgSim::Math::SparseMatrix* D, double scale = 1.0) const;
/// 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;
+ virtual void addStiffness(SurgSim::Math::SparseMatrix* K, double scale = 1.0) const;
/// 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;
+ virtual void addFMDK(SurgSim::Math::Vector* F,
+ SurgSim::Math::SparseMatrix* M,
+ SurgSim::Math::SparseMatrix* D,
+ SurgSim::Math::SparseMatrix* K) const;
/// 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
@@ -164,14 +163,8 @@ public:
/// \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);
+ virtual void addMatVec(double alphaM, double alphaD, double alphaK,
+ const SurgSim::Math::Vector& x, SurgSim::Math::Vector* F) const;
/// Determines whether a given natural coordinate is valid
/// \param naturalCoordinate Coordinate to check
@@ -194,6 +187,28 @@ public:
const SurgSim::Math::OdeState& state,
const SurgSim::Math::Vector& cartesianCoordinate) const = 0;
+ /// Helper method to add a sub-matrix made of squared-blocks into a matrix, for the sake of clarity
+ /// \tparam DerivedSub The type of the 'subMatrix' (can usually be inferred). Can be any type, but does not
+ /// support Eigen expression. If it is a Sparse storage type the alignment must be the same
+ /// as the SparseMatrix: Opt.
+ /// Note that no assertion or verification is done on this type.
+ /// \tparam T, Opt, Index Types and option defining the output matrix type SparseMatrix<T, Opt, Index>
+ /// \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
+ /// \param initialize Optional parameter, default true. If true, the matrix form is assumed to be undefined and is
+ /// initialized when necessary. If false, the matrix form is assumed to be previously defined.
+ template <typename DerivedSub, typename T, int Opt, typename Index>
+ void assembleMatrixBlocks(const DerivedSub& subMatrix, const std::vector<size_t> blockIds,
+ size_t blockSize, Eigen::SparseMatrix<T, Opt, Index>* matrix,
+ bool initialize = true) const;
+
+ /// Update the FemElement based on the given state.
+ /// \param state \f$(x, v)\f$ the current position and velocity to evaluate the various terms with
+ /// \param options Flag to specify which of the F, M, D, K needs to be updated
+ void updateFMDK(const Math::OdeState& state, int options);
+
protected:
/// Sets the number of degrees of freedom per node
/// \param numDofPerNode The number of dof per node
@@ -201,6 +216,17 @@ protected:
/// \note ones able to set this parameter
void setNumDofPerNode(size_t numDofPerNode);
+ /// Update the FemElement based on the given state.
+ /// \param state \f$(x, v)\f$ the current position and velocity to evaluate the various terms with
+ /// \param options Flag to specify which of the F, M, D, K needs to be updated
+ virtual void doUpdateFMDK(const Math::OdeState& state, int options) = 0;
+
+ /// Initialize f, M, D, K variables.
+ void initializeFMDK();
+
+ /// Function to be overridden by the derived classes to initialize the f, M, D, K variables.
+ virtual void doInitializeFMDK();
+
/// Number of degree of freedom per node for this element
size_t m_numDofPerNode;
@@ -215,10 +241,31 @@ protected:
/// Poisson ratio (unitless)
double m_nu;
+
+ /// The force vector.
+ SurgSim::Math::Vector m_f;
+
+ /// The mass matrix.
+ SurgSim::Math::Matrix m_M;
+
+ /// The damping matrix.
+ SurgSim::Math::Matrix m_D;
+
+ /// Flag to specify of the damping is used.
+ bool m_useDamping;
+
+ /// The stiffness matrix.
+ SurgSim::Math::Matrix m_K;
+
+private:
+ /// Flag to check in the f, M, D, K variables have been initialized.
+ bool m_initializedFMDK;
};
} // namespace Physics
} // namespace SurgSim
+#include "SurgSim/Physics/FemElement-inl.h"
+
#endif // SURGSIM_PHYSICS_FEMELEMENT_H
diff --git a/SurgSim/Physics/FemElementStructs.h b/SurgSim/Physics/FemElementStructs.h
new file mode 100644
index 0000000..b9d4844
--- /dev/null
+++ b/SurgSim/Physics/FemElementStructs.h
@@ -0,0 +1,89 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_FEMELEMENTSTRUCTS_H
+#define SURGSIM_PHYSICS_FEMELEMENTSTRUCTS_H
+
+namespace SurgSim
+{
+namespace Physics
+{
+namespace FemElementStructs
+{
+
+/// RotationVectorData is a structure containing the rotational dof per vertex
+/// The nature of the rotation depends on the underlying Representation,
+/// but right now all OSS's representations use rotation vector for rotational dof.
+struct RotationVectorData
+{
+ RotationVectorData() : thetaX(0.0), thetaY(0.0), thetaZ(0.0)
+ {
+ }
+
+ bool operator==(const RotationVectorData& rhs) const
+ {
+ return (thetaX == rhs.thetaX && thetaY == rhs.thetaY && thetaZ == rhs.thetaZ);
+ }
+
+ double thetaX;
+ double thetaY;
+ double thetaZ;
+};
+
+/// FemElementParameter is a structure containing the parameters of an fem element following
+/// the Hooke's law of deformation (linear deformation).
+struct FemElementParameter
+{
+ FemElementParameter() : youngModulus(0.0), poissonRatio(0.0), massDensity(0.0)
+ {
+ }
+
+ virtual ~FemElementParameter(){}
+
+ std::vector<size_t> nodeIds;
+ double youngModulus;
+ double poissonRatio;
+ double massDensity;
+};
+
+/// FemElement1DParameter is a FemElementParameter structure specialized for 1D element.
+struct FemElement1DParameter : public FemElementParameter
+{
+ FemElement1DParameter() : radius(0.0), enableShear(false)
+ {
+ }
+
+ double radius;
+ bool enableShear;
+};
+
+/// FemElement2DParameter is a FemElementParameter structure specialized for 2D element.
+struct FemElement2DParameter : public FemElementParameter
+{
+ FemElement2DParameter() : thickness(0.0)
+ {
+ }
+
+ double thickness;
+};
+
+/// FemElement3DParameter is a FemElementParameter structure specialized for 3D element.
+struct FemElement3DParameter : public FemElementParameter {};
+
+} // namespace FemElementStructs
+} // namespace Physics
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEMELEMENTSTRUCTS_H
diff --git a/SurgSim/Physics/FemLocalization.cpp b/SurgSim/Physics/FemLocalization.cpp
new file mode 100644
index 0000000..bdc21c6
--- /dev/null
+++ b/SurgSim/Physics/FemLocalization.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/Physics/FemLocalization.h"
+
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/FemRepresentation.h"
+#include "SurgSim/Physics/FemElement.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+FemLocalization::FemLocalization(
+ std::shared_ptr<Representation> representation,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition) :
+ Localization()
+{
+ setRepresentation(representation);
+ setLocalPosition(localPosition);
+}
+
+FemLocalization::~FemLocalization()
+{
+}
+
+void FemLocalization::setLocalPosition(
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition)
+{
+ auto femRepresentation = std::static_pointer_cast<FemRepresentation>(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& FemLocalization::getLocalPosition() const
+{
+ return m_position;
+}
+
+SurgSim::Math::Vector3d FemLocalization::doCalculatePosition(double time) const
+{
+ using SurgSim::Math::Vector3d;
+
+ auto femRepresentation = std::static_pointer_cast<FemRepresentation>(getRepresentation());
+
+ SURGSIM_ASSERT(femRepresentation != nullptr) << "FemRepresentation is null, it was probably not" <<
+ " initialized";
+
+ Vector3d currentPosition;
+ Vector3d previousPosition;
+
+ std::shared_ptr<FemElement> femElement = femRepresentation->getFemElement(m_position.index);
+ currentPosition = femElement->computeCartesianCoordinate(*femRepresentation->getCurrentState(),
+ m_position.coordinate);
+ previousPosition = femElement->computeCartesianCoordinate(*femRepresentation->getPreviousState(),
+ m_position.coordinate);
+
+ if (time <= std::numeric_limits<double>::epsilon())
+ {
+ return previousPosition;
+ }
+ else if (time >= 1.0 - std::numeric_limits<double>::epsilon())
+ {
+ return currentPosition;
+ }
+
+ return Math::interpolate(previousPosition, currentPosition, time);
+}
+
+SurgSim::Math::Vector3d FemLocalization::doCalculateVelocity(double time) const
+{
+ using SurgSim::Math::Vector3d;
+
+ auto femRepresentation = std::static_pointer_cast<FemRepresentation>(getRepresentation());
+
+ SURGSIM_ASSERT(femRepresentation != nullptr) << "FemRepresentation is null, it was probably not" <<
+ " initialized";
+
+ Vector3d currentVelocity(0.0, 0.0, 0.0);
+ Vector3d previousVelocity(0.0, 0.0, 0.0);
+
+ const SurgSim::Math::Vector& naturalCoordinate = m_position.coordinate;
+
+ std::shared_ptr<FemElement> femElement = femRepresentation->getFemElement(m_position.index);
+ SURGSIM_ASSERT(femElement->isValidCoordinate(naturalCoordinate)) <<
+ "Invalid naturalCoordinate (" << naturalCoordinate.transpose() << ")";
+ const Math::Vector& currentVelocities = femRepresentation->getCurrentState()->getVelocities();
+ const Math::Vector& previousVelocities = femRepresentation->getPreviousState()->getVelocities();
+
+ auto& nodeIds = femElement->getNodeIds();
+ for (int i = 0; i < 2; i++)
+ {
+ currentVelocity += naturalCoordinate(i) * Math::getSubVector(currentVelocities, nodeIds[i], 6).segment<3>(0);
+ previousVelocity += naturalCoordinate(i) * Math::getSubVector(previousVelocities, nodeIds[i], 6).segment<3>(0);
+ }
+
+ if (time <= std::numeric_limits<double>::epsilon())
+ {
+ return previousVelocity;
+ }
+ else if (time >= 1.0 - std::numeric_limits<double>::epsilon())
+ {
+ return currentVelocity;
+ }
+
+ return Math::interpolate(previousVelocity, currentVelocity, time);
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/FemLocalization.h b/SurgSim/Physics/FemLocalization.h
new file mode 100644
index 0000000..4fadaa4
--- /dev/null
+++ b/SurgSim/Physics/FemLocalization.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_FEMLOCALIZATION_H
+#define SURGSIM_PHYSICS_FEMLOCALIZATION_H
+
+#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/Physics/Localization.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Implementation of Localization for Fem3DRepresentation
+///
+/// FemLocalization tracks the global coordinates of an IndexedLocalCoordinate associated with an
+/// FemRepresentation. The IndexedLocalCoordinate must be related to an FemElement (the index is an FemElement id and
+/// the local coordinates are the barycentric coordinates of the nodes in this FemElement).
+/// 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 FemLocalization : public Localization
+{
+public:
+ /// Constructor
+ /// \param representation The representation to assign to this localization.
+ /// \param localPosition The local position to set the localization at.
+ FemLocalization(std::shared_ptr<Representation> representation,
+ const SurgSim::DataStructures::IndexedLocalCoordinate& localPosition);
+
+ /// Destructor
+ virtual ~FemLocalization();
+
+ /// 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;
+
+private:
+ SurgSim::Math::Vector3d doCalculatePosition(double time) const override;
+
+ SurgSim::Math::Vector3d doCalculateVelocity(double time) const override;
+
+ /// Barycentric position in local coordinates
+ SurgSim::DataStructures::IndexedLocalCoordinate m_position;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_FEMLOCALIZATION_H
diff --git a/SurgSim/Physics/FemPlyFormat.dox b/SurgSim/Physics/FemPlyFormat.dox
new file mode 100644
index 0000000..d34512a
--- /dev/null
+++ b/SurgSim/Physics/FemPlyFormat.dox
@@ -0,0 +1,88 @@
+/*!
+\page femplyformat Fem Ply Format
+\relates FemPlyReaderDelegate
+
+The following documents the capabilities of the ply readers for reading fem structures, all subclasses of `FemPlyReaderDelegate`, i.e. `Fem1DPlyReaderDelegate`, `Fem2DPlyReaderDelegate` and `Fem3DPlyReaderDelgate`. When properties are denoted as "Optional" in the following examples it means that you may use or omit this property in the ply header. Whenever the property is used in the header all elements have to supply values for the property.
+
+### Fem1D
+
+ ply
+ format ascii 1.0
+ comment Created by hand
+ element vertex 7 (Verices of the Fem1d)
+ property double x
+ property double y
+ property double z
+ property double thetaX (Optional)
+ property double thetaY (Optional)
+ property double thetaZ (Optional)
+ element 1d_element 7 (Element Definition of the Fem1d)
+ property list uint uint vertex_indices (Each entry starts with the number of nodes in this element i.e. '2', followed by the nodes' indices)
+ property double mass_density (Optional)
+ property double poisson_ratio (Optional)
+ property double young_modulus (Optional)
+ element boundary_condition 3 (Boundary conditions of the Fem1D)
+ property uint vertex_index (Each entry fixes all the degrees of freedom of the given node index)
+ element radius 1 (Radius for all the elements)
+ property double value
+ element material 1 (Physical material properties for all the elements)
+ property double mass_density
+ property double poisson_ratio
+ property double young_modulus
+ end_header
+
+The ply file for an Fem1d is mostly marked by the `1d_element` element, the `radius` and `material` properties are applied to all of the `1d_elements`. Please note that the `vertex` element is compatible with the graphics file format and you should be able to load the fem1d file into a common graphics application. Optionally you can assign per element material properties at the `1d_element` level, in this case every element will need to carry material values and the `material` element wi [...]
+
+### Fem2D
+
+ ply
+ format ascii 1.0
+ comment Created by hand
+ element vertex 6
+ property double x
+ property double y
+ property double z
+ property double thetaX (Optional)
+ property double thetaY (Optional)
+ property double thetaZ (Optional)
+ element 2d_element 3 (Element Definition of the Fem2d)
+ property list uint uint vertex_indices (each entry starts with the number of nodes in this element i.e. '3', followed by the nodes' indices)
+ property double mass_density (Optional)
+ property double poisson_ratio (Optional)
+ property double young_modulus (Optional)
+ element boundary_condition 2 (Boundary conditions of the Fem2D)
+ property uint vertex_index (Each entry fixes all the degrees of freedom of the given node index)
+ element thickness 1 (Thickness for all the elements)
+ property double value
+ element material 1 (Physical material properties for all the elements)
+ property double mass_density
+ property double poisson_ratio
+ property double young_modulus
+ end_header
+
+The ply file for an Fem2d is indicated by the `2d_element` element, the `thickness` and `material` properties are applied to all of the `2d_elements`. Please note that the `vertex` element is compatible with the graphics file format and you should be able to load the fem1d file into a common graphics application. Additionally you can add `face` elements to this file to embed a graphics mesh with the fem mesh. Currently the format only supports triangles as elements, this means the 2d_ele [...]
+
+### Fem3D
+
+ 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 (Element Definition of the Fem3d)
+ property list uint uint vertex_indices (Each entry starts with the number of nodes in this element i.e. '4' or '8', followed by the nodes' indices)
+ property double mass_density (Optional)
+ property double poisson_ratio (Optional)
+ property double young_modulus (Optional)
+ element boundary_condition 2 (Boundary conditions of the Fem3D)
+ property uint vertex_index (Each entry fixes all the degrees of freedom of the given node index)
+ element material 1 (Physical material properties for all the elements)
+ property double mass_density
+ property double poisson_ratio
+ property double young_modulus
+ end_header
+
+The ply file for an Fem3d is indicated by the `3d_element` element. Please note that the `vertex` element is compatible with the graphics file format and you should be able to load the fem3d file into a common graphics application. Additionally you can add `face` elements to this file to embed a graphics mesh with the fem mesh. Currently the reader supports tehtrahedral and hexahedral elements (4 and 8 nodes respectively), it will depend on the actual representation that is being used wh [...]
+*/
\ No newline at end of file
diff --git a/SurgSim/Physics/FemPlyReaderDelegate.cpp b/SurgSim/Physics/FemPlyReaderDelegate.cpp
index 907df6a..d43554f 100644
--- a/SurgSim/Physics/FemPlyReaderDelegate.cpp
+++ b/SurgSim/Physics/FemPlyReaderDelegate.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2014, SimQuest Solutions Inc.
+// Copyright 2014-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -14,10 +14,8 @@
// 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;
@@ -26,46 +24,47 @@ 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)
+FemPlyReaderDelegate::FemPlyReaderDelegate()
{
}
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),
+ 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));
+ "vertex_indices",
+ PlyReader::TYPE_UNSIGNED_INT,
+ offsetof(ElementData, indices),
+ PlyReader::TYPE_UNSIGNED_INT,
+ offsetof(ElementData, vertexCount));
+
+ // 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, offsetof(Vertex6DData, x));
+ reader->requestScalarProperty("vertex", "y", PlyReader::TYPE_DOUBLE, offsetof(Vertex6DData, y));
+ reader->requestScalarProperty("vertex", "z", PlyReader::TYPE_DOUBLE, offsetof(Vertex6DData, z));
+
+ if (m_hasRotationDOF)
+ {
+ reader->requestScalarProperty("vertex", "thetaX", PlyReader::TYPE_DOUBLE, offsetof(Vertex6DData, thetaX));
+ reader->requestScalarProperty("vertex", "thetaY", PlyReader::TYPE_DOUBLE, offsetof(Vertex6DData, thetaY));
+ reader->requestScalarProperty("vertex", "thetaZ", PlyReader::TYPE_DOUBLE, offsetof(Vertex6DData, thetaZ));
+ }
+
// Boundary Condition Processing
if (m_hasBoundaryConditions)
@@ -73,29 +72,39 @@ bool FemPlyReaderDelegate::registerDelegate(PlyReader* reader)
reader->requestElement(
"boundary_condition",
std::bind(&FemPlyReaderDelegate::beginBoundaryConditions,
- this,
- std::placeholders::_1,
- std::placeholders::_2),
+ 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));
+ if (m_hasMaterial)
+ {
+ 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));
+ }
+ else if (m_hasPerElementMaterial)
+ {
+ reader->requestScalarProperty(
+ getElementName(), "mass_density", PlyReader::TYPE_DOUBLE, offsetof(ElementData, massDensity));
+ reader->requestScalarProperty(
+ getElementName(), "poisson_ratio", PlyReader::TYPE_DOUBLE, offsetof(ElementData, poissonRatio));
+ reader->requestScalarProperty(
+ getElementName(), "young_modulus", PlyReader::TYPE_DOUBLE, offsetof(ElementData, youngModulus));
+ }
+
reader->setEndParseFileCallback(std::bind(&FemPlyReaderDelegate::endParseFile, this));
return true;
@@ -106,75 +115,61 @@ bool FemPlyReaderDelegate::fileIsAcceptable(const PlyReader& reader)
bool result = true;
// Shortcut test if one fails ...
+ result = result && reader.hasElement(getElementName());
+
result = result && reader.hasProperty("vertex", "x");
result = result && reader.hasProperty("vertex", "y");
result = result && reader.hasProperty("vertex", "z");
+ // 6DOF processing (this is not supported for all element types)
+ m_hasRotationDOF = reader.hasProperty("vertex", "thetaX") &&
+ reader.hasProperty("vertex", "thetaY") &&
+ reader.hasProperty("vertex", "thetaZ");
+
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");
+ // Material: either have a default material for all elements
+ // or have per element material properties
+ m_hasMaterial = reader.hasProperty("material", "mass_density") &&
+ reader.hasProperty("material", "poisson_ratio") &&
+ reader.hasProperty("material", "young_modulus");
+
+ m_hasPerElementMaterial = reader.hasProperty(getElementName(), "mass_density") &&
+ reader.hasProperty(getElementName(), "poisson_ratio") &&
+ reader.hasProperty(getElementName(), "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();
+ m_vertexData.overrun1 = 0l;
+ return &m_vertexData;
}
void FemPlyReaderDelegate::endVertices(const std::string& elementName)
{
- m_vertexIterator = nullptr;
+ SURGSIM_ASSERT(m_vertexData.overrun1 == 0l) <<
+ "There was an overrun while reading the vertex structures, it is likely that data " <<
+ "has become corrupted.";
}
void* FemPlyReaderDelegate::beginFemElements(const std::string& elementName, size_t elementCount)
{
- m_femData.overrun = 0l;
- return &m_femData;
+ m_elementData.overrun1 = 0l;
+ m_elementData.overrun2 = 0l;
+ return &m_elementData;
}
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;
+ SURGSIM_ASSERT(m_elementData.overrun1 == 0 && m_elementData.overrun2 == 0) <<
+ "There was an overrun while reading the element structures, it is likely that data " <<
+ "has become corrupted.";
+ m_elementData.indices = nullptr;
}
void* FemPlyReaderDelegate::beginMaterials(const std::string& elementName, size_t materialCount)
@@ -186,20 +181,15 @@ void* FemPlyReaderDelegate::beginMaterials(const std::string& elementName, size_
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.";
+ "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)
+ 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
index bcc247e..cb57750 100644
--- a/SurgSim/Physics/FemPlyReaderDelegate.h
+++ b/SurgSim/Physics/FemPlyReaderDelegate.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2014, SimQuest Solutions Inc.
+// Copyright 2014-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -20,18 +20,12 @@
#include <memory>
#include "SurgSim/DataStructures/PlyReaderDelegate.h"
+#include "SurgSim/Physics/Fem.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.
@@ -40,35 +34,31 @@ 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);
+ FemPlyReaderDelegate();
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();
+ bool registerDelegate(SurgSim::DataStructures::PlyReader* reader) override;
+ bool fileIsAcceptable(const SurgSim::DataStructures::PlyReader& reader) override;
/// Callback for end of PlyReader::parseFile.
- virtual void endParseFile();
+ virtual void endParseFile() = 0;
/// 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);
+ virtual 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);
+ virtual void processVertex(const std::string& elementName) = 0;
/// Callback function to finalize processing of vertices.
/// \param elementName Name of the element.
- void endVertices(const std::string& elementName);
+ virtual void endVertices(const std::string& elementName);
/// Callback function, begin the processing of FemElements.
/// \param elementName Name of the element.
@@ -102,26 +92,27 @@ protected:
/// Callback function to process one boundary condition.
/// \param elementName Name of the element.
- void processBoundaryCondition(const std::string& elementName);
+ virtual void processBoundaryCondition(const std::string& elementName) = 0;
protected:
+ /// Vertex data containing 6 dofs (3 translational and 3 rotational)
+ struct Vertex6DData
+ {
+ double x;
+ double y;
+ double z;
+ int64_t overrun1; ///< Used to check for buffer overruns
+ double thetaX;
+ double thetaY;
+ double thetaZ;
+ int64_t overrun2; ///< Used to check for buffer overruns
+ } m_vertexData;
+
/// 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;
+ unsigned int m_boundaryConditionData;
/// Internal data to receive the "material" data
struct MaterialData
@@ -132,18 +123,26 @@ protected:
int64_t overrun; ///< Used to check for buffer overruns
} m_materialData;
+ bool m_hasMaterial;
+ bool m_hasPerElementMaterial;
+
/// Internal data to receive the fem element
struct ElementData
{
- ElementData();
+ unsigned int type; // “LinearBeam”, “CorotationalTetrahedron”…
+ int64_t overrun1; ///< Used to check for buffer overruns
unsigned int* indices;
unsigned int vertexCount;
- int64_t overrun; ///< Used to check for buffer overruns
- } m_femData;
+ int64_t overrun2; ///< Used to check for buffer overruns
+ double massDensity;
+ double poissonRatio;
+ double youngModulus;
+ } m_elementData;
+ bool m_hasRotationDOF;
};
} // namespace Physics
} // namespace SurgSim
-#endif // SURGSIM_PHYSICS_FEMPLYREADERDELEGATE_H
\ No newline at end of file
+#endif // SURGSIM_PHYSICS_FEMPLYREADERDELEGATE_H
diff --git a/SurgSim/Physics/FemRepresentation.cpp b/SurgSim/Physics/FemRepresentation.cpp
index 325f676..0691040 100644
--- a/SurgSim/Physics/FemRepresentation.cpp
+++ b/SurgSim/Physics/FemRepresentation.cpp
@@ -21,10 +21,15 @@
#include "SurgSim/Framework/Runtime.h"
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/SparseMatrix.h"
#include "SurgSim/Physics/FemElement.h"
#include "SurgSim/Physics/FemPlyReaderDelegate.h"
#include "SurgSim/Physics/FemRepresentation.h"
+using SurgSim::Math::Matrix;
+using SurgSim::Math::OdeState;
+using SurgSim::Math::SparseMatrix;
+
namespace SurgSim
{
@@ -32,77 +37,52 @@ namespace Physics
{
FemRepresentation::FemRepresentation(const std::string& name) :
- DeformableRepresentation(name)
+ DeformableRepresentation(name),
+ m_useComplianceWarping(false),
+ m_isInitialComplianceMatrixComputed(false)
{
m_rayleighDamping.massCoefficient = 0.0;
m_rayleighDamping.stiffnessCoefficient = 0.0;
- SURGSIM_ADD_SERIALIZABLE_PROPERTY(FemRepresentation, std::string, Filename, getFilename, setFilename);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(FemRepresentation, bool, ComplianceWarping,
+ getComplianceWarping, setComplianceWarping);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(FemRepresentation, std::string, FemElementType,
+ getFemElementType, setFemElementType);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(FemRepresentation, double, RayleighDampingMass,
+ getRayleighDampingMass, setRayleighDampingMass);
+
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(FemRepresentation, double, RayleighDampingStiffness,
+ getRayleighDampingStiffness, setRayleighDampingStiffness);
}
FemRepresentation::~FemRepresentation()
{
}
-void FemRepresentation::setFilename(const std::string& filename)
+void FemRepresentation::setFemElementType(const std::string& type)
{
- m_filename = filename;
+ SURGSIM_ASSERT(!isInitialized()) << "You must set the FemElement type before initializing";
+ m_femElementType = type;
}
-const std::string& FemRepresentation::getFilename() const
+const std::string& FemRepresentation::getFemElementType() 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;
+ return m_femElementType;
}
bool FemRepresentation::doInitialize()
{
- if (!m_filename.empty() && !loadFile())
+ // DeformableRepresentation::doInitialize will
+ // 1) assert if initial state is not set
+ // 2) transform m_initialState properly with the initial pose
+ // => FemElement::initialize(m_initialState) is using the correct transformed state
+ if (!DeformableRepresentation::doInitialize())
{
- 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++)
{
@@ -125,6 +105,55 @@ bool FemRepresentation::doInitialize()
}
}
+ typedef SparseMatrix::Index Index;
+
+ // Precompute the sparsity pattern for the global arrays.
+ m_M.resize(static_cast<Index>(getNumDof()), static_cast<Index>(getNumDof()));
+ m_D.resize(static_cast<Index>(getNumDof()), static_cast<Index>(getNumDof()));
+ m_K.resize(static_cast<Index>(getNumDof()), static_cast<Index>(getNumDof()));
+ for (auto femElement = std::begin(m_femElements); femElement != std::end(m_femElements); femElement++)
+ {
+ Math::Matrix block = Math::Matrix::Zero(getNumDofPerNode() * (*femElement)->getNumNodes(),
+ getNumDofPerNode() * (*femElement)->getNumNodes());
+ (*femElement)->assembleMatrixBlocks(block, (*femElement)->getNodeIds(),
+ getNumDofPerNode(), &m_M, true);
+ }
+ m_M.makeCompressed();
+ m_D = m_K = m_M;
+
+ // If we are using compliance warping for this representation, let's pre-allocate the rotation matrix
+ // and pre-define its pattern, so we only access existing elements later on.
+ if (m_useComplianceWarping)
+ {
+ Index numDofPerNode = static_cast<Index>(getNumDofPerNode());
+ Index numDof = static_cast<Index>(getNumDof());
+
+ // Rotation matrix allocation and creation of the sparse matrix pattern.
+ m_complianceWarpingTransformation.resize(numDof, numDof);
+ // n columns with numDofPerNode non-zero elements each
+ m_complianceWarpingTransformation.reserve(
+ Eigen::Matrix<Index, Eigen::Dynamic, 1>::Constant(numDof, numDofPerNode));
+
+ auto logger = SurgSim::Framework::Logger::getLogger("Physics/FemRepresentation");
+ SURGSIM_LOG_IF(numDofPerNode % 3 != 0, logger, SEVERE) << "Using compliance warping with representation " <<
+ getName() << " which has " << numDofPerNode << " dof per node (not a factor of 3)";
+
+ // Use a mask of 1 to setup the sparse matrix pattern
+ for (Index nodeId = 0; nodeId < static_cast<Index>(m_initialState->getNumNodes()); ++nodeId)
+ {
+ for (Index i = 0; i < numDofPerNode; ++i)
+ {
+ for (Index j = 0; j < numDofPerNode; ++j)
+ {
+ m_complianceWarpingTransformation.insert(nodeId * numDofPerNode + i, nodeId * numDofPerNode + j) =
+ 1.0;
+ }
+ }
+ }
+
+ m_complianceWarpingTransformation.makeCompressed();
+ }
+
return true;
}
@@ -147,7 +176,7 @@ std::shared_ptr<FemElement> FemRepresentation::getFemElement(size_t femElementId
bool FemRepresentation::isValidCoordinate(const SurgSim::DataStructures::IndexedLocalCoordinate& coordinate) const
{
return (coordinate.index < m_femElements.size())
- && m_femElements[coordinate.index]->isValidCoordinate(coordinate.coordinate);
+ && m_femElements[coordinate.index]->isValidCoordinate(coordinate.coordinate);
}
double FemRepresentation::getTotalMass() const
@@ -190,73 +219,153 @@ void FemRepresentation::beforeUpdate(double dt)
<< "State has not been initialized yet, call setInitialState() prior to running the simulation";
}
-void FemRepresentation::afterUpdate(double dt)
+void FemRepresentation::update(double dt)
{
- DeformableRepresentation::afterUpdate(dt);
+ if (!isActive())
+ {
+ return;
+ }
- // 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;
+ 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() ?";
- setLocalActive(false);
- return;
- }
+ // Solve the ode and compute the requested compliance matrix
+ if (getComplianceWarping())
+ {
+ if (!isInitialComplianceMatrixComputed())
+ {
+ m_odeSolver->computeMatrices(dt, *m_initialState, true);
+ setIsInitialComplianceMatrixComputed(true);
}
- );
+ m_odeSolver->solve(dt, *m_currentState, m_newState.get(), false);
+
+ // Update the compliance matrix
+ updateComplianceMatrix(*m_newState);
+ }
+ else
+ {
+ 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 FemRepresentation::setComplianceWarping(bool useComplianceWarping)
+{
+ SURGSIM_ASSERT(!isInitialized()) << "Compliance warping cannot be modified once the component is initialized";
+
+ m_useComplianceWarping = useComplianceWarping;
+}
+
+bool FemRepresentation::getComplianceWarping() const
+{
+ return m_useComplianceWarping;
}
-SurgSim::Math::Vector& FemRepresentation::computeF(const SurgSim::Math::OdeState& state)
+Math::Matrix FemRepresentation::applyCompliance(const Math::OdeState& state, const Math::Matrix& b)
+{
+ SURGSIM_ASSERT(m_odeSolver) << "Ode solver not initialized, it should have been initialized on wake-up";
+
+ if (m_useComplianceWarping)
+ {
+ // Then, update the compliance matrix using compliance warping
+ return (m_complianceWarpingTransformation * DeformableRepresentation::applyCompliance(state,
+ m_complianceWarpingTransformation.transpose() * b));
+ }
+ return DeformableRepresentation::applyCompliance(state, b);
+}
+
+const SurgSim::Math::Matrix& FemRepresentation::getComplianceMatrix() const
+{
+ SURGSIM_ASSERT(m_odeSolver) << "Ode solver not initialized, it should have been initialized on wake-up";
+
+ if (m_useComplianceWarping)
+ {
+ return m_complianceWarpingMatrix;
+ }
+ return m_odeSolver->getComplianceMatrix();
+}
+
+SurgSim::Math::Matrix FemRepresentation::getNodeTransformation(const SurgSim::Math::OdeState& state, size_t nodeId)
+{
+ SURGSIM_FAILURE() << "Any representation using compliance warping should override this method to provide the " <<
+ "proper nodes transformation";
+
+ return SurgSim::Math::Matrix();
+}
+
+void FemRepresentation::updateComplianceMatrix(const SurgSim::Math::OdeState& state)
+{
+ using SurgSim::Math::assignSubMatrix;
+
+ // Update the compliance warping transformation using all the nodes' transformation
+ for (size_t nodeId = 0; nodeId < state.getNumNodes(); ++nodeId)
+ {
+ assignSubMatrix(getNodeTransformation(state, nodeId), nodeId,
+ nodeId, &m_complianceWarpingTransformation, false);
+ }
+
+ // Then, transform the initial compliance matrix to get the current compliance warping matrix
+ m_complianceWarpingMatrix = m_complianceWarpingTransformation * m_odeSolver->getComplianceMatrix() *
+ m_complianceWarpingTransformation.transpose();
+}
+
+void 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();
+ m_f.setZero(state.getNumDof());
addGravityForce(&m_f, state);
addRayleighDampingForce(&m_f, state);
addFemElementsForce(&m_f, state);
// Add external generalized force
- m_f += m_externalGeneralizedForce;
-
- return m_f;
+ if (m_hasExternalGeneralizedForce)
+ {
+ m_f += m_externalGeneralizedForce;
+ }
}
-const SurgSim::Math::Matrix& FemRepresentation::computeM(const SurgSim::Math::OdeState& state)
+void 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();
+ Math::clearMatrix(&m_M);
for (auto femElement = std::begin(m_femElements); femElement != std::end(m_femElements); femElement++)
{
- (*femElement)->addMass(state, &m_M);
+ (*femElement)->addMass(&m_M);
}
-
- return m_M;
}
-const SurgSim::Math::Matrix& FemRepresentation::computeD(const SurgSim::Math::OdeState& state)
+void 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();
+ Math::clearMatrix(&m_D);
// 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);
+ (*femElement)->addMass(&m_D, rayleighMass);
}
}
@@ -265,62 +374,58 @@ const SurgSim::Math::Matrix& FemRepresentation::computeD(const SurgSim::Math::Od
{
for (auto femElement = std::begin(m_femElements); femElement != std::end(m_femElements); femElement++)
{
- (*femElement)->addStiffness(state, &m_D, rayleighStiffness);
+ (*femElement)->addStiffness(&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);
+ (*femElement)->addDamping(&m_D);
}
// Add external generalized damping
- m_D += m_externalGeneralizedDamping;
-
- return m_D;
+ if (m_hasExternalGeneralizedForce)
+ {
+ m_D += m_externalGeneralizedDamping;
+ }
}
-const SurgSim::Math::Matrix& FemRepresentation::computeK(const SurgSim::Math::OdeState& state)
+void 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();
+ Math::clearMatrix(&m_K);
for (auto femElement = std::begin(m_femElements); femElement != std::end(m_femElements); femElement++)
{
- (*femElement)->addStiffness(state, &m_K);
+ (*femElement)->addStiffness(&m_K);
}
// Add external generalized stiffness
- m_K += m_externalGeneralizedStiffness;
-
- return m_K;
+ if (m_hasExternalGeneralizedForce)
+ {
+ m_K += m_externalGeneralizedStiffness;
+ }
}
-void FemRepresentation::computeFMDK(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector** f,
- SurgSim::Math::Matrix** M, SurgSim::Math::Matrix** D, SurgSim::Math::Matrix** K)
+void FemRepresentation::computeFMDK(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();
+ m_f.setZero(state.getNumDof());
// Make sure the mass matrix has been properly allocated and zeroed out
- m_M.resize(state.getNumDof(), state.getNumDof());
- m_M.setZero();
+ Math::clearMatrix(&m_M);
// Make sure the damping matrix has been properly allocated and zeroed out
- m_D.resize(state.getNumDof(), state.getNumDof());
- m_D.setZero();
+ Math::clearMatrix(&m_D);
// Make sure the stiffness matrix has been properly allocated and zeroed out
- m_K.resize(state.getNumDof(), state.getNumDof());
- m_K.setZero();
+ Math::clearMatrix(&m_K);
// 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);
+ (*femElement)->addFMDK(&m_f, &m_M, &m_D, &m_K);
}
// Add the Rayleigh damping matrix
@@ -340,19 +445,40 @@ void FemRepresentation::computeFMDK(const SurgSim::Math::OdeState& state, SurgSi
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;
+ if (m_previousHasExternalGeneralizedForce != m_hasExternalGeneralizedForce)
+ {
+ setIsInitialComplianceMatrixComputed(false);
+ }
- *f = &m_f;
- *M = &m_M;
- *D = &m_D;
- *K = &m_K;
+ if (m_hasExternalGeneralizedForce)
+ {
+ m_f += m_externalGeneralizedForce;
+ m_K += m_externalGeneralizedStiffness;
+ m_D += m_externalGeneralizedDamping;
+
+ if (!m_previousExternalGeneralizedStiffness.isApprox(m_externalGeneralizedStiffness) ||
+ !m_previousExternalGeneralizedDamping.isApprox(m_externalGeneralizedDamping))
+ {
+ setIsInitialComplianceMatrixComputed(false);
+ }
+ }
+}
+
+void FemRepresentation::updateFMDK(const SurgSim::Math::OdeState& state, int options)
+{
+ // This function updates the matrices needed to calculate F, M, D, K for each element.
+ // Note that the relevant matrices are updated only for non-linear elements.
+ for (auto femElement = std::begin(m_femElements); femElement != std::end(m_femElements); femElement++)
+ {
+ (*femElement)->updateFMDK(state, options);
+ }
+
+ OdeEquation::updateFMDK(state, options);
}
void FemRepresentation::addRayleighDampingForce(
- SurgSim::Math::Vector* force, const SurgSim::Math::OdeState& state,
- bool useGlobalStiffnessMatrix, bool useGlobalMassMatrix, double scale)
+ SurgSim::Math::Vector* force, const SurgSim::Math::OdeState& state,
+ bool useGlobalStiffnessMatrix, bool useGlobalMassMatrix, double scale)
{
// Temporary variables for convenience
double& rayleighMass = m_rayleighDamping.massCoefficient;
@@ -365,14 +491,15 @@ void FemRepresentation::addRayleighDampingForce(
// If we have the mass matrix, we can compute directly F = -rayleighMass.M.v(t)
if (useGlobalMassMatrix)
{
- *force -= (scale * rayleighMass) * (m_M * v);
+ Math::Vector tempForce = (scale * rayleighMass) * (m_M * v);
+ *force -= tempForce;
}
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);
+ (*femElement)->addMatVec(- scale * rayleighMass, 0.0, 0.0, v, force);
}
}
}
@@ -383,37 +510,38 @@ void FemRepresentation::addRayleighDampingForce(
{
if (useGlobalStiffnessMatrix)
{
- *force -= scale * rayleighStiffness * (m_K * v);
+ Math::Vector tempForce = scale * rayleighStiffness * (m_K * v);
+ *force -= tempForce;
}
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);
+ (*femElement)->addMatVec(0.0, 0.0, - scale * rayleighStiffness, v, force);
}
}
}
}
void FemRepresentation::addFemElementsForce(SurgSim::Math::Vector* force,
- const SurgSim::Math::OdeState& state,
- double scale)
+ 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);
+ (*femElement)->addForce(force, scale);
}
}
void FemRepresentation::addGravityForce(SurgSim::Math::Vector* f,
- const SurgSim::Math::OdeState& state,
- double scale)
+ 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() ?";
+ "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());
@@ -429,6 +557,16 @@ void FemRepresentation::addGravityForce(SurgSim::Math::Vector* f,
}
}
+bool FemRepresentation::isInitialComplianceMatrixComputed() const
+{
+ return m_isInitialComplianceMatrixComputed;
+}
+
+void FemRepresentation::setIsInitialComplianceMatrixComputed(bool flag)
+{
+ m_isInitialComplianceMatrixComputed = flag;
+}
+
} // namespace Physics
} // namespace SurgSim
diff --git a/SurgSim/Physics/FemRepresentation.h b/SurgSim/Physics/FemRepresentation.h
index a42572e..972e099 100644
--- a/SurgSim/Physics/FemRepresentation.h
+++ b/SurgSim/Physics/FemRepresentation.h
@@ -19,8 +19,11 @@
#include <memory>
#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/SparseMatrix.h"
#include "SurgSim/Math/Vector.h"
#include "SurgSim/Physics/DeformableRepresentation.h"
+#include "SurgSim/Physics/Fem.h"
namespace SurgSim
{
@@ -31,10 +34,15 @@ 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)
+/// 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), therefore it defines
+/// a dynamic system \f$M.a=F(x,v)\f$
+/// \note + The model handles damping through the Rayleigh damping (it is a combination of mass and stiffness)
+/// \note + The model handles compliance warping (optional) from the paper:
+/// \note "Efficient Contact Modeling using Compliance Warping", G Saupin, C Duriez, S Cotin, L Grisoni;
+/// Computer %Graphics International (CGI), Istanbul, Turkey, june 2008.
+/// \note To use compliance warping, it needs to be turned on by calling setComplianceWarping(true) and the method
+/// updateNodesRotations() needs to be overloaded properly.
class FemRepresentation : public DeformableRepresentation
{
public:
@@ -45,17 +53,17 @@ public:
/// 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);
+ /// Loads the FEM file into an Fem class data structure
+ /// \param filename The file to load
+ virtual void loadFem(const std::string& filename) = 0;
- /// Gets the name of the file to be loaded
- /// \return filename The name of the file to be loaded
- const std::string& getFilename() const;
+ /// Sets the FemElement type pulled from the object factory
+ /// \param type A string of the full registered OSS class name in the factory
+ virtual void setFemElementType(const std::string& type);
- /// Loads the file
- /// \return true if successful
- bool loadFile();
+ /// Gets the FemElement type pulled from the object factory
+ /// \return A string of the full registered OSS class name in the factory
+ const std::string& getFemElementType() const;
/// Adds a FemElement
/// \param element The FemElement to add to the representation
@@ -95,53 +103,33 @@ public:
/// 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;
+ 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;
+ 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;
+ void update(double dt) override;
+
+ /// Set the compliance warping flag
+ /// \param useComplianceWarping True to use compliance warping, False otherwise
+ /// \exception SurgSim::Framework::AssertionFailure If the call is done after initialization
+ /// \note Compliance warping is currently disabled in this version.
+ void setComplianceWarping(bool useComplianceWarping);
+
+ /// Get the compliance warping flag (default = false)
+ /// \return True if compliance warping is used, False otherwise
+ bool getComplianceWarping() const;
+
+ /// Calculate the product C.b where C is the compliance matrix with boundary conditions
+ /// \param state \f$(x, v)\f$ the current position and velocity to evaluate the various terms with
+ /// \param b The input matrix b
+ /// \return Returns the matrix \f$C.b\f$
+ Math::Matrix applyCompliance(const Math::OdeState& state, const Math::Matrix& b) override;
+
+ const SurgSim::Math::Matrix& getComplianceMatrix() const override;
+
+ void updateFMDK(const SurgSim::Math::OdeState& state, int options) override;
protected:
/// Adds the Rayleigh damping forces
@@ -156,7 +144,8 @@ protected:
/// \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);
+ 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
@@ -169,31 +158,70 @@ protected:
/// \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);
+ void addGravityForce(SurgSim::Math::Vector* f, const SurgSim::Math::OdeState& state, double scale = 1.0);
- virtual bool doInitialize() override;
+ bool doInitialize() override;
- /// Useful information per node
- std::vector<double> m_massPerNode; //< Useful in setting up the gravity force F=mg
+ /// Updates the compliance matrix using nodes transformation (useful for compliance warping)
+ /// \param state The state to compute the nodes transformation from
+ /// \note This computes the diagonal block matrix m_complianceWarpingTransformation and
+ /// transforms the initial compliance matrix with it.
+ void updateComplianceMatrix(const SurgSim::Math::OdeState& state);
- /// Filename for loading the fem representation.
- std::string m_filename;
+ /// Retrieves a specific node transformation (useful for compliance warping)
+ /// \param state The state to extract the node transformation from
+ /// \param nodeId The node to update the rotation for
+ /// \return The node transformation. i.e. a numDofPerNode x numDofPerNode matrix
+ virtual SurgSim::Math::Matrix getNodeTransformation(const SurgSim::Math::OdeState& state, size_t nodeId);
-private:
- /// To be implemented by derived classes.
- /// \return The delegate to load the corresponding derived class.
- virtual std::shared_ptr<FemPlyReaderDelegate> getDelegate() = 0;
+ /// Gets the flag keeping track of the initial compliance matrix calculation (compliance warping case)
+ /// \return True if the initial compliance matrix has been computed, False otherwise
+ bool isInitialComplianceMatrixComputed() const;
+
+ /// Sets the flag keeping track of the initial compliance matrix calculation (compliance warping case)
+ /// \param flag True if the initial compliance matrix is computed, False otherwise
+ void setIsInitialComplianceMatrixComputed(bool flag);
+
+ void computeF(const SurgSim::Math::OdeState& state) override;
+
+ void computeM(const SurgSim::Math::OdeState& state) override;
+
+ void computeD(const SurgSim::Math::OdeState& state) override;
+
+ void computeK(const SurgSim::Math::OdeState& state) override;
+
+ void computeFMDK(const SurgSim::Math::OdeState& state) override;
+
+ /// Useful information per node
+ std::vector<double> m_massPerNode; ///< Useful in setting up the gravity force F=mg
/// FemElements
std::vector<std::shared_ptr<FemElement>> m_femElements;
+ /// The FemElement factory parameter invoked in doInitialize() when using a MeshAsset
+ /// either with LoadFem(const std::stirng& filename) or setFem(std::shared_ptr<Asset> mesh)
+ /// This ensures that when using a mesh Asset, a single FemElement type is used. Therefore we do
+ /// not need to define this type in the ply file, but rather is part of the Representation properties (YAML).
+ std::string m_femElementType;
+
+private:
/// Rayleigh damping parameters (massCoefficient and stiffnessCoefficient)
/// D = massCoefficient.M + stiffnessCoefficient.K
/// Matrices: D = damping, M = mass, K = stiffness
- struct {
+ struct
+ {
double massCoefficient;
double stiffnessCoefficient;
} m_rayleighDamping;
+
+ bool m_useComplianceWarping; ///< Are we using Compliance Warping or not ?
+
+ bool m_isInitialComplianceMatrixComputed; ///< For compliance warping: Is the initial compliance matrix computed ?
+
+ SurgSim::Math::Matrix m_complianceWarpingMatrix; ///< The compliance warping matrix if compliance warping in use
+
+ /// The system-size transformation matrix. It contains nodes transformation on the diagonal blocks.
+ Eigen::SparseMatrix<double> m_complianceWarpingTransformation;
};
} // namespace Physics
diff --git a/SurgSim/Physics/FemRepresentationParameters.cpp b/SurgSim/Physics/FemRepresentationParameters.cpp
deleted file mode 100644
index 8629325..0000000
--- a/SurgSim/Physics/FemRepresentationParameters.cpp
+++ /dev/null
@@ -1,198 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index ab7386a..0000000
--- a/SurgSim/Physics/FemRepresentationParameters.h
+++ /dev/null
@@ -1,171 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/FixedConstraintFixedPoint.cpp b/SurgSim/Physics/FixedConstraintFixedPoint.cpp
new file mode 100644
index 0000000..475a695
--- /dev/null
+++ b/SurgSim/Physics/FixedConstraintFixedPoint.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/Physics/FixedConstraintFixedPoint.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/Localization.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+FixedConstraintFixedPoint::FixedConstraintFixedPoint()
+{
+}
+
+FixedConstraintFixedPoint::~FixedConstraintFixedPoint()
+{
+}
+
+void FixedConstraintFixedPoint::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::Physics::ConstraintType FixedConstraintFixedPoint::getConstraintType() const
+{
+ return SurgSim::Physics::FIXED_3DPOINT;
+}
+
+size_t FixedConstraintFixedPoint::doGetNumDof() const
+{
+ return 3;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/FixedConstraintFixedPoint.h b/SurgSim/Physics/FixedConstraintFixedPoint.h
new file mode 100644
index 0000000..36328fe
--- /dev/null
+++ b/SurgSim/Physics/FixedConstraintFixedPoint.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_FIXEDCONSTRAINTFIXEDPOINT_H
+#define SURGSIM_PHYSICS_FIXEDCONSTRAINTFIXEDPOINT_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// FixedRepresentation bilateral 3d constraint implementation.
+///
+/// The family of FixedPoint constraints enforce equality between two points.
+class FixedConstraintFixedPoint : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ FixedConstraintFixedPoint();
+
+ /// Destructor
+ virtual ~FixedConstraintFixedPoint();
+
+ /// Gets the constraint type for this ConstraintImplementation
+ /// \return The constraint type corresponding to this constraint implementation
+ SurgSim::Physics::ConstraintType getConstraintType() 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_FIXEDCONSTRAINTFIXEDPOINT_H
diff --git a/SurgSim/Physics/FixedConstraintFixedRotationVector.cpp b/SurgSim/Physics/FixedConstraintFixedRotationVector.cpp
new file mode 100644
index 0000000..db65d84
--- /dev/null
+++ b/SurgSim/Physics/FixedConstraintFixedRotationVector.cpp
@@ -0,0 +1,72 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/FixedConstraintFixedRotationVector.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/RotationVectorConstraintData.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+FixedConstraintFixedRotationVector::FixedConstraintFixedRotationVector()
+{
+}
+
+FixedConstraintFixedRotationVector::~FixedConstraintFixedRotationVector()
+{
+}
+
+void FixedConstraintFixedRotationVector::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;
+ const auto& constRotVecData = static_cast<const RotationVectorRigidFem1DConstraintData&>(data);
+ SurgSim::Math::Vector3d rotationVector = constRotVecData.getCurrentRotationVector();
+
+ // Fill up b with the constraint equation...
+ mlcp->b.segment<3>(indexOfConstraint) += rotationVector * scale;
+}
+
+SurgSim::Physics::ConstraintType FixedConstraintFixedRotationVector::getConstraintType() const
+{
+ return SurgSim::Physics::FIXED_3DROTATION_VECTOR;
+}
+
+size_t FixedConstraintFixedRotationVector::doGetNumDof() const
+{
+ return 3;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/FixedConstraintFixedRotationVector.h b/SurgSim/Physics/FixedConstraintFixedRotationVector.h
new file mode 100644
index 0000000..80a8e95
--- /dev/null
+++ b/SurgSim/Physics/FixedConstraintFixedRotationVector.h
@@ -0,0 +1,68 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_FIXEDCONSTRAINTFIXEDROTATIONVECTOR_H
+#define SURGSIM_PHYSICS_FIXEDCONSTRAINTFIXEDROTATIONVECTOR_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// FixedRepresentation fixed rotation vector constraint
+///
+/// This implementation simply fixes the rotational dof of the constraint, effectively controlling the
+/// other representation orientation.
+class FixedConstraintFixedRotationVector : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ FixedConstraintFixedRotationVector();
+
+ /// Destructor
+ virtual ~FixedConstraintFixedRotationVector();
+
+ SurgSim::Physics::ConstraintType getConstraintType() const override;
+
+private:
+ size_t doGetNumDof() const override;
+
+ /// \note This is only setting the mlcp->b violation to the fixed representation rotation vector.
+ /// It means that the fixed won't receive any forces back, it simply will control the other representation's
+ /// orientation in this constraint.
+ ///
+ /// \note The constraint violation being calculated based on a quaternion interpolation (slerp), and this
+ /// type of interpolation being highly non-linear, the classical way of using the implementation one after the
+ /// other one won't work.
+ /// Therefore, the RotationVectorConstraint will use the vector mlcp->b to retrieve both representation's
+ /// rotation vector, then calculate the proper slerp and set the violation back in mlcp->b
+ 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_FIXEDCONSTRAINTFIXEDROTATIONVECTOR_H
diff --git a/SurgSim/Physics/FixedConstraintFrictionlessContact.cpp b/SurgSim/Physics/FixedConstraintFrictionlessContact.cpp
new file mode 100644
index 0000000..00bcec4
--- /dev/null
+++ b/SurgSim/Physics/FixedConstraintFrictionlessContact.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/FixedConstraintFrictionlessContact.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/Representation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+FixedConstraintFrictionlessContact::FixedConstraintFrictionlessContact()
+{
+}
+
+FixedConstraintFrictionlessContact::~FixedConstraintFrictionlessContact()
+{
+}
+
+void FixedConstraintFrictionlessContact::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();
+
+ // FRICTIONLESS CONTACT in a LCP
+ // (n, d) defines the plane of contact
+ // P(t) the point of contact
+ // b = n.P(t) + d
+ // Since the d term will be added to the constraint for one side of the contact and subtracted from the other,
+ // and because it is not clear which distance should be used, we leave it out.
+
+ SurgSim::Math::Vector3d globalPosition = localization->calculatePosition();
+
+ // Fill up b with the constraint equation...
+ double violation = n.dot(globalPosition);
+ b[indexOfConstraint] += violation * scale;
+}
+
+SurgSim::Physics::ConstraintType FixedConstraintFrictionlessContact::getConstraintType() const
+{
+ return SurgSim::Physics::FRICTIONLESS_3DCONTACT;
+}
+
+size_t FixedConstraintFrictionlessContact::doGetNumDof() const
+{
+ return 1;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/FixedConstraintFrictionlessContact.h b/SurgSim/Physics/FixedConstraintFrictionlessContact.h
new file mode 100644
index 0000000..482d0ca
--- /dev/null
+++ b/SurgSim/Physics/FixedConstraintFrictionlessContact.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_FIXEDCONSTRAINTFRICTIONLESSCONTACT_H
+#define SURGSIM_PHYSICS_FIXEDCONSTRAINTFRICTIONLESSCONTACT_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// FixedRepresentation frictionless contact implementation.
+class FixedConstraintFrictionlessContact : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ FixedConstraintFrictionlessContact();
+
+ /// Destructor
+ virtual ~FixedConstraintFrictionlessContact();
+
+
+ /// Gets the constraint type for this ConstraintImplementation
+ /// \return The constraint type corresponding to this constraint implementation
+ SurgSim::Physics::ConstraintType getConstraintType() 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_FIXEDCONSTRAINTFRICTIONLESSCONTACT_H
diff --git a/SurgSim/Physics/FixedRepresentation.cpp b/SurgSim/Physics/FixedRepresentation.cpp
index b055e9d..54c8d8d 100644
--- a/SurgSim/Physics/FixedRepresentation.cpp
+++ b/SurgSim/Physics/FixedRepresentation.cpp
@@ -30,13 +30,7 @@ FixedRepresentation::~FixedRepresentation()
{
}
-
-RepresentationType FixedRepresentation::getType() const
-{
- return REPRESENTATION_TYPE_FIXED;
-}
-
-void FixedRepresentation::updateGlobalInertiaMatrices(const RigidRepresentationState& state)
+void FixedRepresentation::updateGlobalInertiaMatrices(const RigidState& state)
{
// Do Nothing it is a fixed object
}
diff --git a/SurgSim/Physics/FixedRepresentation.h b/SurgSim/Physics/FixedRepresentation.h
index 4a38126..9a0e133 100644
--- a/SurgSim/Physics/FixedRepresentation.h
+++ b/SurgSim/Physics/FixedRepresentation.h
@@ -24,7 +24,9 @@ namespace SurgSim
namespace Physics
{
-class RigidRepresentationState;
+class RigidState;
+
+typedef RigidLocalization FixedLocalization;
SURGSIM_STATIC_REGISTRATION(FixedRepresentation);
@@ -42,11 +44,9 @@ public:
SURGSIM_CLASSNAME(SurgSim::Physics::FixedRepresentation);
- virtual RepresentationType getType() const override;
-
- virtual void updateGlobalInertiaMatrices(const RigidRepresentationState& state) override;
+ void updateGlobalInertiaMatrices(const RigidState& state) override;
- virtual void update(double dt) override;
+ void update(double dt) override;
};
}; // Physics
diff --git a/SurgSim/Physics/FixedRepresentationBilateral3D.cpp b/SurgSim/Physics/FixedRepresentationBilateral3D.cpp
deleted file mode 100644
index 14866de..0000000
--- a/SurgSim/Physics/FixedRepresentationBilateral3D.cpp
+++ /dev/null
@@ -1,77 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index bbd69be..0000000
--- a/SurgSim/Physics/FixedRepresentationBilateral3D.h
+++ /dev/null
@@ -1,74 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
deleted file mode 100644
index 3cd6db7..0000000
--- a/SurgSim/Physics/FixedRepresentationContact.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index a98f026..0000000
--- a/SurgSim/Physics/FixedRepresentationContact.h
+++ /dev/null
@@ -1,76 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
deleted file mode 100644
index 04b0d6d..0000000
--- a/SurgSim/Physics/FixedRepresentationLocalization.h
+++ /dev/null
@@ -1,105 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
index 10b3b85..df2be3a 100644
--- a/SurgSim/Physics/FreeMotion.cpp
+++ b/SurgSim/Physics/FreeMotion.cpp
@@ -17,9 +17,11 @@
#include <memory>
#include <vector>
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/ThreadPool.h"
#include "SurgSim/Physics/FreeMotion.h"
-#include "SurgSim/Physics/Representation.h"
#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/Representation.h"
namespace SurgSim
{
@@ -27,7 +29,8 @@ namespace Physics
{
-FreeMotion::FreeMotion(bool doCopyState) : Computation(doCopyState)
+FreeMotion::FreeMotion(bool doCopyState) :
+ Computation(doCopyState)
{
}
@@ -37,21 +40,31 @@ FreeMotion::~FreeMotion()
}
-std::shared_ptr<PhysicsManagerState> FreeMotion::doUpdate(
- const double& dt,
- const std::shared_ptr<PhysicsManagerState>& state)
+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)
+ auto threadPool = Framework::Runtime::getThreadPool();
+ std::vector<std::future<void>> tasks;
+
+ auto& representations = result->getActiveRepresentations();
+ for (auto& representation : representations)
+ {
+ tasks.push_back(threadPool->enqueue<void>([dt, &representation]() { representation->update(dt); }));
+ }
+
+ auto& particleRepresentations = result->getActiveParticleRepresentations();
+ for (auto& representation : particleRepresentations)
{
- (*it)->update(dt);
+ tasks.push_back(threadPool->enqueue<void>([dt, &representation]() { representation->update(dt); }));
}
- result->setRepresentations(representations);
+ for (auto& task : tasks)
+ {
+ task.get();
+ }
return result;
}
diff --git a/SurgSim/Physics/FreeMotion.h b/SurgSim/Physics/FreeMotion.h
index 32d11e3..802c31d 100644
--- a/SurgSim/Physics/FreeMotion.h
+++ b/SurgSim/Physics/FreeMotion.h
@@ -19,7 +19,7 @@
#include <memory>
#include <vector>
-
+#include "SurgSim/Framework/Macros.h"
#include "SurgSim/Physics/Computation.h"
namespace SurgSim
@@ -29,7 +29,7 @@ namespace Physics
class Representation;
-/// Apply the Freemotion calcluation to all physics representations
+/// Apply the FreeMotion calculation to all physics representations
class FreeMotion : public Computation
{
public:
@@ -38,19 +38,18 @@ public:
/// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
explicit FreeMotion(bool doCopyState = false);
+ SURGSIM_CLASSNAME(SurgSim::Physics::FreeMotion);
+
/// Destructor
~FreeMotion();
protected:
-
/// Override doUpdate from superclass
- virtual std::shared_ptr<PhysicsManagerState> doUpdate(
- const double& dt,
- const std::shared_ptr<PhysicsManagerState>& state) override;
-
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+ override;
};
}; // Physics
}; // SurgSim
-#endif
\ No newline at end of file
+#endif
diff --git a/SurgSim/Physics/LinearSpring.cpp b/SurgSim/Physics/LinearSpring.cpp
index 92c0939..936aea8 100644
--- a/SurgSim/Physics/LinearSpring.cpp
+++ b/SurgSim/Physics/LinearSpring.cpp
@@ -16,12 +16,13 @@
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Math/Geometry.h"
#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/SparseMatrix.h"
#include "SurgSim/Physics/LinearSpring.h"
-using SurgSim::Math::addSubMatrix;
-using SurgSim::Math::addSubVector;
-using SurgSim::Math::getSubVector;
+using SurgSim::Math::Matrix;
using SurgSim::Math::Matrix33d;
+using SurgSim::Math::OdeState;
+using SurgSim::Math::SparseMatrix;
using SurgSim::Math::Vector;
using SurgSim::Math::Vector3d;
@@ -32,14 +33,23 @@ namespace Physics
{
LinearSpring::LinearSpring(size_t nodeId0, size_t nodeId1) :
- Spring(), m_restLength(0.0), m_stiffness(0.0), m_damping(0.0)
+ Spring(), m_restLength(-1.0), m_stiffness(-1.0), m_damping(0.0)
{
m_nodeIds.push_back(nodeId0);
m_nodeIds.push_back(nodeId1);
}
+void LinearSpring::initialize(const OdeState& state)
+{
+ Spring::initialize(state);
+
+ SURGSIM_ASSERT(m_restLength >= 0.0) << "Spring rest length was not set, please call setRestLength()";
+ SURGSIM_ASSERT(m_stiffness >= 0.0) << "Spring stiffness was not set, please call setStiffness()";
+}
+
void LinearSpring::setStiffness(double stiffness)
{
+ SURGSIM_ASSERT(stiffness >= 0.0) << "Spring stiffness cannot be negative";
m_stiffness = stiffness;
}
@@ -50,6 +60,7 @@ double LinearSpring::getStiffness() const
void LinearSpring::setDamping(double damping)
{
+ SURGSIM_ASSERT(damping >= 0.0) << "Spring damping cannot be negative";
m_damping = damping;
}
@@ -60,6 +71,7 @@ double LinearSpring::getDamping() const
void LinearSpring::setRestLength(double restLength)
{
+ SURGSIM_ASSERT(restLength >= 0.0) << "Spring rest length cannot be negative";
m_restLength = restLength;
}
@@ -68,130 +80,150 @@ double LinearSpring::getRestLength() const
return m_restLength;
}
-void LinearSpring::addForce(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F, double scale)
+void LinearSpring::addForce(const OdeState& state, 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);
-
+ const auto& x0 = state.getPositions().segment<3>(3 * m_nodeIds[0]);
+ const auto& x1 = state.getPositions().segment<3>(3 * m_nodeIds[1]);
+ const auto& v0 = state.getVelocities().segment<3>(3 * m_nodeIds[0]);
+ const auto& v1 = state.getVelocities().segment<3>(3 * m_nodeIds[1]);
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";
+ "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;
+ 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);
+ F->segment<3>(3 * m_nodeIds[0]) += f;
+ F->segment<3>(3 * m_nodeIds[1]) -= f;
}
-void LinearSpring::addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* D, double scale)
+void LinearSpring::addDamping(const OdeState& state, Math::SparseMatrix* 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)
+ Matrix33d De;
+
+ // The spring has 2 nodes with positions {x1, x2}, velocities {v1, v2} and force {F1, F2=-F1}
+ // Also note from addForce that the positions and velocities play a symmetric role in the force calculation
+ // i.e. dFi/dx1 = -dFi/dx2 and dFi/dv1 = -dFi/dv2
+ // The damping matrix is D = -dF/dv = (-dF1/dv1 -dF1/dv2) = (-dF1/dv1 dF1/dv1)
+ // (-dF2/dv1 -dF2/dv2) ( dF1/dv1 -dF1/dv1)
+
+ // Let's compute De = -dF1/dv1
+ if (!computeDampingAndStiffness(state, &De, nullptr))
{
- 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());
+ De *= scale;
// 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);
+ Math::addSubMatrix(De, static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ D, false);
+ Matrix33d negativeDe = -De;
+ Math::addSubMatrix(negativeDe, static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ D, false);
+ Math::addSubMatrix(negativeDe, static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ D, false);
+ Math::addSubMatrix(De, static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ D, false);
}
-void LinearSpring::addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix* K, double scale)
+void LinearSpring::addStiffness(const OdeState& state, Math::SparseMatrix* 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)
+ Matrix33d Ke;
+
+ // The spring has 2 nodes with positions {x1, x2}, velocities {v1, v2} and force {F1, F2=-F1}
+ // Also note from addForce that the positions and velocities play a symmetric role in the force calculation
+ // i.e. dFi/dx1 = -dFi/dx2 and dFi/dv1 = -dFi/dv2
+ // The stiffness matrix is K = -dF/dx = (-dF1/dx1 -dF1/dx2) = (-dF1/dx1 dF1/dx1)
+ // (-dF2/dx1 -dF2/dx2) ( dF1/dx1 -dF1/dx1)
+
+ // Let's compute Ke = -dF1/dx1
+ if (!computeDampingAndStiffness(state, nullptr, &Ke))
{
- 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;
+ Ke *= 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);
+ Math::addSubMatrix(Ke, static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ K, false);
+ Matrix33d negativeKe = -Ke;
+ Math::addSubMatrix(negativeKe, static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ K, false);
+ Math::addSubMatrix(negativeKe, static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ K, false);
+ Math::addSubMatrix(Ke, static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ K, false);
}
-void LinearSpring::addFDK(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
- SurgSim::Math::Matrix* D, SurgSim::Math::Matrix* K)
+void LinearSpring::addFDK(const OdeState& state, Vector* F, Math::SparseMatrix* D, Math::SparseMatrix* 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)
+ Matrix33d De, Ke;
+
+ // Assembly stage in F. Note that the force calculation does not rely on any matrices.
+ addForce(state, F);
+
+ // The spring has 2 nodes with positions {x1, x2}, velocities {v1, v2} and force {F1, F2=-F1}
+ // Also note from addForce that the positions and velocities play a symmetric role in the force calculation
+ // i.e. dFi/dx1 = -dFi/dx2 and dFi/dv1 = -dFi/dv2
+ // The stiffness matrix is K = -dF/dx = (-dF1/dx1 -dF1/dx2) = (-dF1/dx1 dF1/dx1)
+ // (-dF2/dx1 -dF2/dx2) ( dF1/dx1 -dF1/dx1)
+ // The damping matrix is D = -dF/dv = (-dF1/dv1 -dF1/dv2) = (-dF1/dv1 dF1/dv1)
+ // (-dF2/dv1 -dF2/dv2) ( dF1/dv1 -dF1/dv1)
+
+ // Let's compute De = -dF1/dv1 and Ke = -dF1/dx1
+ if (!computeDampingAndStiffness(state, &De, &Ke))
{
- 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);
+
+ // Assembly stage in K
+ Math::addSubMatrix(Ke, static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ K, false);
+ Matrix33d negativeKe = -Ke;
+ Math::addSubMatrix(negativeKe, static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ K, false);
+ Math::addSubMatrix(negativeKe, static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ K, false);
+ Math::addSubMatrix(Ke, static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ K, false);
+
+ // Assembly stage in D
+ Math::addSubMatrix(De, static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ D, false);
+ Matrix33d negativeDe = -De;
+ Math::addSubMatrix(negativeDe, static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ D, false);
+ Math::addSubMatrix(negativeDe, static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[0]),
+ D, false);
+ Math::addSubMatrix(De, static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ static_cast<SparseMatrix::Index>(m_nodeIds[1]),
+ D, false);
}
-void LinearSpring::addMatVec(const SurgSim::Math::OdeState& state, double alphaD, double alphaK,
- const SurgSim::Math::Vector& vector, SurgSim::Math::Vector* F)
+void LinearSpring::addMatVec(const OdeState& state, double alphaD, double alphaK, const Vector& vector, Vector* F)
{
// Premature return if both factors are zero
if (alphaK == 0.0 && alphaD == 0.0)
@@ -199,68 +231,83 @@ void LinearSpring::addMatVec(const SurgSim::Math::OdeState& state, double alphaD
return;
}
- // Shared data: the 6D vector to multiply the 6x6 matrix with
- Eigen::Matrix<double, 6, 1> vector6D;
- getSubVector(vector, m_nodeIds, 3, &vector6D);
+ Matrix33d De, Ke;
+ if (!computeDampingAndStiffness(state, (alphaD != 0 ? &De : nullptr), (alphaK != 0 ? &Ke : nullptr)))
+ {
+ return;
+ }
+
+ // Shared data: the 2x 3D vectors to multiply the matrices with
+ const auto& vector1 = vector.segment<3>(3 * m_nodeIds[0]);
+ const auto& vector2 = vector.segment<3>(3 * m_nodeIds[1]);
+
+ if (alphaD != 0.0)
+ {
+ const Vector3d force = alphaD * (De * (vector1 - vector2));
+ F->segment<3>(3 * m_nodeIds[0]) += force;
+ F->segment<3>(3 * m_nodeIds[1]) -= force;
+ }
+
+ if (alphaK != 0.0)
+ {
+ const Vector3d force = alphaK * (Ke * (vector1 - vector2));
+ F->segment<3>(3 * m_nodeIds[0]) += force;
+ F->segment<3>(3 * m_nodeIds[1]) -= force;
+ }
+}
- // 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);
+bool LinearSpring::computeDampingAndStiffness(const OdeState& state, Matrix33d* De, Matrix33d* Ke)
+{
+ const auto& x0 = state.getPositions().segment<3>(3 * m_nodeIds[0]);
+ const auto& x1 = state.getPositions().segment<3>(3 * m_nodeIds[1]);
+ const auto& v0 = state.getVelocities().segment<3>(3 * m_nodeIds[0]);
+ const auto& v1 = state.getVelocities().segment<3>(3 * m_nodeIds[1]);
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;
+ "Spring (initial length = " << m_restLength <<
+ ") became degenerated with 0 length => force derivative degenerated";
+ return false;
}
u /= length;
+ double lRatio = (length - m_restLength) / length;
+ double vRatio = (v1 - v0).dot(u) / length;
+ Matrix33d uuT = u * u.transpose();
- if (alphaD != 0.0)
+ // Update the stiffness matrix
+ if (Ke != nullptr)
{
- 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);
+ *Ke = Matrix33d::Identity() * (m_stiffness * lRatio + m_damping * vRatio);
+ *Ke -= uuT * (m_stiffness * (lRatio - 1.0) + 2.0 * m_damping * vRatio);
+ *Ke += m_damping * (u * (v1 - v0).transpose()) / length;
}
- if (alphaK != 0.0)
+ // Update the damping matrix
+ if (De != nullptr)
{
- 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);
+ *De = m_damping * uuT;
}
+
+ return true;
}
+
bool LinearSpring::operator ==(const Spring& spring) const
{
- const LinearSpring *ls = dynamic_cast<const LinearSpring*>(&spring);
+ 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;
+ 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);
+ return !((*this) == spring);
}
} // namespace Physics
diff --git a/SurgSim/Physics/LinearSpring.h b/SurgSim/Physics/LinearSpring.h
index a474108..cac7d18 100644
--- a/SurgSim/Physics/LinearSpring.h
+++ b/SurgSim/Physics/LinearSpring.h
@@ -32,8 +32,11 @@ public:
/// \param nodeId0, nodeId1 The node ids on which the spring is attached
LinearSpring(size_t nodeId0, size_t nodeId1);
+ void initialize(const SurgSim::Math::OdeState& state) override;
+
/// Sets the spring stiffness parameter
/// \param stiffness The stiffness to assign to the spring (in N.m-1)
+ /// \exception SurgSim::Framework::AssertionFailure stiffness cannot be negative
void setStiffness(double stiffness);
/// Gets the spring stiffness parameter
@@ -42,6 +45,7 @@ public:
/// Sets the spring damping parameter
/// \param damping The damping to assign to the spring (in N.s.m-1)
+ /// \exception SurgSim::Framework::AssertionFailure damping cannot be negative
void setDamping(double damping);
/// Gets the spring damping parameter
@@ -50,6 +54,7 @@ public:
/// Sets the rest length of the spring
/// \param restLength The rest length to assign to the spring (in m)
+ /// \exception SurgSim::Framework::AssertionFailure rest length cannot be negative
void setRestLength(double restLength);
/// Gets the rest length of the spring
@@ -60,24 +65,22 @@ public:
/// \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;
+ 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;
+ void addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::SparseMatrix* 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;
+ void addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::SparseMatrix* 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)
@@ -85,8 +88,8 @@ public:
/// \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;
+ void addFDK(const SurgSim::Math::OdeState& state, SurgSim::Math::Vector* F,
+ SurgSim::Math::SparseMatrix* D, SurgSim::Math::SparseMatrix* 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)
@@ -95,8 +98,8 @@ public:
/// \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;
+ 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
@@ -110,6 +113,18 @@ public:
/// \note Comparison is based on spring type, rest length, stiffness and damping coefficients ONLY
bool operator !=(const Spring& spring) const;
+protected:
+ /// Compute the stiffness matrix Ke = -dF1/dx1 and damping matrix De = -dF1/dv1 of this spring for a given state,
+ /// where this spring is defined by its 2 nodes positions {x1, x2}, velocities {v1, v2} and forces {F1, F2=-F1}.
+ /// \param state The state to compute the jacobians from
+ /// \param [out] De, Ke Respectively the damping and stiffness matrices De and Ke
+ /// \return True if the matrices could be computed, False otherwise (current length is null)
+ /// \note This method calculates only the 3x3 parts related to the force applied on the first node,
+ /// \note derived w.r.t. first node. By nature, we have dF2/dx2 = dF1/dx1 = -dF1/dx2 = -dF2/dx1.
+ bool computeDampingAndStiffness(const SurgSim::Math::OdeState& state,
+ SurgSim::Math::Matrix33d* De,
+ SurgSim::Math::Matrix33d* Ke);
+
private:
/// Rest length (in m)
double m_restLength;
diff --git a/SurgSim/Physics/Localization.cpp b/SurgSim/Physics/Localization.cpp
index 6a19dfc..f180ab0 100644
--- a/SurgSim/Physics/Localization.cpp
+++ b/SurgSim/Physics/Localization.cpp
@@ -14,16 +14,20 @@
// limitations under the License.
#include "SurgSim/Physics/Localization.h"
+
+#include "SurgSim/DataStructures/Location.h"
#include "SurgSim/Physics/Representation.h"
-using SurgSim::Physics::Localization;
-using SurgSim::Physics::Representation;
+namespace SurgSim
+{
+namespace Physics
+{
Localization::Localization()
{
}
-Localization::Localization(std::shared_ptr<Representation> representation) :
-m_representation(representation)
+
+Localization::Localization(std::shared_ptr<Representation> representation) : m_representation(representation)
{
}
@@ -31,8 +35,55 @@ Localization::~Localization()
{
}
-bool SurgSim::Physics::Localization::isValidRepresentation(std::shared_ptr<Representation> representation)
+void Localization::setRepresentation(std::shared_ptr<Representation> representation)
+{
+ if (isValidRepresentation(representation))
+ {
+ m_representation = representation;
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Unexpected representation type" << std::endl;
+ }
+}
+
+std::shared_ptr<Representation> Localization::getRepresentation() const
+{
+ return m_representation;
+}
+
+Math::Vector3d Localization::calculatePosition(double time) const
+{
+ SURGSIM_ASSERT(time >= 0.0 && time <= 1.0) << "Invalid time " << time << " out-of-range [0..1]";
+
+ return doCalculatePosition(time);
+}
+
+Math::Vector3d Localization::calculateVelocity(double time) const
+{
+ SURGSIM_ASSERT(time >= 0.0 && time <= 1.0) << "Invalid time " << time << " out-of-range [0..1]";
+
+ return doCalculateVelocity(time);
+}
+
+bool Localization::isValidRepresentation(std::shared_ptr<Representation> representation)
{
// Localization base class does not care about the type
return true;
}
+
+Math::RigidTransform3d Localization::getElementPose()
+{
+ SURGSIM_FAILURE() << "Localization::getElementPose() is not implemented for " <<
+ getRepresentation()->getFullName();
+ return Math::RigidTransform3d();
+}
+
+bool Localization::moveClosestTo(const Math::Vector3d& point, bool *hasReachedEnd)
+{
+ SURGSIM_FAILURE() << "Localization::moveClosestTo() is not implemented for " << getRepresentation()->getFullName();
+ return false;
+}
+
+}
+}
diff --git a/SurgSim/Physics/Localization.h b/SurgSim/Physics/Localization.h
index 2402cbf..9036d22 100644
--- a/SurgSim/Physics/Localization.h
+++ b/SurgSim/Physics/Localization.h
@@ -19,11 +19,17 @@
#include <memory>
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Framework/Assert.h"
namespace SurgSim
{
+namespace DataStructures
+{
+struct Location;
+}
+
namespace Physics
{
@@ -45,48 +51,58 @@ public:
/// 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;
- }
- }
+ void setRepresentation(std::shared_ptr<Representation> representation);
/// 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;
- }
+ std::shared_ptr<Representation> getRepresentation() const;
/// 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);
- }
+ /// \exception SurgSim::Framework::AssertionFailure if time is out-of-range [0..1]
+ SurgSim::Math::Vector3d calculatePosition(double time = 1.0) const;
+
+ /// Calculates the global velocity of this localization
+ /// \param time The time in [0..1] at which the velocity should be calculated
+ /// \return The global velocity of the localization at the requested time
+ /// \note time can useful when dealing with CCD
+ /// \exception SurgSim::Framework::AssertionFailure if time is out-of-range [0..1]
+ SurgSim::Math::Vector3d calculateVelocity(double time = 1.0) const;
virtual bool isValidRepresentation(std::shared_ptr<Representation> representation);
+ /// Find a pose that the localization is represented with respect to.
+ /// In the case of a rigid representation, the pose of the representation is the pose returned. In case of a fem
+ /// representation, the pose is calculated from the fem element which this localization is a part of.
+ /// \return The pose that the localization is represented with respect to.
+ virtual Math::RigidTransform3d getElementPose();
+
+ /// \param point Move this localization closest to this point
+ /// \param hasReachedEnd [out] Flag to set, when the localization reaches the end of the representation.
+ /// \return Whether the localization was moved or not.
+ virtual bool moveClosestTo(const Math::Vector3d& point, bool *hasReachedEnd);
+
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;
+ virtual SurgSim::Math::Vector3d doCalculatePosition(double time) const = 0;
+
+ /// Calculates the global velocity of this localization
+ /// \param time The time in [0..1] at which the velocity should be calculated
+ /// \return The global velocity of the localization at the requested time
+ /// \note time can useful when dealing with CCD
+ virtual SurgSim::Math::Vector3d doCalculateVelocity(double time) const = 0;
+
/// The representation on which the localization is defined
std::shared_ptr<Representation> m_representation;
};
-}; // namespace Physics
+} // namespace Physics
-}; // namespace SurgSim
+} // namespace SurgSim
-#endif // SURGSIM_PHYSICS_LOCALIZATION_H
+#endif // SURGSIM_PHYSICS_LOCALIZATION_H
diff --git a/SurgSim/Physics/MassSpringConstraintFixedPoint.cpp b/SurgSim/Physics/MassSpringConstraintFixedPoint.cpp
new file mode 100644
index 0000000..3c53c46
--- /dev/null
+++ b/SurgSim/Physics/MassSpringConstraintFixedPoint.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/Physics/LinearSpring.h"
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/MassSpringConstraintFixedPoint.h"
+#include "SurgSim/Physics/MassSpringLocalization.h"
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+MassSpringConstraintFixedPoint::MassSpringConstraintFixedPoint()
+{
+}
+
+MassSpringConstraintFixedPoint::~MassSpringConstraintFixedPoint()
+{
+}
+
+void MassSpringConstraintFixedPoint::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<MassSpringRepresentation> massSpring
+ = std::static_pointer_cast<MassSpringRepresentation>(localization->getRepresentation());
+
+ if (!massSpring->isActive())
+ {
+ return;
+ }
+
+ const double scale = (sign == CONSTRAINT_POSITIVE_SIDE) ? 1.0 : -1.0;
+ size_t nodeId = std::static_pointer_cast<MassSpringLocalization>(localization)->getLocalNode();
+
+ Vector3d globalPosition = localization->calculatePosition();
+
+ // Fixed point constraint in MCLP
+ // n the number of dof in the Mass-Spring
+ // p is the constrained point before free motion
+ // pfree is the constrained point after free motion
+ // vfree is the constrained point's velocity after free motion
+ // v is the velocity in general
+ // u is the position variation needed to enforce the constraint from the free motion
+ // u' is the velocity variation needed to enforce the constraint from the free motion
+ // target is defined as position to be constrained to (in this method, it corresponds to the other part of the
+ // constraint that we don't have access to)
+ //
+ // The constraint equation is
+ // C: pfree + u - target = 0 (note that the constraint is defined with 3 equations, each along an axis X-Y-Z)
+ //
+ // Using backward-Euler integration, the constraint can be expressed on the velocity level:
+ // pfree = p + dt.vfree -> free motion
+ // pfree + u = p + dt.[vfree + u'] -> free motion + constraint correction
+ // => u = dt.u'
+ // C: pfree - target + dt.u' = 0
+ //
+ // Noting the system matrix S, the constraint matrix H = dC/dv is expressed on the velocity level as the
+ // general system to solve is on the velocity level:
+ // (S H^T) ( v ) = (Impulse)
+ // (H 0 ) (-lambda) (initial constraint violation)
+ //
+ // H = dC/dv
+ // The matrix H is of size 3xn (3 equations to fix each axis X-Y-Z relating to the n dof of the model)
+ // The constraint only involves the velocity of a single node (the constrained node), so the matrix H
+ // is full of zero except for a 3x3 part corresponding to the constrained node for which we have:
+ // dC/du' = du/du' = dt.Id(3x3)
+
+ // 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(massSpring->getNumDof());
+ // m_newH is a member variable, so 'reserve' only needs to allocate memory on the first run.
+ m_newH.reserve(3);
+
+ for (size_t axis = 0; axis < 3; axis++)
+ {
+ m_newH.setZero();
+ m_newH.insert(3 * nodeId + axis) = dt * scale;
+ mlcp->updateConstraint(m_newH, massSpring->getComplianceMatrix() * m_newH.transpose(),
+ indexOfRepresentation, indexOfConstraint + axis);
+ }
+}
+
+SurgSim::Physics::ConstraintType MassSpringConstraintFixedPoint::getConstraintType() const
+{
+ return SurgSim::Physics::FIXED_3DPOINT;
+}
+
+size_t MassSpringConstraintFixedPoint::doGetNumDof() const
+{
+ return 3;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/MassSpringConstraintFixedPoint.h b/SurgSim/Physics/MassSpringConstraintFixedPoint.h
new file mode 100644
index 0000000..7332e9e
--- /dev/null
+++ b/SurgSim/Physics/MassSpringConstraintFixedPoint.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_MASSSPRINGCONSTRAINTFIXEDPOINT_H
+#define SURGSIM_PHYSICS_MASSSPRINGCONSTRAINTFIXEDPOINT_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// MassSpringRepresentation bilateral 3d constraint implementation.
+///
+/// The family of FixedPoint constraints enforce equality between two points.
+class MassSpringConstraintFixedPoint : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ MassSpringConstraintFixedPoint();
+
+ /// Destructor
+ virtual ~MassSpringConstraintFixedPoint();
+
+ SurgSim::Physics::ConstraintType getConstraintType() const override;
+
+private:
+ size_t doGetNumDof() const override;
+
+ 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_MASSSPRINGCONSTRAINTFIXEDPOINT_H
diff --git a/SurgSim/Physics/MassSpringConstraintFrictionlessContact.cpp b/SurgSim/Physics/MassSpringConstraintFrictionlessContact.cpp
new file mode 100644
index 0000000..ef6f8e5
--- /dev/null
+++ b/SurgSim/Physics/MassSpringConstraintFrictionlessContact.cpp
@@ -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.
+
+#include <memory>
+
+#include "SurgSim/Physics/MassSpringConstraintFrictionlessContact.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/MassSpringLocalization.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+MassSpringConstraintFrictionlessContact::MassSpringConstraintFrictionlessContact()
+{
+
+}
+
+MassSpringConstraintFrictionlessContact::~MassSpringConstraintFrictionlessContact()
+{
+
+}
+
+void MassSpringConstraintFrictionlessContact::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<MassSpringLocalization>(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();
+
+ // 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
+ // Since the d term will be added to the constraint for one side of the contact and subtracted from the other,
+ // and because it is not clear which distance should be used, we leave it out.
+
+ // Update b with new violation U
+ Vector3d globalPosition = localization->calculatePosition();
+ double violation = n.dot(globalPosition);
+
+ 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() * m_newH.transpose(),
+ indexOfRepresentation, indexOfConstraint);
+}
+
+SurgSim::Physics::ConstraintType MassSpringConstraintFrictionlessContact::getConstraintType() const
+{
+ return SurgSim::Physics::FRICTIONLESS_3DCONTACT;
+}
+
+size_t MassSpringConstraintFrictionlessContact::doGetNumDof() const
+{
+ return 1;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/MassSpringConstraintFrictionlessContact.h b/SurgSim/Physics/MassSpringConstraintFrictionlessContact.h
new file mode 100644
index 0000000..93423cf
--- /dev/null
+++ b/SurgSim/Physics/MassSpringConstraintFrictionlessContact.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_MASSSPRINGCONSTRAINTFRICTIONLESSCONTACT_H
+#define SURGSIM_PHYSICS_MASSSPRINGCONSTRAINTFRICTIONLESSCONTACT_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.
+///
+/// MassSpringConstraintFrictionlessContact implements the frictionless contact constraint for the
+/// MassSpringRepresentation, which prevents nodes from passing through a surface.
+/// See MassSpringConstraintFrictionlessContact::doBuild for more information.
+class MassSpringConstraintFrictionlessContact : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ MassSpringConstraintFrictionlessContact();
+
+ /// Destructor
+ virtual ~MassSpringConstraintFrictionlessContact();
+
+ /// Gets the constraint type for this ConstraintImplementation
+ /// \return The constraint type corresponding to this constraint implementation
+ SurgSim::Physics::ConstraintType getConstraintType() 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).
+ 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).
+ 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_MASSSPRINGCONSTRAINTFRICTIONLESSCONTACT_H
diff --git a/SurgSim/Physics/MassSpringLocalization.cpp b/SurgSim/Physics/MassSpringLocalization.cpp
new file mode 100644
index 0000000..aabe40a
--- /dev/null
+++ b/SurgSim/Physics/MassSpringLocalization.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/Physics/MassSpringLocalization.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+MassSpringLocalization::MassSpringLocalization()
+{
+
+}
+
+MassSpringLocalization::MassSpringLocalization(
+ std::shared_ptr<Representation> representation) :
+ Localization()
+{
+ setRepresentation(representation);
+}
+
+MassSpringLocalization::~MassSpringLocalization()
+{
+
+}
+
+void MassSpringLocalization::setLocalNode(size_t nodeID)
+{
+ m_nodeID = nodeID;
+}
+
+const size_t& MassSpringLocalization::getLocalNode() const
+{
+ return m_nodeID;
+}
+
+SurgSim::Math::Vector3d MassSpringLocalization::doCalculatePosition(double time) const
+{
+ std::shared_ptr<MassSpringRepresentation> massSpringRepresentation =
+ std::static_pointer_cast<MassSpringRepresentation>(getRepresentation());
+
+ SURGSIM_ASSERT(massSpringRepresentation != nullptr) << "MassSpringRepresentation is null, it was probably not" <<
+ " initialized";
+
+ if (time <= std::numeric_limits<double>::epsilon())
+ {
+ return massSpringRepresentation->getPreviousState()->getPosition(m_nodeID);
+ }
+ else if (time >= 1.0 - std::numeric_limits<double>::epsilon())
+ {
+ 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);
+}
+
+SurgSim::Math::Vector3d MassSpringLocalization::doCalculateVelocity(double time) const
+{
+ std::shared_ptr<MassSpringRepresentation> massSpringRepresentation =
+ std::static_pointer_cast<MassSpringRepresentation>(getRepresentation());
+
+ SURGSIM_ASSERT(massSpringRepresentation != nullptr) << "MassSpringRepresentation is null, it was probably not" <<
+ " initialized";
+
+ if (time <= std::numeric_limits<double>::epsilon())
+ {
+ return massSpringRepresentation->getPreviousState()->getVelocity(m_nodeID);
+ }
+ else if (time >= 1.0 - std::numeric_limits<double>::epsilon())
+ {
+ return massSpringRepresentation->getCurrentState()->getVelocity(m_nodeID);
+ }
+
+ const SurgSim::Math::Vector3d& currentPoint = massSpringRepresentation->getCurrentState()->getVelocity(m_nodeID);
+ const SurgSim::Math::Vector3d& previousPoint = massSpringRepresentation->getPreviousState()->getVelocity(m_nodeID);
+
+ return SurgSim::Math::interpolate(previousPoint, currentPoint, time);
+}
+
+bool MassSpringLocalization::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/MassSpringLocalization.h b/SurgSim/Physics/MassSpringLocalization.h
new file mode 100644
index 0000000..2d0e09a
--- /dev/null
+++ b/SurgSim/Physics/MassSpringLocalization.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_MASSSPRINGLOCALIZATION_H
+#define SURGSIM_PHYSICS_MASSSPRINGLOCALIZATION_H
+
+#include "SurgSim/Physics/Localization.h"
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Implementation of Localization for MassSpringRepresentation
+///
+/// MassSpringLocalization 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.
+///
+/// MassSpringLocalization 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 MassSpringLocalization::calculatePosition to find the node's position in global
+/// coordinates in the current state.
+class MassSpringLocalization: public Localization
+{
+public:
+ /// Default constructor
+ MassSpringLocalization();
+
+ /// Constructor
+ /// \param representation The representation to assign to this localization.
+ explicit MassSpringLocalization(std::shared_ptr<Representation> representation);
+
+ /// Destructor
+ virtual ~MassSpringLocalization();
+
+ /// 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.
+ 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.
+ SurgSim::Math::Vector3d doCalculatePosition(double time) const override;
+
+ SurgSim::Math::Vector3d doCalculateVelocity(double time) const override;
+
+ /// Node defining the localization.
+ size_t m_nodeID;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_MASSSPRINGLOCALIZATION_H
diff --git a/SurgSim/Physics/MassSpringRepresentation.cpp b/SurgSim/Physics/MassSpringRepresentation.cpp
index 2a19922..f9b2655 100644
--- a/SurgSim/Physics/MassSpringRepresentation.cpp
+++ b/SurgSim/Physics/MassSpringRepresentation.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -17,11 +17,14 @@
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Math/OdeState.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Physics/MassSpringLocalization.h"
#include "SurgSim/Physics/MassSpringRepresentation.h"
-#include "SurgSim/Physics/MassSpringRepresentationLocalization.h"
+using SurgSim::DataStructures::Location;
using SurgSim::Math::Vector;
using SurgSim::Math::Matrix;
+using SurgSim::Math::SparseMatrix;
namespace SurgSim
{
@@ -44,6 +47,52 @@ MassSpringRepresentation::~MassSpringRepresentation()
{
}
+bool MassSpringRepresentation::doInitialize()
+{
+ // DeformableRepresentation::doInitialize will
+ // 1) assert if initial state is not set
+ // 2) transform m_initialState properly with the initial pose
+ // => Spring::initialize(m_initialState) is using the correct transformed state
+ if (!DeformableRepresentation::doInitialize())
+ {
+ return false;
+ }
+
+ // Initialize the Springs
+ for (auto spring : m_springs)
+ {
+ spring->initialize(*m_initialState);
+ }
+
+ // Precompute the sparsity pattern for the global arrays. M is diagonal, the
+ // rest need to be calculated.
+ m_M.resize(static_cast<SparseMatrix::Index>(getNumDof()), static_cast<SparseMatrix::Index>(getNumDof()));
+ m_D.resize(static_cast<SparseMatrix::Index>(getNumDof()), static_cast<SparseMatrix::Index>(getNumDof()));
+ m_K.resize(static_cast<SparseMatrix::Index>(getNumDof()), static_cast<SparseMatrix::Index>(getNumDof()));
+ for (auto& spring : m_springs)
+ {
+ Math::Matrix block = Math::Matrix::Zero(getNumDofPerNode(),
+ getNumDofPerNode());
+ for (auto nodeId1 : spring->getNodeIds())
+ {
+ for (auto nodeId2 : spring->getNodeIds())
+ {
+ Math::addSubMatrix(block, static_cast<SparseMatrix::Index>(nodeId1),
+ static_cast<SparseMatrix::Index>(nodeId2), &m_D, true);
+ Math::addSubMatrix(block, static_cast<SparseMatrix::Index>(nodeId1),
+ static_cast<SparseMatrix::Index>(nodeId2), &m_K, true);
+ }
+ }
+ }
+ m_M.setIdentity();
+ m_M.makeCompressed();
+ Math::clearMatrix(&m_M);
+ m_D.makeCompressed();
+ m_K.makeCompressed();
+
+ return true;
+}
+
void MassSpringRepresentation::addMass(const std::shared_ptr<Mass> mass)
{
m_masses.push_back(mass);
@@ -106,30 +155,28 @@ 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)
+ const 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);
+ std::shared_ptr<MassSpringLocalization> localization3D =
+ std::dynamic_pointer_cast<MassSpringLocalization>(localization);
SURGSIM_ASSERT(localization3D != nullptr) <<
- "Invalid localization type (not a MassSpringRepresentationLocalization)";
+ "Invalid localization type (not a MassSpringLocalization)";
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() << "}";
+ ". 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;
+ Math::addSubMatrix(K, static_cast<SparseMatrix::Index>(nodeId), static_cast<SparseMatrix::Index>(nodeId),
+ &m_externalGeneralizedStiffness, true);
+ Math::addSubMatrix(D, static_cast<SparseMatrix::Index>(nodeId), static_cast<SparseMatrix::Index>(nodeId),
+ &m_externalGeneralizedDamping, true);
+ m_hasExternalGeneralizedForce = true;
}
void MassSpringRepresentation::beforeUpdate(double dt)
@@ -143,85 +190,66 @@ void MassSpringRepresentation::beforeUpdate(double dt)
}
SURGSIM_ASSERT(3 * getNumMasses() == getNumDof()) <<
- "Mismatch between the number of masses ("<<getNumMasses()<<") and the number of dof ("<<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";
+ SURGSIM_ASSERT(getNumDof()) << "State has not been initialized yet, call setInitialState() " <<
+ "prior to running the simulation";
}
-Vector& MassSpringRepresentation::computeF(const SurgSim::Math::OdeState& state)
+void 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();
+ m_f.setZero(state.getNumDof());
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++)
+ if (m_hasExternalGeneralizedForce)
{
- m_f[*boundaryCondition] = 0.0;
+ m_f += m_externalGeneralizedForce;
}
-
- return m_f;
}
-const Matrix& MassSpringRepresentation::computeM(const SurgSim::Math::OdeState& state)
+void 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();
+ Math::clearMatrix(&m_M);
- Eigen::MatrixBase<Matrix>::DiagonalReturnType diagonal = m_M.diagonal();
-
- for (size_t massId = 0; massId < getNumMasses(); massId++)
+ for (SparseMatrix::Index massId = 0; massId < static_cast<SparseMatrix::Index>(getNumMasses()); massId++)
{
- setSubVector(Vector3d::Ones() * getMass(massId)->getMass(), massId, 3, &diagonal);
+ m_M.coeffRef(3 * massId, 3 * massId) = getMass(massId)->getMass();
+ m_M.coeffRef(3 * massId + 1, 3 * massId + 1) = getMass(massId)->getMass();
+ m_M.coeffRef(3 * massId + 2, 3 * massId + 2) = getMass(massId)->getMass();
}
-
- // 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)
+void 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();
+ Math::clearMatrix(&m_D);
// D += rayleighMass.M
if (rayleighMass != 0.0)
{
- for (size_t massId = 0; massId < getNumMasses(); massId++)
+ for (SparseMatrix::Index massId = 0; massId < static_cast<SparseMatrix::Index>(getNumMasses()); massId++)
{
double coef = rayleighMass * getMass(massId)->getMass();
- Eigen::MatrixBase<Matrix>::DiagonalReturnType Ddiagonal = m_D.diagonal();
- setSubVector(Vector3d::Ones() * coef, massId, 3, &Ddiagonal);
+ m_D.coeffRef(3 * massId, 3 * massId) = coef;
+ m_D.coeffRef(3 * massId + 1, 3 * massId + 1) = coef;
+ m_D.coeffRef(3 * massId + 2, 3 * massId + 2) = coef;
}
}
@@ -241,28 +269,16 @@ const Matrix& MassSpringRepresentation::computeD(const SurgSim::Math::OdeState&
}
// 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++)
+ if (m_hasExternalGeneralizedForce)
{
- m_D.block(*boundaryCondition, 0, 1, getNumDof()).setZero();
- m_D.block(0, *boundaryCondition, getNumDof(), 1).setZero();
- m_D(*boundaryCondition, *boundaryCondition) = 1e9;
+ m_D += m_externalGeneralizedDamping;
}
-
- return m_D;
}
-const Matrix& MassSpringRepresentation::computeK(const SurgSim::Math::OdeState& state)
+void 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();
+ Math::clearMatrix(&m_K);
for (auto spring = std::begin(m_springs); spring != std::end(m_springs); spring++)
{
@@ -270,42 +286,27 @@ const Matrix& MassSpringRepresentation::computeK(const SurgSim::Math::OdeState&
}
// 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++)
+ if (m_hasExternalGeneralizedForce)
{
- m_K.block(*boundaryCondition, 0, 1, getNumDof()).setZero();
- m_K.block(0, *boundaryCondition, getNumDof(), 1).setZero();
- m_K(*boundaryCondition, *boundaryCondition) = 1e9;
+ m_K += m_externalGeneralizedStiffness;
}
-
- return m_K;
}
-void MassSpringRepresentation::computeFMDK(const SurgSim::Math::OdeState& state,
- Vector** f, Matrix** M, Matrix** D, Matrix** K)
+void MassSpringRepresentation::computeFMDK(const SurgSim::Math::OdeState& state)
{
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();
+ m_f.setZero(state.getNumDof());
// Make sure the mass matrix has been properly allocated
- m_M.resize(state.getNumDof(), state.getNumDof());
- m_M.setZero();
+ Math::clearMatrix(&m_M);
// Make sure the damping matrix has been properly allocated and zeroed out
- m_D.resize(state.getNumDof(), state.getNumDof());
- m_D.setZero();
+ Math::clearMatrix(&m_D);
// Make sure the stiffness matrix has been properly allocated and zeroed out
- m_K.resize(state.getNumDof(), state.getNumDof());
- m_K.setZero();
+ Math::clearMatrix(&m_K);
// Computes the mass matrix m_M
computeM(state);
@@ -321,7 +322,11 @@ void MassSpringRepresentation::computeFMDK(const SurgSim::Math::OdeState& state,
// Add the Rayleigh damping matrix
if (m_rayleighDamping.massCoefficient)
{
- m_D.diagonal() += m_M.diagonal() * m_rayleighDamping.massCoefficient;
+ for (SparseMatrix::Index diagonal = 0; diagonal < static_cast<SparseMatrix::Index>(state.getNumDof());
+ ++diagonal)
+ {
+ m_D.coeffRef(diagonal, diagonal) += m_M.coeff(diagonal, diagonal) * m_rayleighDamping.massCoefficient;
+ }
}
if (m_rayleighDamping.stiffnessCoefficient)
{
@@ -335,36 +340,16 @@ void MassSpringRepresentation::computeFMDK(const SurgSim::Math::OdeState& state,
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++)
+ if (m_hasExternalGeneralizedForce)
{
- 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;
+ 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 MassSpringRepresentation::addRayleighDampingForce(Vector* force, const SurgSim::Math::OdeState& state,
- bool useGlobalStiffnessMatrix, bool useGlobalMassMatrix, double scale)
+ bool useGlobalStiffnessMatrix, bool useGlobalMassMatrix, double scale)
{
using SurgSim::Math::getSubVector;
using SurgSim::Math::addSubVector;
@@ -400,7 +385,8 @@ void MassSpringRepresentation::addRayleighDampingForce(Vector* force, const Surg
{
if (useGlobalStiffnessMatrix)
{
- *force -= scale * rayleighStiffness * (m_K * v);
+ Math::Vector tempVector = (scale * rayleighStiffness) * (m_K * v);
+ *force -= tempVector;
}
else
{
@@ -413,7 +399,7 @@ void MassSpringRepresentation::addRayleighDampingForce(Vector* force, const Surg
}
}
-void MassSpringRepresentation::addSpringsForce(Vector *force, const SurgSim::Math::OdeState& state, double scale)
+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++)
{
@@ -421,7 +407,7 @@ void MassSpringRepresentation::addSpringsForce(Vector *force, const SurgSim::Mat
}
}
-void MassSpringRepresentation::addGravityForce(Vector *f, const SurgSim::Math::OdeState& state, double scale)
+void MassSpringRepresentation::addGravityForce(Vector* f, const SurgSim::Math::OdeState& state, double scale)
{
using SurgSim::Math::addSubVector;
@@ -439,7 +425,7 @@ static void transformVectorByBlockOf3(const SurgSim::Math::RigidTransform3d& tra
{
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)";
+ "Unexpected number of dof in a MassSpring state vector (not a multiple of 3)";
for (size_t nodeId = 0; nodeId < numNodes; nodeId++)
{
@@ -458,12 +444,30 @@ static void transformVectorByBlockOf3(const SurgSim::Math::RigidTransform3d& tra
}
void MassSpringRepresentation::transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
- const SurgSim::Math::RigidTransform3d& transform)
+ const SurgSim::Math::RigidTransform3d& transform)
{
transformVectorByBlockOf3(transform, &state->getPositions());
transformVectorByBlockOf3(transform, &state->getVelocities(), true);
}
+std::shared_ptr<Localization> MassSpringRepresentation::createLocalization(
+ const SurgSim::DataStructures::Location& location)
+{
+ auto result = std::make_shared<MassSpringLocalization>(
+ std::static_pointer_cast<Physics::Representation>(getSharedPtr()));
+
+ if (location.index.hasValue())
+ {
+ result->setLocalNode(*location.index);
+ }
+ else
+ {
+ SURGSIM_FAILURE() << "Invalid location to create a MassSpringLocalization" << std::endl;
+ }
+
+ return result;
+}
+
} // namespace Physics
} // namespace SurgSim
diff --git a/SurgSim/Physics/MassSpringRepresentation.h b/SurgSim/Physics/MassSpringRepresentation.h
index bd147a5..0befe29 100644
--- a/SurgSim/Physics/MassSpringRepresentation.h
+++ b/SurgSim/Physics/MassSpringRepresentation.h
@@ -97,54 +97,16 @@ public:
/// \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;
+ void addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ const 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;
+ void beforeUpdate(double dt) override;
+
+ std::shared_ptr<Localization> createLocalization(const SurgSim::DataStructures::Location& location) override;
protected:
/// Add the Rayleigh damping forces
@@ -158,7 +120,8 @@ protected:
/// \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);
+ 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
@@ -171,13 +134,25 @@ protected:
/// \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);
+ 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);
+ const SurgSim::Math::RigidTransform3d& transform);
+
+ bool doInitialize() override;
+
+ void computeF(const SurgSim::Math::OdeState& state) override;
+
+ void computeM(const SurgSim::Math::OdeState& state) override;
+
+ void computeD(const SurgSim::Math::OdeState& state) override;
+
+ void computeK(const SurgSim::Math::OdeState& state) override;
+
+ void computeFMDK(const SurgSim::Math::OdeState& state) override;
private:
/// Masses
@@ -189,7 +164,8 @@ private:
/// Rayleigh damping parameters (massCoefficient and stiffnessCoefficient)
/// D = massCoefficient.M + stiffnessCoefficient.K
/// Matrices: D = damping, M = mass, K = stiffness
- struct {
+ struct
+ {
double massCoefficient;
double stiffnessCoefficient;
} m_rayleighDamping;
diff --git a/SurgSim/Physics/MassSpringRepresentationContact.cpp b/SurgSim/Physics/MassSpringRepresentationContact.cpp
deleted file mode 100644
index 8ab3a97..0000000
--- a/SurgSim/Physics/MassSpringRepresentationContact.cpp
+++ /dev/null
@@ -1,109 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 9ca376f..0000000
--- a/SurgSim/Physics/MassSpringRepresentationContact.h
+++ /dev/null
@@ -1,79 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
deleted file mode 100644
index 1ae8c03..0000000
--- a/SurgSim/Physics/MassSpringRepresentationLocalization.cpp
+++ /dev/null
@@ -1,88 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 2a2b1b9..0000000
--- a/SurgSim/Physics/MassSpringRepresentationLocalization.h
+++ /dev/null
@@ -1,81 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/MlcpPhysicsProblem-inl.h b/SurgSim/Physics/MlcpPhysicsProblem-inl.h
deleted file mode 100644
index cc3df97..0000000
--- a/SurgSim/Physics/MlcpPhysicsProblem-inl.h
+++ /dev/null
@@ -1,57 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
index d309532..739292c 100644
--- a/SurgSim/Physics/MlcpPhysicsProblem.cpp
+++ b/SurgSim/Physics/MlcpPhysicsProblem.cpp
@@ -13,6 +13,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Math/Vector.h"
#include "SurgSim/Physics/MlcpPhysicsProblem.h"
namespace SurgSim
@@ -30,9 +32,7 @@ void MlcpPhysicsProblem::setZero(size_t numDof, size_t numConstraintDof, size_t
MlcpProblem::setZero(numDof, numConstraintDof, numConstraints);
H.resize(numConstraintDof, numDof);
- H.setZero();
- CHt.resize(numDof, numConstraintDof);
- CHt.setZero();
+ CHt.setZero(numDof, numConstraintDof);
}
MlcpPhysicsProblem MlcpPhysicsProblem::Zero(size_t numDof, size_t numConstraintDof, size_t numConstraints)
@@ -43,6 +43,36 @@ MlcpPhysicsProblem MlcpPhysicsProblem::Zero(size_t numDof, size_t numConstraintD
return result;
}
+void MlcpPhysicsProblem::updateConstraint(
+ const Eigen::SparseVector<double, Eigen::RowMajor, ptrdiff_t>& newSubH,
+ const Vector& newCHt,
+ size_t indexSubC,
+ size_t indexNewSubH)
+{
+ // 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];
+
+ A.col(indexNewSubH) += H.middleCols(indexSubC, newCHt.rows()) * newCHt;
+
+ // Calculate: H.block(indexNewSubH, indexSubC, 1, newCHt.rows()) += newSubH;
+ for (Eigen::SparseVector<double, Eigen::RowMajor, ptrdiff_t>::InnerIterator it(newSubH); it; ++it)
+ {
+ H.coeffRef(indexNewSubH, indexSubC + it.index()) += it.value();
+ }
+
+ CHt.block(indexSubC, indexNewSubH, newCHt.rows(), 1) += newCHt;
+ A.row(indexNewSubH) += newSubH * CHt.middleRows(indexSubC, newCHt.rows());
+}
+
}; // namespace Physics
}; // namespace SurgSim
diff --git a/SurgSim/Physics/MlcpPhysicsProblem.h b/SurgSim/Physics/MlcpPhysicsProblem.h
index 3af2e0b..ca7f8b0 100644
--- a/SurgSim/Physics/MlcpPhysicsProblem.h
+++ b/SurgSim/Physics/MlcpPhysicsProblem.h
@@ -17,7 +17,7 @@
#define SURGSIM_PHYSICS_MLCPPHYSICSPROBLEM_H
#include "SurgSim/Math/MlcpProblem.h"
-#include <Eigen/SparseCore>
+#include "SurgSim/Math/SparseMatrix.h"
namespace SurgSim
{
@@ -43,7 +43,7 @@ namespace Physics
struct MlcpPhysicsProblem : public SurgSim::Math::MlcpProblem
{
/// Destructor
- virtual ~MlcpPhysicsProblem() override;
+ ~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
@@ -53,7 +53,7 @@ struct MlcpPhysicsProblem : public SurgSim::Math::MlcpProblem
/// 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;
+ Eigen::SparseMatrix<double, Eigen::RowMajor, ptrdiff_t> 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.
@@ -61,14 +61,12 @@ struct MlcpPhysicsProblem : public SurgSim::Math::MlcpProblem
/// 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 newCHt Compliance matrix (system matrix inverse) times newSubH
/// \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,
+ const Eigen::SparseVector<double, Eigen::RowMajor, ptrdiff_t>& newSubH,
+ const Vector& newCHt,
size_t indexSubC,
size_t indexNewSubH);
@@ -76,7 +74,7 @@ struct MlcpPhysicsProblem : public SurgSim::Math::MlcpProblem
/// \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;
+ 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.
@@ -89,6 +87,4 @@ struct MlcpPhysicsProblem : public SurgSim::Math::MlcpProblem
}; // namespace Physics
}; // namespace SurgSim
-#include "SurgSim/Physics/MlcpPhysicsProblem-inl.h"
-
#endif // SURGSIM_PHYSICS_MLCPPHYSICSPROBLEM_H
diff --git a/SurgSim/Physics/ParticleCollisionResponse.cpp b/SurgSim/Physics/ParticleCollisionResponse.cpp
new file mode 100644
index 0000000..2220e15
--- /dev/null
+++ b/SurgSim/Physics/ParticleCollisionResponse.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 <vector>
+
+#include "SurgSim/Particles/Representation.h"
+#include "SurgSim/Physics/ParticleCollisionResponse.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+ParticleCollisionResponse::ParticleCollisionResponse(bool doCopyState) :
+ Computation(doCopyState)
+{
+}
+
+std::shared_ptr<PhysicsManagerState> ParticleCollisionResponse::doUpdate(const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ std::shared_ptr<PhysicsManagerState> result = state;
+
+ auto& particleRepresentations = result->getActiveParticleRepresentations();
+ for (auto& representation : particleRepresentations)
+ {
+ representation->handleCollisions(dt);
+ }
+
+ return result;
+}
+
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/ParticleCollisionResponse.h b/SurgSim/Physics/ParticleCollisionResponse.h
new file mode 100644
index 0000000..dc60e08
--- /dev/null
+++ b/SurgSim/Physics/ParticleCollisionResponse.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_PARTICLECOLLISIONRESPONSE_H
+#define SURGSIM_PHYSICS_PARTICLECOLLISIONRESPONSE_H
+
+#include <memory>
+
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Physics/Computation.h"
+
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+/// Allows the Particle Representations to respond to collisions
+class ParticleCollisionResponse : public Computation
+{
+public:
+
+ /// Constructor
+ /// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
+ explicit ParticleCollisionResponse(bool doCopyState = false);
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::ParticleCollisionResponse);
+
+protected:
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+ override;
+
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif //SURGSIM_PHYSICS_PARTICLECOLLISIONRESPONSE_H
diff --git a/SurgSim/Physics/PerformanceTests/CMakeLists.txt b/SurgSim/Physics/PerformanceTests/CMakeLists.txt
index b8306ae..b46724f 100644
--- a/SurgSim/Physics/PerformanceTests/CMakeLists.txt
+++ b/SurgSim/Physics/PerformanceTests/CMakeLists.txt
@@ -16,15 +16,17 @@
include_directories(
${gtest_SOURCE_DIR}/include
- ${OSG_INCLUDE_DIR}
${OPENTHREADS_INCLUDE_DIR}
)
set(UNIT_TEST_SOURCES
+ DivisibleCubeRepresentation.cpp
Fem3DPerformanceTest.cpp
+ Fem3DSolutionComponentsTest.cpp
)
set(UNIT_TEST_HEADERS
+ DivisibleCubeRepresentation.h
)
set(LIBS
@@ -32,8 +34,6 @@ set(LIBS
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"
diff --git a/SurgSim/Physics/PerformanceTests/Data/Fem3DPerformanceTest/wound_deformable.ply b/SurgSim/Physics/PerformanceTests/Data/Fem3DPerformanceTest/wound_deformable.ply
deleted file mode 100644
index b4de671..0000000
--- a/SurgSim/Physics/PerformanceTests/Data/Fem3DPerformanceTest/wound_deformable.ply
+++ /dev/null
@@ -1,2389 +0,0 @@
-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/DivisibleCubeRepresentation.cpp b/SurgSim/Physics/PerformanceTests/DivisibleCubeRepresentation.cpp
new file mode 100644
index 0000000..c04d6cb
--- /dev/null
+++ b/SurgSim/Physics/PerformanceTests/DivisibleCubeRepresentation.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/Framework/Assert.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DElementCube.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/PerformanceTests/DivisibleCubeRepresentation.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+DivisibleCubeRepresentation::DivisibleCubeRepresentation(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);
+}
+
+bool DivisibleCubeRepresentation::initializeNoWakeUp()
+{
+ return doInitialize();
+}
+
+std::shared_ptr<SurgSim::Math::OdeSolver> DivisibleCubeRepresentation::getOdeSolver()
+{
+ return m_odeSolver;
+}
+
+/// 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 DivisibleCubeRepresentation::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 DivisibleCubeRepresentation::fillUpDeformableState(std::shared_ptr<SurgSim::Math::OdeState> state)
+{
+ SURGSIM_ASSERT(state != nullptr);
+ 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 DivisibleCubeRepresentation::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);
+ }
+ }
+ }
+}
+
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/PerformanceTests/DivisibleCubeRepresentation.h b/SurgSim/Physics/PerformanceTests/DivisibleCubeRepresentation.h
new file mode 100644
index 0000000..0a6f9a3
--- /dev/null
+++ b/SurgSim/Physics/PerformanceTests/DivisibleCubeRepresentation.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_PERFORMANCETESTS_DIVISIBLECUBEREPRESENTATION_H
+#define SURGSIM_PHYSICS_PERFORMANCETESTS_DIVISIBLECUBEREPRESENTATION_H
+
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DElementCube.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class DivisibleCubeRepresentation : public Fem3DRepresentation
+{
+public:
+ /// Constructor
+ /// \param name The name of the divisible cube representation.
+ /// \param nodesPerAxis The number of nodes per axis
+ DivisibleCubeRepresentation(const std::string& name, size_t nodesPerAxis);
+
+
+ /// Initialize without setting up the complete system so that we can separately
+ /// time the component algorithms.
+ /// \return Initialization success or failure
+ bool initializeNoWakeUp();
+
+ /// Return a pointer to the OdeSolver component
+ /// \return The ODE solver
+ std::shared_ptr<SurgSim::Math::OdeSolver> getOdeSolver();
+
+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);
+
+ /// 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);
+
+ /// Adds the Fem3D elements of small cubes
+ /// \param state The state for initialization.
+ void addFemCubes(std::shared_ptr<SurgSim::Math::OdeState> state);
+
+private:
+ // Number of point per dimensions
+ size_t m_numNodesPerAxis;
+
+ // Corner nodes of the original cube
+ std::array<SurgSim::Math::Vector3d, 8> m_cubeNodes;
+};
+
+} // namespace Physics
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_PERFORMANCETESTS_DIVISIBLECUBEREPRESENTATION_H
diff --git a/SurgSim/Physics/PerformanceTests/Fem3DPerformanceTest.cpp b/SurgSim/Physics/PerformanceTests/Fem3DPerformanceTest.cpp
index 60cd974..484c71a 100644
--- a/SurgSim/Physics/PerformanceTests/Fem3DPerformanceTest.cpp
+++ b/SurgSim/Physics/PerformanceTests/Fem3DPerformanceTest.cpp
@@ -22,11 +22,13 @@
#include "SurgSim/Framework/Assert.h"
#include "SurgSim/Framework/Runtime.h"
#include "SurgSim/Framework/Timer.h"
+#include "SurgSim/Math/LinearSparseSolveAndInverse.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/Physics/PerformanceTests/DivisibleCubeRepresentation.h"
#include "SurgSim/Testing/MockPhysicsManager.h"
using SurgSim::Math::Vector3d;
@@ -34,19 +36,21 @@ using SurgSim::Math::Vector3d;
namespace
{
static const double dt = 0.001;
-static const int frameCount = 100;
+static const double maxIterationConstant = 0.1;
+static const double tolerance = 1.0e-03;
+static const int frameCount = 10;
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_EULER_EXPLICIT);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT_MODIFIED);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT);
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);
@@ -56,8 +60,23 @@ static std::unordered_map<SurgSim::Math::IntegrationScheme, std::string, std::ha
return result;
}
+static std::unordered_map<SurgSim::Math::LinearSolver, std::string, std::hash<int>> getLinearSolverNames()
+{
+ std::unordered_map<SurgSim::Math::LinearSolver, std::string, std::hash<int>> result;
+
+#define FEM3DPERFORMANCETEST_MAP_NAME(map, name) (map)[name] = #name
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::LINEARSOLVER_LU);
+ FEM3DPERFORMANCETEST_MAP_NAME(result, SurgSim::Math::LINEARSOLVER_CONJUGATEGRADIENT);
+#undef FEM3DPERFORMANCETEST_MAP_NAME
+
+ return result;
+}
+
static std::unordered_map<SurgSim::Math::IntegrationScheme, std::string, std::hash<int>> IntegrationSchemeNames
- = getIntegrationSchemeNames();
+ = getIntegrationSchemeNames();
+
+static std::unordered_map<SurgSim::Math::LinearSolver, std::string, std::hash<int>> LinearSolverNames
+ = getLinearSolverNames();
}
namespace SurgSim
@@ -65,153 +84,50 @@ namespace SurgSim
namespace Physics
{
-class DivisbleCubeRepresentation : public Fem3DRepresentation
+class Fem3DPerformanceTestBase : public ::testing::Test
{
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)
+ virtual void SetUp()
{
- return m_numNodesPerAxis * m_numNodesPerAxis * i + m_numNodesPerAxis * j + k;
- }
+ m_physicsManager = std::make_shared<SurgSim::Testing::MockPhysicsManager>();
- /// 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();
+ m_physicsManager->doInitialize();
+ m_physicsManager->doStartUp();
- 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);
- }
- }
- }
+ m_runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
}
- /// Adds the Fem3D elements of small cubes
- /// \param state The state for initialization.
- void addFemCubes(std::shared_ptr<SurgSim::Math::OdeState> state)
+ void initializeRepresentation(std::shared_ptr<Fem3DRepresentation> fem)
{
- for (size_t i = 0; i < m_numNodesPerAxis - 1; i++)
+ fem->initialize(m_runtime);
+ fem->wakeUp();
+ std::shared_ptr<SurgSim::Math::LinearSparseSolveAndInverseCG> solver =
+ std::dynamic_pointer_cast<SurgSim::Math::LinearSparseSolveAndInverseCG>(
+ fem->getOdeSolver()->getLinearSolver());
+ if (solver != nullptr)
{
- 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);
- }
- }
+ solver->setMaxIterations(static_cast<SurgSim::Math::SparseMatrix::Index>(
+ maxIterationConstant * fem->getNumDof()));
+ solver->setTolerance(tolerance);
}
- }
-
-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();
+ m_physicsManager->executeAdditions(fem);
}
- void initializeRepresentation(std::shared_ptr<Fem3DRepresentation> fem)
+ void initializeRepresentation(std::shared_ptr<DivisibleCubeRepresentation> fem)
{
- fem->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ fem->initialize(std::make_shared<SurgSim::Framework::Runtime>("config.txt"));
fem->wakeUp();
+ std::shared_ptr<SurgSim::Math::LinearSparseSolveAndInverseCG> solver =
+ std::dynamic_pointer_cast<SurgSim::Math::LinearSparseSolveAndInverseCG>(
+ fem->getOdeSolver()->getLinearSolver());
+ if (solver != nullptr)
+ {
+ solver->setMaxIterations(static_cast<SurgSim::Math::SparseMatrix::Index>(
+ maxIterationConstant * fem->getNumDof()));
+ solver->setTolerance(tolerance);
+ }
+
m_physicsManager->executeAdditions(fem);
}
@@ -236,27 +152,33 @@ public:
protected:
std::shared_ptr<SurgSim::Testing::MockPhysicsManager> m_physicsManager;
+ std::shared_ptr<SurgSim::Framework::Runtime> m_runtime;
};
class IntegrationSchemeParamTest : public Fem3DPerformanceTestBase,
- public ::testing::WithParamInterface<SurgSim::Math::IntegrationScheme>
+ public ::testing::WithParamInterface<std::tuple<SurgSim::Math::IntegrationScheme, SurgSim::Math::LinearSolver>>
{
};
class IntegrationSchemeAndCountParamTest
: public Fem3DPerformanceTestBase,
- public ::testing::WithParamInterface<std::tuple<SurgSim::Math::IntegrationScheme, int> >
+ public ::testing::WithParamInterface<std::tuple<SurgSim::Math::IntegrationScheme,
+ SurgSim::Math::LinearSolver, int>>
{
};
TEST_P(IntegrationSchemeParamTest, WoundTest)
{
- SurgSim::Math::IntegrationScheme integrationScheme = GetParam();
+ SurgSim::Math::IntegrationScheme integrationScheme;
+ SurgSim::Math::LinearSolver linearSolver;
+ std::tie(integrationScheme, linearSolver) = GetParam();
RecordProperty("IntegrationScheme", IntegrationSchemeNames[integrationScheme]);
+ RecordProperty("LinearSolver", LinearSolverNames[linearSolver]);
auto fem = std::make_shared<SurgSim::Physics::Fem3DRepresentation>("wound");
- fem->setFilename("Data/Fem3DPerformanceTest/wound_deformable.ply");
+ fem->loadFem("Geometry/wound_deformable.ply");
fem->setIntegrationScheme(integrationScheme);
+ fem->setLinearSolver(linearSolver);
initializeRepresentation(fem);
performTimingTest();
@@ -266,12 +188,21 @@ TEST_P(IntegrationSchemeAndCountParamTest, CubeTest)
{
int numCubes;
SurgSim::Math::IntegrationScheme integrationScheme;
- std::tie(integrationScheme, numCubes) = GetParam();
+ SurgSim::Math::LinearSolver linearSolver;
+ std::tie(integrationScheme, linearSolver, numCubes) = GetParam();
RecordProperty("IntegrationScheme", IntegrationSchemeNames[integrationScheme]);
RecordProperty("CubeDivisions", boost::to_string(numCubes));
+ RecordProperty("LinearSolver", LinearSolverNames[linearSolver]);
+
+ auto fem = std::make_shared<DivisibleCubeRepresentation>("cube", numCubes);
+
+ // We need to add some boundary conditions for the static solver to not run into a singular matrix
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(0);
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(1);
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(2);
- auto fem = std::make_shared<DivisbleCubeRepresentation>("cube", numCubes);
fem->setIntegrationScheme(integrationScheme);
+ fem->setLinearSolver(linearSolver);
initializeRepresentation(fem);
performTimingTest();
@@ -279,30 +210,34 @@ TEST_P(IntegrationSchemeAndCountParamTest, CubeTest)
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));
+ ::testing::Combine(::testing::Values(SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT,
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT_MODIFIED,
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT,
+ SurgSim::Math::INTEGRATIONSCHEME_STATIC,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC,
+ SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4),
+ ::testing::Values(SurgSim::Math::LINEARSOLVER_LU,
+ SurgSim::Math::LINEARSOLVER_CONJUGATEGRADIENT)));
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::Combine(::testing::Values(SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT,
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT_MODIFIED,
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT,
+ SurgSim::Math::INTEGRATIONSCHEME_STATIC,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC,
+ SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4),
+ ::testing::Values(SurgSim::Math::LINEARSOLVER_LU,
+ SurgSim::Math::LINEARSOLVER_CONJUGATEGRADIENT),
::testing::Values(2, 3, 4, 5, 6, 7, 8)));
} // namespace Physics
diff --git a/SurgSim/Physics/PerformanceTests/Fem3DSolutionComponentsTest.cpp b/SurgSim/Physics/PerformanceTests/Fem3DSolutionComponentsTest.cpp
new file mode 100644
index 0000000..7adfc1b
--- /dev/null
+++ b/SurgSim/Physics/PerformanceTests/Fem3DSolutionComponentsTest.cpp
@@ -0,0 +1,335 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/LinearSparseSolveAndInverse.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DPlyReaderDelegate.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/Fem3DElementCube.h"
+#include "SurgSim/Physics/PerformanceTests/DivisibleCubeRepresentation.h"
+#include "SurgSim/Testing/MockPhysicsManager.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+static const double dt = 0.001;
+static const double maxIterationConstant = 0.1;
+static const double tolerance = 1.0e-03;
+static const int frameCount = 10;
+
+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 FEM3DSOLUTIONCOMPONEMENTSTEST_MAP_NAME(map, name) (map)[name] = #name
+ FEM3DSOLUTIONCOMPONEMENTSTEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT);
+ FEM3DSOLUTIONCOMPONEMENTSTEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT);
+ FEM3DSOLUTIONCOMPONEMENTSTEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED);
+ FEM3DSOLUTIONCOMPONEMENTSTEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT_MODIFIED);
+ FEM3DSOLUTIONCOMPONEMENTSTEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT);
+ FEM3DSOLUTIONCOMPONEMENTSTEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT);
+ FEM3DSOLUTIONCOMPONEMENTSTEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_STATIC);
+ FEM3DSOLUTIONCOMPONEMENTSTEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC);
+ FEM3DSOLUTIONCOMPONEMENTSTEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4);
+ FEM3DSOLUTIONCOMPONEMENTSTEST_MAP_NAME(result, SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4);
+#undef FEM3DSOLUTIONCOMPONEMENTSTEST_MAP_NAME
+
+ return result;
+}
+
+static std::unordered_map<SurgSim::Math::LinearSolver, std::string, std::hash<int>> getLinearSolverNames()
+{
+ std::unordered_map<SurgSim::Math::LinearSolver, std::string, std::hash<int>> result;
+
+#define FEM3DSOLUTIONCOMPONENTSTEST_MAP_NAME(map, name) (map)[name] = #name
+ FEM3DSOLUTIONCOMPONENTSTEST_MAP_NAME(result, SurgSim::Math::LINEARSOLVER_LU);
+ FEM3DSOLUTIONCOMPONENTSTEST_MAP_NAME(result, SurgSim::Math::LINEARSOLVER_CONJUGATEGRADIENT);
+#undef FEM3DSOLUTIONCOMPONENTSTEST_MAP_NAME
+
+ return result;
+}
+
+static std::unordered_map<SurgSim::Math::IntegrationScheme, std::string, std::hash<int>> IntegrationSchemeNames
+ = getIntegrationSchemeNames();
+
+static std::unordered_map<SurgSim::Math::LinearSolver, std::string, std::hash<int>> LinearSolverNames
+ = getLinearSolverNames();
+}
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class Fem3DSolutionComponentsTestBase : public ::testing::Test
+{
+public:
+ virtual void SetUp()
+ {
+ m_physicsManager = std::make_shared<SurgSim::Testing::MockPhysicsManager>();
+
+ m_physicsManager->doInitialize();
+ m_physicsManager->doStartUp();
+ }
+
+ void timeInitializeRepresentation(std::shared_ptr<DivisibleCubeRepresentation> fem)
+ {
+ SurgSim::Framework::Timer totalTime;
+ totalTime.beginFrame();
+ for (int i = 0; i < frameCount; i++)
+ {
+ fem->initializeNoWakeUp();
+ }
+ totalTime.endFrame();
+ RecordProperty("Duration", boost::to_string(totalTime.getCumulativeTime()));
+ }
+
+ void timeMatrixAssembly(std::shared_ptr<DivisibleCubeRepresentation> fem)
+ {
+ SurgSim::Framework::Timer totalTime;
+ totalTime.beginFrame();
+ for (int i = 0; i < frameCount; i++)
+ {
+ fem->getOdeSolver()->computeMatrices(dt, *(fem->getInitialState()), false);
+ }
+ totalTime.endFrame();
+ RecordProperty("Duration", boost::to_string(totalTime.getCumulativeTime()));
+ }
+
+ void timeComputeLinearSystem(std::shared_ptr<DivisibleCubeRepresentation> fem)
+ {
+ fem->getOdeSolver()->computeMatrices(dt, *(fem->getInitialState()), false);
+ SurgSim::Framework::Timer totalTime;
+ totalTime.beginFrame();
+ for (int i = 0; i < frameCount; i++)
+ {
+ fem->getOdeSolver()->getLinearSolver()->setMatrix(fem->getOdeSolver()->getSystemMatrix());
+ }
+ totalTime.endFrame();
+ RecordProperty("Duration", boost::to_string(totalTime.getCumulativeTime()));
+ }
+
+ void timeSolveSystem(std::shared_ptr<DivisibleCubeRepresentation> fem)
+ {
+ fem->getOdeSolver()->computeMatrices(dt, *(fem->getInitialState()), false);
+ fem->getOdeSolver()->getLinearSolver()->setMatrix(fem->getOdeSolver()->getSystemMatrix());
+ std::shared_ptr<SurgSim::Math::LinearSparseSolveAndInverseCG> solver =
+ std::dynamic_pointer_cast<SurgSim::Math::LinearSparseSolveAndInverseCG>(
+ fem->getOdeSolver()->getLinearSolver());
+ if (solver != nullptr)
+ {
+ solver->setMaxIterations(static_cast<SurgSim::Math::SparseMatrix::Index>(
+ maxIterationConstant * fem->getNumDof()));
+ solver->setTolerance(tolerance);
+ }
+
+ SurgSim::Framework::Timer totalTime;
+ totalTime.beginFrame();
+ for (int i = 0; i < frameCount; i++)
+ {
+ fem->getOdeSolver()->getLinearSolver()->solve(Math::Vector::Ones(fem->getInitialState()->getNumDof()));
+ }
+ totalTime.endFrame();
+ RecordProperty("Duration", boost::to_string(totalTime.getCumulativeTime()));
+ }
+
+ void timeInvertSystem(std::shared_ptr<DivisibleCubeRepresentation> fem)
+ {
+ fem->getOdeSolver()->computeMatrices(dt, *(fem->getInitialState()), false);
+ fem->getOdeSolver()->getLinearSolver()->setMatrix(fem->getOdeSolver()->getSystemMatrix());
+ std::shared_ptr<SurgSim::Math::LinearSparseSolveAndInverseCG> solver =
+ std::dynamic_pointer_cast<SurgSim::Math::LinearSparseSolveAndInverseCG>(
+ fem->getOdeSolver()->getLinearSolver());
+ if (solver != nullptr)
+ {
+ solver->setMaxIterations(static_cast<SurgSim::Math::SparseMatrix::Index>(
+ maxIterationConstant * fem->getNumDof()));
+ solver->setTolerance(tolerance);
+ }
+
+ SurgSim::Framework::Timer totalTime;
+ totalTime.beginFrame();
+ for (int i = 0; i < frameCount; i++)
+ {
+ fem->getOdeSolver()->getLinearSolver()->getInverse();
+ }
+ totalTime.endFrame();
+ RecordProperty("Duration", boost::to_string(totalTime.getCumulativeTime()));
+ }
+
+ void initializeRepresentation(std::shared_ptr<Fem3DRepresentation> fem)
+ {
+ fem->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ fem->wakeUp();
+ m_physicsManager->executeAdditions(fem);
+ }
+
+protected:
+ std::shared_ptr<SurgSim::Testing::MockPhysicsManager> m_physicsManager;
+};
+
+class ComponentIntegrationSchemeAndCountParamTest
+ : public Fem3DSolutionComponentsTestBase,
+ public ::testing::WithParamInterface<std::tuple<SurgSim::Math::IntegrationScheme,
+ SurgSim::Math::LinearSolver, int>>
+{
+};
+
+TEST_P(ComponentIntegrationSchemeAndCountParamTest, TimeMatrixInitialization)
+{
+ int numCubes;
+ SurgSim::Math::IntegrationScheme integrationScheme;
+ SurgSim::Math::LinearSolver linearSolver;
+ std::tie(integrationScheme, linearSolver, numCubes) = GetParam();
+ RecordProperty("IntegrationScheme", IntegrationSchemeNames[integrationScheme]);
+ RecordProperty("LinearSolver", LinearSolverNames[linearSolver]);
+ RecordProperty("CubeDivisions", boost::to_string(numCubes));
+
+ auto fem = std::make_shared<DivisibleCubeRepresentation>("cube", numCubes);
+ // We need to add some boundary conditions for the static solver to not run into a singular matrix
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(0);
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(1);
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(2);
+ fem->setIntegrationScheme(integrationScheme);
+ fem->setLinearSolver(linearSolver);
+
+ timeInitializeRepresentation(fem);
+}
+
+TEST_P(ComponentIntegrationSchemeAndCountParamTest, TimeMatrixAssembly)
+{
+ int numCubes;
+ SurgSim::Math::IntegrationScheme integrationScheme;
+ SurgSim::Math::LinearSolver linearSolver;
+ std::tie(integrationScheme, linearSolver, numCubes) = GetParam();
+ RecordProperty("IntegrationScheme", IntegrationSchemeNames[integrationScheme]);
+ RecordProperty("LinearSolver", LinearSolverNames[linearSolver]);
+ RecordProperty("CubeDivisions", boost::to_string(numCubes));
+
+ auto fem = std::make_shared<DivisibleCubeRepresentation>("cube", numCubes);
+ fem->setIntegrationScheme(integrationScheme);
+ fem->setLinearSolver(linearSolver);
+
+ // We need to add some boundary conditions for the static solver to not run into a singular matrix
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(0);
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(1);
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(2);
+ initializeRepresentation(fem);
+
+ timeMatrixAssembly(fem);
+}
+
+TEST_P(ComponentIntegrationSchemeAndCountParamTest, TimeComputeLinearSystem)
+{
+ int numCubes;
+ SurgSim::Math::IntegrationScheme integrationScheme;
+ SurgSim::Math::LinearSolver linearSolver;
+ std::tie(integrationScheme, linearSolver, numCubes) = GetParam();
+ RecordProperty("IntegrationScheme", IntegrationSchemeNames[integrationScheme]);
+ RecordProperty("LinearSolver", LinearSolverNames[linearSolver]);
+ RecordProperty("CubeDivisions", boost::to_string(numCubes));
+
+ auto fem = std::make_shared<DivisibleCubeRepresentation>("cube", numCubes);
+ fem->setIntegrationScheme(integrationScheme);
+ fem->setLinearSolver(linearSolver);
+
+ // We need to add some boundary conditions for the static solver to not run into a singular matrix
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(0);
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(1);
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(2);
+ initializeRepresentation(fem);
+
+ timeComputeLinearSystem(fem);
+}
+
+TEST_P(ComponentIntegrationSchemeAndCountParamTest, TimeSolveSystem)
+{
+ int numCubes;
+ SurgSim::Math::IntegrationScheme integrationScheme;
+ SurgSim::Math::LinearSolver linearSolver;
+ std::tie(integrationScheme, linearSolver, numCubes) = GetParam();
+ RecordProperty("IntegrationScheme", IntegrationSchemeNames[integrationScheme]);
+ RecordProperty("LinearSolver", LinearSolverNames[linearSolver]);
+ RecordProperty("CubeDivisions", boost::to_string(numCubes));
+
+ auto fem = std::make_shared<DivisibleCubeRepresentation>("cube", numCubes);
+ fem->setIntegrationScheme(integrationScheme);
+ fem->setLinearSolver(linearSolver);
+
+ // We need to add some boundary conditions for the static solver to not run into a singular matrix
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(0);
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(1);
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(2);
+ initializeRepresentation(fem);
+
+ timeSolveSystem(fem);
+}
+
+TEST_P(ComponentIntegrationSchemeAndCountParamTest, TimeInvertSystem)
+{
+ int numCubes;
+ SurgSim::Math::IntegrationScheme integrationScheme;
+ SurgSim::Math::LinearSolver linearSolver;
+ std::tie(integrationScheme, linearSolver, numCubes) = GetParam();
+ RecordProperty("IntegrationScheme", IntegrationSchemeNames[integrationScheme]);
+ RecordProperty("LinearSolver", LinearSolverNames[linearSolver]);
+ RecordProperty("CubeDivisions", boost::to_string(numCubes));
+
+ auto fem = std::make_shared<DivisibleCubeRepresentation>("cube", numCubes);
+ fem->setIntegrationScheme(integrationScheme);
+ fem->setLinearSolver(linearSolver);
+
+ // We need to add some boundary conditions for the static solver to not run into a singular matrix
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(0);
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(1);
+ std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(2);
+ initializeRepresentation(fem);
+
+ timeInvertSystem(fem);
+}
+
+
+INSTANTIATE_TEST_CASE_P(
+ Fem3DSolutionComponentsTest,
+ ComponentIntegrationSchemeAndCountParamTest,
+ ::testing::Combine(::testing::Values(SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT,
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT_MODIFIED,
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT,
+ SurgSim::Math::INTEGRATIONSCHEME_STATIC,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_STATIC,
+ SurgSim::Math::INTEGRATIONSCHEME_RUNGE_KUTTA_4,
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4),
+ ::testing::Values(SurgSim::Math::LINEARSOLVER_LU,
+ SurgSim::Math::LINEARSOLVER_CONJUGATEGRADIENT),
+ ::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
index 9c1cf05..8dc3922 100644
--- a/SurgSim/Physics/PerformanceTests/config.txt.in
+++ b/SurgSim/Physics/PerformanceTests/config.txt.in
@@ -1 +1 @@
-${CMAKE_CURRENT_BINARY_DIR}/Data/
\ No newline at end of file
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data/
diff --git a/SurgSim/Physics/PhysicsConvert.cpp b/SurgSim/Physics/PhysicsConvert.cpp
index cd67227..cafad7b 100644
--- a/SurgSim/Physics/PhysicsConvert.cpp
+++ b/SurgSim/Physics/PhysicsConvert.cpp
@@ -15,12 +15,12 @@
#include "SurgSim/Physics/PhysicsConvert.h"
-#include "SurgSim/Physics/RigidRepresentationState.h"
+#include "SurgSim/Physics/RigidState.h"
namespace YAML
{
-Node convert<SurgSim::Physics::RigidRepresentationState>::encode(const SurgSim::Physics::RigidRepresentationState& rhs)
+Node convert<SurgSim::Physics::RigidState>::encode(const SurgSim::Physics::RigidState& rhs)
{
YAML::Node data(rhs.encode());
@@ -30,9 +30,9 @@ Node convert<SurgSim::Physics::RigidRepresentationState>::encode(const SurgSim::
return result;
}
-bool convert<SurgSim::Physics::RigidRepresentationState>::decode(
+bool convert<SurgSim::Physics::RigidState>::decode(
const Node& node,
- SurgSim::Physics::RigidRepresentationState& rhs)
+ SurgSim::Physics::RigidState& rhs) //NOLINT
{
bool result = false;
if (node[rhs.getClassName()].IsDefined())
diff --git a/SurgSim/Physics/PhysicsConvert.h b/SurgSim/Physics/PhysicsConvert.h
index 1d185e8..8dcad96 100644
--- a/SurgSim/Physics/PhysicsConvert.h
+++ b/SurgSim/Physics/PhysicsConvert.h
@@ -22,7 +22,7 @@ namespace SurgSim
{
namespace Physics
{
-class RigidRepresentationState;
+class RigidState;
}
}
@@ -30,10 +30,10 @@ namespace YAML
{
template <>
-struct convert<SurgSim::Physics::RigidRepresentationState>
+struct convert<SurgSim::Physics::RigidState>
{
- static Node encode(const SurgSim::Physics::RigidRepresentationState& rhs);
- static bool decode(const Node& node, SurgSim::Physics::RigidRepresentationState& rhs);
+ static Node encode(const SurgSim::Physics::RigidState& rhs);
+ static bool decode(const Node& node, SurgSim::Physics::RigidState& rhs); //NOLINT
};
diff --git a/SurgSim/Physics/PhysicsManager.cpp b/SurgSim/Physics/PhysicsManager.cpp
index 0339e49..bd3f423 100644
--- a/SurgSim/Physics/PhysicsManager.cpp
+++ b/SurgSim/Physics/PhysicsManager.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -17,19 +17,24 @@
#include "SurgSim/Framework/Component.h"
#include "SurgSim/Physics/BuildMlcp.h"
+#include "SurgSim/Physics/CcdCollision.h"
+#include "SurgSim/Physics/CcdCollisionLoop.h"
#include "SurgSim/Physics/ConstraintComponent.h"
#include "SurgSim/Physics/ContactConstraintGeneration.h"
+#include "SurgSim/Physics/ContactFiltering.h"
#include "SurgSim/Physics/DcdCollision.h"
#include "SurgSim/Physics/FreeMotion.h"
#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/ParticleCollisionResponse.h"
#include "SurgSim/Physics/PostUpdate.h"
+#include "SurgSim/Physics/PrepareCollisionPairs.h"
#include "SurgSim/Physics/PreUpdate.h"
#include "SurgSim/Physics/PushResults.h"
#include "SurgSim/Physics/Representation.h"
#include "SurgSim/Physics/SolveMlcp.h"
+#include "SurgSim/Physics/UpdateCollisionData.h"
#include "SurgSim/Physics/UpdateCollisionRepresentations.h"
-
-#include <list>
+#include "SurgSim/Physics/UpdateDcdData.h"
namespace SurgSim
{
@@ -54,121 +59,153 @@ int PhysicsManager::getType() const
bool PhysicsManager::doInitialize()
{
- initializeComputations(false);
- return m_logger != nullptr;
-}
-
-
-bool PhysicsManager::doStartUp()
-{
+ if (m_computations.size() == 0)
+ {
+ setComputations(createDcdPipeline(false));
+ }
return true;
}
-void PhysicsManager::getFinalState(SurgSim::Physics::PhysicsManagerState* s) const
+void PhysicsManager::addComputation(std::shared_ptr<SurgSim::Physics::Computation> computation)
{
- m_finalState.get(s);
+ SURGSIM_ASSERT(!isInitialized()) << "Can't reconfigure the physics manager after it has been initialized";
+ m_computations.push_back(computation);
}
-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)
+void PhysicsManager::setComputations(std::vector<std::shared_ptr<Physics::Computation>> computations)
{
- 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);
- });
+ SURGSIM_ASSERT(!isInitialized()) << "Can't reconfigure the physics manager after it has been initialized";
+ m_computations = computations;
}
-void PhysicsManager::addExcludedCollisionPair(std::shared_ptr<SurgSim::Collision::Representation> representation1,
- std::shared_ptr<SurgSim::Collision::Representation> representation2)
+bool PhysicsManager::doStartUp()
{
- 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));
- }
+ return true;
}
-void PhysicsManager::removeExcludedCollisionPair(std::shared_ptr<SurgSim::Collision::Representation> representation1,
- std::shared_ptr<SurgSim::Collision::Representation> representation2)
+void PhysicsManager::getFinalState(SurgSim::Physics::PhysicsManagerState* s) const
{
- boost::mutex::scoped_lock lock(m_excludedCollisionPairMutex);
-
- auto candidatePair = findExcludedCollisionPair(representation1, representation2);
-
- if (candidatePair != m_excludedCollisionPairs.end())
- {
- m_excludedCollisionPairs.erase(candidatePair);
- }
+ m_finalState.get(s);
}
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<Collision::Representation> collisionRep = tryAddComponent(component, &m_collisionRepresentations);
+ std::shared_ptr<Collision::ContactFilter> contactFilter = tryAddComponent(component, &m_contactFilters);
+ std::shared_ptr<Particles::Representation> particles = tryAddComponent(component, &m_particleRepresentations);
std::shared_ptr<ConstraintComponent> constraintComponent = tryAddComponent(component, &m_constraintComponents);
- return representation != nullptr || collisionRep != nullptr || constraintComponent != nullptr;
+
+ return representation != nullptr || collisionRep != nullptr || contactFilter != nullptr ||
+ particles != 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;
+ return tryRemoveComponent(component, &m_representations) ||
+ tryRemoveComponent(component, &m_collisionRepresentations) ||
+ tryRemoveComponent(component, &m_contactFilters) ||
+ tryRemoveComponent(component, &m_constraintComponents) ||
+ tryRemoveComponent(component, &m_particleRepresentations);
}
bool PhysicsManager::doUpdate(double dt)
{
- // Add all components that came in before the last update
- processComponents();
-
processBehaviors(dt);
+ processComponents();
- std::list<std::shared_ptr<PhysicsManagerState>> stateList;
- std::shared_ptr<PhysicsManagerState> state = std::make_shared<PhysicsManagerState>();
- stateList.push_back(state);
+ auto state = std::make_shared<PhysicsManagerState>();
+ std::list<std::shared_ptr<PhysicsManagerState>> stateList(1, state);
state->setRepresentations(m_representations);
state->setCollisionRepresentations(m_collisionRepresentations);
+ state->setContactFilters(m_contactFilters);
+ state->setParticleRepresentations(m_particleRepresentations);
state->setConstraintComponents(m_constraintComponents);
+ for (const auto& computation : m_computations)
{
- boost::mutex::scoped_lock lock(m_excludedCollisionPairMutex);
- state->setExcludedCollisionPairs(m_excludedCollisionPairs);
+ stateList.push_back(computation->update(dt, stateList.back()));
}
- 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()));
+ if (m_logger->getThreshold() <= SURGSIM_LOG_LEVEL(DEBUG))
+ {
+ if (m_computations.front()->getTimer().isBufferFull())
+ {
+ double totalTime = 0.0;
+ for (const auto& computation : m_computations)
+ {
+ totalTime += computation->getTimer().getAverageFramePeriod();
+ }
+ const size_t newFrames = static_cast<size_t>(10.0 / std::max(totalTime, dt));
+ for (const auto& computation : m_computations)
+ {
+ auto& timer = computation->getTimer();
+ const double period = timer.getAverageFramePeriod();
+ SURGSIM_LOG_DEBUG(m_logger)
+ << std::fixed << std::setprecision(0)
+ << computation->getClassName() << " \taverage duration " << 1e6 * period << " us (max "
+ << 1e6 * timer.getMaxFramePeriod() << " us), " << 100.0 * period / totalTime << "% of Physics.";
+ timer.setMaxNumberOfFrames(newFrames);
+ timer.start();
+ }
+ }
+ }
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));
+void PhysicsManager::doBeforeStop()
+{
+ // Empty the physics manager state
+ m_finalState.set(PhysicsManagerState());
+
+ // Give all known components a chance to untangle themselves
+ retireComponents(m_representations);
+ retireComponents(m_particleRepresentations);
+ retireComponents(m_collisionRepresentations);
+ retireComponents(m_constraintComponents);
+
+ // Call up the class hierarchy
+ ComponentManager::doBeforeStop();
+}
+
+std::vector<std::shared_ptr<Physics::Computation>> createDcdPipeline(bool copyState)
+{
+ // When updating this don't forget to update the documentation for this function
+ std::vector<std::shared_ptr<Physics::Computation>> result;
+ result.push_back(std::make_shared<PreUpdate>(copyState));
+ result.push_back(std::make_shared<FreeMotion>(copyState));
+ result.push_back(std::make_shared<UpdateCollisionData>(copyState));
+ result.push_back(std::make_shared<PrepareCollisionPairs>(copyState));
+ result.push_back(std::make_shared<UpdateDcdData>(copyState));
+ result.push_back(std::make_shared<DcdCollision>(copyState));
+ result.push_back(std::make_shared<ContactFiltering>(copyState));
+ result.push_back(std::make_shared<ContactConstraintGeneration>(copyState));
+ result.push_back(std::make_shared<BuildMlcp>(copyState));
+ result.push_back(std::make_shared<SolveMlcp>(copyState));
+ result.push_back(std::make_shared<PushResults>(copyState));
+ result.push_back(std::make_shared<ParticleCollisionResponse>(copyState));
+ result.push_back(std::make_shared<UpdateCollisionRepresentations>(copyState));
+ result.push_back(std::make_shared<PostUpdate>(copyState));
+
+ return result;
+}
+
+std::vector<std::shared_ptr<Physics::Computation>> createCcdPipeline(bool copyState)
+{
+ // When updating this don't forget to update the documentation for this function
+ std::vector<std::shared_ptr<Physics::Computation>> result;
+ result.push_back(std::make_shared<PreUpdate>(copyState));
+ result.push_back(std::make_shared<FreeMotion>(copyState));
+ result.push_back(std::make_shared<UpdateCollisionData>(copyState));
+ result.push_back(std::make_shared<PrepareCollisionPairs>(copyState));
+ result.push_back(std::make_shared<CcdCollisionLoop>(copyState));
+ result.push_back(std::make_shared<UpdateCollisionRepresentations>(copyState));
+ result.push_back(std::make_shared<PostUpdate>(copyState));
+
+ return result;
}
diff --git a/SurgSim/Physics/PhysicsManager.h b/SurgSim/Physics/PhysicsManager.h
index 0d92970..87971e2 100644
--- a/SurgSim/Physics/PhysicsManager.h
+++ b/SurgSim/Physics/PhysicsManager.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -17,6 +17,8 @@
#define SURGSIM_PHYSICS_PHYSICSMANAGER_H
#include <boost/thread/mutex.hpp>
+
+#include <list>
#include <memory>
#include <vector>
@@ -27,33 +29,33 @@
namespace SurgSim
{
+
+namespace Collision
+{
+class CollisionPair;
+class Representation;
+class ContactFilter;
+}
+
namespace Framework
{
class Component;
}
-namespace Collision
+namespace Particles
{
-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;
+class Computation;
/// PhyicsManager handles the physics and motion calculation, it uses Computations to
/// separate the algorithmic steps into smaller pieces.
+/// The PhysicsManager pipeline can be configured, using the
+/// addComputation/setComputaton method.
class PhysicsManager : public SurgSim::Framework::ComponentManager
{
public:
@@ -63,7 +65,7 @@ public:
virtual ~PhysicsManager();
/// Overrides ComponentManager::getType()
- virtual int getType() const override;
+ int getType() const override;
friend class PhysicsManagerTest;
@@ -72,76 +74,59 @@ public:
/// \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);
+ /// Add a computation to the list of computations to perform each update
+ /// \note The computations will be run in order they were added. This can't be done after initialization
+ /// \param computation The Computation to add.
+ void addComputation(std::shared_ptr<SurgSim::Physics::Computation> computation);
- /// 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);
+ /// Set a group of computations, the computations will be run in the order in the vector
+ /// \note this can't be done after initialization
+ /// \param computations The computations that you want to set on the physics manager
+ void setComputations(std::vector<std::shared_ptr<Physics::Computation>> computations);
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);
+
+ /// Initialize the Physics Manager
+ /// Derived class(es) should override this method to have a customized list of computations.
+ /// \note Current default is to only use DCD contacts
+ /// \sa SurgSim::Physics::Computation
+ bool doInitialize() override;
+
+ bool doStartUp() override;
+
+ bool doUpdate(double dt) override;
+
+ void doBeforeStop() override;
std::vector<std::shared_ptr<Representation>> m_representations;
- std::vector<std::shared_ptr<SurgSim::Collision::Representation>> m_collisionRepresentations;
+ std::vector<std::shared_ptr<Collision::Representation>> m_collisionRepresentations;
+
+ std::vector<std::shared_ptr<Collision::ContactFilter>> m_contactFilters;
+
+ std::vector<std::shared_ptr<Particles::Representation>> m_particleRepresentations;
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 list of computations, to perform the physics update.
+ std::vector<std::shared_ptr<SurgSim::Physics::Computation>> m_computations;
/// A thread-safe copy of the last PhysicsManagerState in the previous update.
SurgSim::Framework::LockedContainer<SurgSim::Physics::PhysicsManagerState> m_finalState;
};
+/// Creates default DCD pipeline, this currently does basic DCD without regard to CCD
+/// \param copyState if true the physics manager will maintain a copy of the Physics manager state for each computation
+std::vector<std::shared_ptr<Physics::Computation>> createDcdPipeline(bool copyState = false);
+
+/// Create default CCD pipeline, this currently does basic CCD without regard to DCD
+/// \param copyState if true the physics manager will maintain a copy of the Physics manager state for each computation
+std::vector<std::shared_ptr<Physics::Computation>> createCcdPipeline(bool copyState = false);
}; // namespace Physics
}; // namespace SurgSim
-
-
#endif
diff --git a/SurgSim/Physics/PhysicsManagerState.cpp b/SurgSim/Physics/PhysicsManagerState.cpp
index efccea3..e460022 100644
--- a/SurgSim/Physics/PhysicsManagerState.cpp
+++ b/SurgSim/Physics/PhysicsManagerState.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -26,7 +26,9 @@ namespace SurgSim
namespace Physics
{
-PhysicsManagerState::PhysicsManagerState()
+PhysicsManagerState::PhysicsManagerState() :
+ m_abortGroup(false),
+ m_timeOfImpact(0)
{
}
@@ -70,9 +72,9 @@ const std::vector<std::shared_ptr<Representation>>& PhysicsManagerState::getActi
return m_activeRepresentations;
}
-const std::unordered_map<
- std::shared_ptr<SurgSim::Collision::Representation>,
- std::shared_ptr<SurgSim::Physics::Representation>>&
+const std::unordered_map <
+std::shared_ptr<SurgSim::Collision::Representation>,
+ std::shared_ptr<SurgSim::Physics::Representation >>&
PhysicsManagerState::getCollisionToPhysicsMap() const
{
return m_collisionsToPhysicsMap;
@@ -85,11 +87,44 @@ void PhysicsManagerState::setCollisionRepresentations(
}
const std::vector<std::shared_ptr<SurgSim::Collision::Representation>>&
-PhysicsManagerState::getCollisionRepresentations()
+ PhysicsManagerState::getCollisionRepresentations()
{
return m_collisionRepresentations;
}
+void PhysicsManagerState::setActiveCollisionRepresentations(
+ const std::vector<std::shared_ptr<SurgSim::Collision::Representation>>& val)
+{
+ m_activeCollisionRepresentations = val;
+}
+
+const std::vector<std::shared_ptr<SurgSim::Collision::Representation>>&
+ PhysicsManagerState::getActiveCollisionRepresentations()
+{
+ return m_activeCollisionRepresentations;
+}
+
+void PhysicsManagerState::setParticleRepresentations(const std::vector<std::shared_ptr<Particles::Representation>>& val)
+{
+ m_particleRepresentations = val;
+}
+
+const std::vector<std::shared_ptr<Particles::Representation>>& PhysicsManagerState::getParticleRepresentations()
+{
+ return m_particleRepresentations;
+}
+
+void PhysicsManagerState::setActiveParticleRepresentations(
+ const std::vector<std::shared_ptr<Particles::Representation>>& val)
+{
+ m_activeParticleRepresentations = val;
+}
+
+const std::vector<std::shared_ptr<Particles::Representation>>& PhysicsManagerState::getActiveParticleRepresentations()
+{
+ return m_activeParticleRepresentations;
+}
+
void PhysicsManagerState::setConstraintComponents(const std::vector<std::shared_ptr<ConstraintComponent>>& val)
{
m_constraintComponents = val;
@@ -100,7 +135,7 @@ void PhysicsManagerState::setConstraintComponents(const std::vector<std::shared_
constraints.clear();
for (auto it = m_constraintComponents.cbegin(); it != m_constraintComponents.cend(); ++it)
{
- if ((*it)->getConstraint()->isActive())
+ if ((*it)->isActive() && (*it)->getConstraint()->isActive())
{
constraints.push_back((*it)->getConstraint());
}
@@ -114,7 +149,7 @@ const std::vector<std::shared_ptr<ConstraintComponent>>& PhysicsManagerState::ge
return m_constraintComponents;
}
-void PhysicsManagerState::setCollisionPairs(std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>> val)
+void PhysicsManagerState::setCollisionPairs(const std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>>& val)
{
m_collisionPairs = val;
}
@@ -124,16 +159,14 @@ const std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>>& PhysicsMa
return m_collisionPairs;
}
-void PhysicsManagerState::setExcludedCollisionPairs(
- const std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>>& val)
+void PhysicsManagerState::setContactFilters(const std::vector<std::shared_ptr<SurgSim::Collision::ContactFilter>>& val)
{
- m_excludedCollisionPairs = val;
+ m_contactFilters = val;
}
-const std::vector<std::shared_ptr<SurgSim::Collision::CollisionPair>>&
- PhysicsManagerState::getExcludedCollisionPairs() const
+std::vector<std::shared_ptr<SurgSim::Collision::ContactFilter>> PhysicsManagerState::getContactFilters() const
{
- return m_excludedCollisionPairs;
+ return m_contactFilters;
}
void PhysicsManagerState::setConstraintGroup(
@@ -204,5 +237,25 @@ void PhysicsManagerState::setConstraintsMapping(const MlcpMapping<Constraint>& c
m_constraintsIndexMapping = constraintsMapping;
}
+void PhysicsManagerState::setTimeOfImpact(double timeOfImpact)
+{
+ m_timeOfImpact = timeOfImpact;
+}
+
+double PhysicsManagerState::getTimeOfImpact()
+{
+ return m_timeOfImpact;
+}
+
+bool PhysicsManagerState::shouldAbortGroup() const
+{
+ return m_abortGroup;
+}
+
+void PhysicsManagerState::setAbortGroup(bool val)
+{
+ m_abortGroup = val;
+}
+
}; // Physics
}; // SurgSim
diff --git a/SurgSim/Physics/PhysicsManagerState.h b/SurgSim/Physics/PhysicsManagerState.h
index f29a8d9..1dcc913 100644
--- a/SurgSim/Physics/PhysicsManagerState.h
+++ b/SurgSim/Physics/PhysicsManagerState.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -20,14 +20,16 @@
#include <vector>
#include <unordered_map>
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/ContactFilter.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Particles/Representation.h"
#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
{
@@ -76,6 +78,30 @@ public:
/// \return The collision representations that are known to the state.
const std::vector<std::shared_ptr<SurgSim::Collision::Representation>>& getCollisionRepresentations();
+ /// Sets the active collision representations for the state.
+ /// \param val collection of all active collision representations.
+ void setActiveCollisionRepresentations(const std::vector<std::shared_ptr<SurgSim::Collision::Representation>>& val);
+
+ /// Gets the list of active collision representations.
+ /// \return The active collision representations that are known to the state.
+ const std::vector<std::shared_ptr<SurgSim::Collision::Representation>>& getActiveCollisionRepresentations();
+
+ /// Sets the particle representations for the state.
+ /// \param val collection of all particle representations.
+ void setParticleRepresentations(const std::vector<std::shared_ptr<SurgSim::Particles::Representation>>& val);
+
+ /// Gets the particle representations.
+ /// \return The particle representations that are known to the state.
+ const std::vector<std::shared_ptr<SurgSim::Particles::Representation>>& getParticleRepresentations();
+
+ /// Sets the active particle representations for the state.
+ /// \param val collection of all active particle representations.
+ void setActiveParticleRepresentations(const std::vector<std::shared_ptr<SurgSim::Particles::Representation>>& val);
+
+ /// Gets the list of active particle representations.
+ /// \return The active particle representations that are known to the state.
+ const std::vector<std::shared_ptr<SurgSim::Particles::Representation>>& getActiveParticleRepresentations();
+
/// Sets the list of constraint components
/// \param val collection of all constraint components
void setConstraintComponents(const std::vector<std::shared_ptr<ConstraintComponent>>& val);
@@ -92,19 +118,19 @@ public:
/// 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);
+ void setCollisionPairs(const 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);
+ /// Set the list of contact filters
+ /// \param val the list of contact filters
+ void setContactFilters(const std::vector<std::shared_ptr<SurgSim::Collision::ContactFilter>>& val);
+
+ /// \return the list of contact filters
+ std::vector<std::shared_ptr<SurgSim::Collision::ContactFilter>> getContactFilters() const;
- /// 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.
@@ -158,6 +184,20 @@ public:
/// \param constraintsMapping The constraints mapping (mapping between the constraints and the mlcp)
void setConstraintsMapping(const MlcpMapping<Constraint>& constraintsMapping);
+ /// Set the time of impact
+ /// \param timeOfImpact the time of impact for CCD
+ void setTimeOfImpact(double timeOfImpact);
+
+ /// \return the last time of impact when a CCD calculation ran
+ double getTimeOfImpact();
+
+ /// \return whether to abort a grouped computation by the calling group
+ bool shouldAbortGroup() const;
+
+ /// Set whether to abort the current grouped computation
+ /// \param val set to true to signal to an above computation to abort
+ void setAbortGroup(bool val);
+
private:
///@{
@@ -170,9 +210,21 @@ private:
/// The list of active representations.
std::vector<std::shared_ptr<Representation>> m_activeRepresentations;
- /// List of all the collision representations know to the state
+ /// List of all the collision representations known to the state
std::vector<std::shared_ptr<SurgSim::Collision::Representation>> m_collisionRepresentations;
+ /// List of all the active collision representations known to the state
+ std::vector<std::shared_ptr<SurgSim::Collision::Representation>> m_activeCollisionRepresentations;
+
+ /// List of all the collision filters known to the state
+ std::vector <std::shared_ptr<Collision::ContactFilter>> m_contactFilters;
+
+ /// List of all the particle representations known to the state
+ std::vector<std::shared_ptr<SurgSim::Particles::Representation>> m_particleRepresentations;
+
+ /// List of all the active particle representations known to the state
+ std::vector<std::shared_ptr<SurgSim::Particles::Representation>> m_activeParticleRepresentations;
+
/// List of the constraint components
std::vector<std::shared_ptr<ConstraintComponent>> m_constraintComponents;
@@ -183,9 +235,6 @@ private:
/// 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;
@@ -204,6 +253,12 @@ private:
/// Mlcp solution for this Physics Manager State
MlcpPhysicsSolution m_mlcpPhysicsSolution;
+
+ /// Flag for the abort group case
+ bool m_abortGroup;
+
+ /// last time of impact for a CCD calculation
+ double m_timeOfImpact;
};
}; // Physics
diff --git a/SurgSim/Physics/PostUpdate.cpp b/SurgSim/Physics/PostUpdate.cpp
index 992278c..03f9dea 100644
--- a/SurgSim/Physics/PostUpdate.cpp
+++ b/SurgSim/Physics/PostUpdate.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -40,10 +40,35 @@ std::shared_ptr<PhysicsManagerState> PostUpdate::doUpdate(
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);
+ const auto& representations = result->getActiveRepresentations();
+ for (auto& representation : representations)
+ {
+ representation->afterUpdate(dt);
+ }
+ }
+
+ {
+ // Clear the collisions
+ auto const& representations = result->getCollisionRepresentations();
+
+ for (auto& representation : representations)
+ {
+ representation->getCollisions().unsafeGet().clear();
+ }
+
+ // Update the representations with the contact data.
+ auto& pairs = result->getCollisionPairs();
+ for (auto& pair : pairs)
+ {
+ pair->updateRepresentations();
+ }
+
+ // Publish Results
+ for (auto& representation : representations)
+ {
+ representation->getCollisions().publish();
+ }
}
return result;
diff --git a/SurgSim/Physics/PostUpdate.h b/SurgSim/Physics/PostUpdate.h
index 12c1a45..284b458 100644
--- a/SurgSim/Physics/PostUpdate.h
+++ b/SurgSim/Physics/PostUpdate.h
@@ -18,6 +18,7 @@
#include <memory>
+#include "SurgSim/Framework/Macros.h"
#include "SurgSim/Physics/Computation.h"
namespace SurgSim
@@ -34,14 +35,16 @@ public:
/// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
explicit PostUpdate(bool doCopyState = false);
+ SURGSIM_CLASSNAME(SurgSim::Physics::PostUpdate);
+
/// Destructor
virtual ~PostUpdate();
protected:
/// Override doUpdate from superclass
- virtual std::shared_ptr<PhysicsManagerState>
- doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state) override;
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+ override;
};
}; // Physics
diff --git a/SurgSim/Physics/PreUpdate.cpp b/SurgSim/Physics/PreUpdate.cpp
index 05c0c25..7187c43 100644
--- a/SurgSim/Physics/PreUpdate.cpp
+++ b/SurgSim/Physics/PreUpdate.cpp
@@ -40,10 +40,10 @@ std::shared_ptr<PhysicsManagerState> PreUpdate::doUpdate(
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)
+ auto& representations = result->getActiveRepresentations();
+ for (auto& representation : representations)
{
- (*it)->beforeUpdate(dt);
+ representation->beforeUpdate(dt);
}
return result;
diff --git a/SurgSim/Physics/PreUpdate.h b/SurgSim/Physics/PreUpdate.h
index 8a35917..98e9e22 100644
--- a/SurgSim/Physics/PreUpdate.h
+++ b/SurgSim/Physics/PreUpdate.h
@@ -18,6 +18,7 @@
#include <memory>
+#include "SurgSim/Framework/Macros.h"
#include "SurgSim/Physics/Computation.h"
namespace SurgSim
@@ -33,13 +34,15 @@ public:
/// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
explicit PreUpdate(bool doCopyState = false);
+ SURGSIM_CLASSNAME(SurgSim::Physics::PreUpdate);
+
/// Destructor
virtual ~PreUpdate();
protected:
/// Override doUpdate from superclass
- virtual std::shared_ptr<PhysicsManagerState>
- doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state) override;
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+ override;
};
}; // Physics
diff --git a/SurgSim/Physics/PrepareCollisionPairs.cpp b/SurgSim/Physics/PrepareCollisionPairs.cpp
new file mode 100644
index 0000000..d2bd5f4
--- /dev/null
+++ b/SurgSim/Physics/PrepareCollisionPairs.cpp
@@ -0,0 +1,112 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/PrepareCollisionPairs.h"
+#include "SurgSim/Math/Aabb.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+PrepareCollisionPairs::PrepareCollisionPairs(bool doCopyState) :
+ Computation(doCopyState),
+ m_timeSinceLog(0.0),
+ m_logger(Framework::Logger::getLogger("Physics/PrepareCollisionPairs"))
+{
+}
+
+PrepareCollisionPairs::~PrepareCollisionPairs()
+{
+}
+
+std::shared_ptr<PhysicsManagerState> PrepareCollisionPairs::doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ std::shared_ptr<PhysicsManagerState> result = state;
+ auto& representations = result->getActiveCollisionRepresentations();
+
+ std::vector<std::shared_ptr<Collision::CollisionPair>> pairs;
+
+ auto end = std::end(representations);
+ for (auto first = std::begin(representations); first != end; ++first)
+ {
+ for (auto second = first; second != end; ++second)
+ {
+ if (!(*first)->isIgnoring(*second) && !(*second)->isIgnoring(*first))
+ {
+ auto pair = std::make_shared<Collision::CollisionPair>(*first, *second);
+
+ if (pair->getType() != Collision::COLLISION_DETECTION_TYPE_NONE && pair->mayIntersect())
+ {
+ pairs.push_back(pair);
+ }
+ }
+ }
+ }
+
+ result->setCollisionPairs(pairs);
+
+ if (m_logger->getThreshold() <= SURGSIM_LOG_LEVEL(DEBUG))
+ {
+ m_timeSinceLog += dt;
+ if (m_timeSinceLog > 5.0)
+ {
+ m_timeSinceLog = 0.0;
+ typedef std::tuple<std::string, std::string, Collision::CollisionDetectionType> PairType;
+ std::vector<PairType> names;
+ for (const auto& pair : pairs)
+ {
+ std::string first = pair->getFirst()->getFullName();
+ std::string second = pair->getSecond()->getFullName();
+ if (first < second)
+ {
+ names.emplace_back(first, second, pair->getType());
+ }
+ else
+ {
+ names.emplace_back(second, first, pair->getType());
+ }
+ }
+ std::sort(names.begin(), names.end(), [](const PairType & i, const PairType & j)
+ {
+ return (std::get<0>(i) < std::get<0>(j)) ||
+ ((std::get<0>(i) == std::get<0>(j)) && (std::get<1>(i) < std::get<1>(j)));
+ });
+ std::string message = "All collision pairs for testing:\n";
+ for (const auto& name : names)
+ {
+ message += "\t" + std::get<0>(name) + " : " + std::get<1>(name) +
+ (
+ (std::get<2>(name) == SurgSim::Collision::COLLISION_DETECTION_TYPE_CONTINUOUS) ?
+ " CCD" : " DCD"
+ ) + "\n";
+ }
+ SURGSIM_LOG_DEBUG(m_logger) << message;
+ }
+ }
+
+ return result;
+}
+
+}; // Physics
+}; // SurgSim
diff --git a/SurgSim/Physics/PrepareCollisionPairs.h b/SurgSim/Physics/PrepareCollisionPairs.h
new file mode 100644
index 0000000..bc3638e
--- /dev/null
+++ b/SurgSim/Physics/PrepareCollisionPairs.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_PREPARECOLLISIONPAIRS_H
+#define SURGSIM_PHYSICS_PREPARECOLLISIONPAIRS_H
+
+#include <memory>
+
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Physics/Computation.h"
+
+namespace SurgSim
+{
+
+namespace Collision
+{
+class ContactCalculation;
+}
+
+namespace Framework
+{
+class Logger;
+}
+
+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 When a new ContactCalculation type gets implemented, the type needs to be registered with the table
+/// inside of ContactCalculation
+class PrepareCollisionPairs : public Computation
+{
+public:
+ /// Constructor
+ /// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
+ explicit PrepareCollisionPairs(bool doCopyState = false);
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::PrepareCollisionPairs);
+
+ /// Destructor
+ virtual ~PrepareCollisionPairs();
+
+protected:
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+ override;
+
+private:
+ /// The time since the collision pairs were last logged.
+ double m_timeSinceLog;
+
+ /// The logger.
+ std::shared_ptr<Framework::Logger> m_logger;
+};
+
+}; // Physics
+}; // SurgSim
+
+#endif // SURGSIM_PHYSICS_PREPARECOLLISIONPAIRS_H
diff --git a/SurgSim/Physics/PushResults.cpp b/SurgSim/Physics/PushResults.cpp
index 1930f50..a8d4a9e 100644
--- a/SurgSim/Physics/PushResults.cpp
+++ b/SurgSim/Physics/PushResults.cpp
@@ -24,14 +24,18 @@ namespace SurgSim
namespace Physics
{
-PushResults::PushResults(bool doCopyState) : Computation(doCopyState)
-{}
+PushResults::PushResults(bool doCopyState) :
+ Computation(doCopyState),
+ m_logger(SurgSim::Framework::Logger::getLogger("Physics/PushResults"))
+{
+}
PushResults::~PushResults()
-{}
+{
+}
std::shared_ptr<PhysicsManagerState>
- PushResults::doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+PushResults::doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
{
std::shared_ptr<PhysicsManagerState> result = state;
// 1st step
@@ -46,17 +50,22 @@ std::shared_ptr<PhysicsManagerState>
SurgSim::Math::MlcpSolution::Vector& dofCorrection = result->getMlcpSolution().dofCorrection;
dofCorrection = CHt * lambda;
+ SURGSIM_LOG_DEBUG(m_logger) << "b:\t" << result->getMlcpProblem().b.transpose();
+ SURGSIM_LOG_DEBUG(m_logger) << "final:\t" <<
+ (result->getMlcpProblem().A * lambda + result->getMlcpProblem().b).transpose();
+ SURGSIM_LOG_DEBUG(m_logger) << "Lambda:\t" << lambda.transpose();
+
// 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)
+ auto& representations = result->getActiveRepresentations();
+ for (auto& representation : representations)
{
- if ((*it)->isActive())
+ if (representation->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()));
+ ptrdiff_t index = result->getRepresentationsMapping().getValue(representation.get());
+ SURGSIM_ASSERT(index >= 0) << "Bad index found for representation " << representation->getName()
+ << std::endl;
+ representation->applyCorrection(dt, dofCorrection.segment(index, representation->getNumDof()));
}
}
diff --git a/SurgSim/Physics/PushResults.h b/SurgSim/Physics/PushResults.h
index 1fbb4e6..6505881 100644
--- a/SurgSim/Physics/PushResults.h
+++ b/SurgSim/Physics/PushResults.h
@@ -16,10 +16,16 @@
#ifndef SURGSIM_PHYSICS_PUSHRESULTS_H
#define SURGSIM_PHYSICS_PUSHRESULTS_H
+#include "SurgSim/Framework/Macros.h"
#include "SurgSim/Physics/Computation.h"
namespace SurgSim
{
+namespace Framework
+{
+class Logger;
+}
+
namespace Physics
{
@@ -33,14 +39,18 @@ public:
/// \param doCopyState Specify if the output state in Computation::Update() is a copy or not of the input state
explicit PushResults(bool doCopyState = false);
+ SURGSIM_CLASSNAME(SurgSim::Physics::PushResults);
+
/// Destructor
virtual ~PushResults();
protected:
/// Override doUpdate from superclass
- virtual std::shared_ptr<PhysicsManagerState>
- doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state) override;
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+ override;
+
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
};
}; // namespace Physics
diff --git a/SurgSim/Physics/RenderTests/CMakeLists.txt b/SurgSim/Physics/RenderTests/CMakeLists.txt
index 7292395..ac2cee7 100644
--- a/SurgSim/Physics/RenderTests/CMakeLists.txt
+++ b/SurgSim/Physics/RenderTests/CMakeLists.txt
@@ -16,14 +16,15 @@
include_directories(
${gtest_SOURCE_DIR}/include
- ${OSG_INCLUDE_DIR}
${OPENTHREADS_INCLUDE_DIR}
)
set(UNIT_TEST_SOURCES
+ CompoundCollisionRenderTest.cpp
Fem3DMeshRenderTest.cpp
Fem3DvsTruthCubeRenderTest.cpp
RenderTest.cpp
+ RenderTestCcdSuture.cpp
RenderTestFem1D.cpp
RenderTestFem2D.cpp
RenderTestFem3D.cpp
diff --git a/SurgSim/Physics/RenderTests/CompoundCollisionRenderTest.cpp b/SurgSim/Physics/RenderTests/CompoundCollisionRenderTest.cpp
new file mode 100644
index 0000000..f9eff57
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/CompoundCollisionRenderTest.cpp
@@ -0,0 +1,317 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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 <array>
+
+#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Graphics/OsgBoxRepresentation.h"
+#include "SurgSim/Graphics/OsgPlaneRepresentation.h"
+#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+#include "SurgSim/Graphics/View.h"
+#include "SurgSim/Graphics/ViewElement.h"
+#include "SurgSim/Math/BoxShape.h"
+#include "SurgSim/Math/CompoundShape.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/PlaneShape.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SphereShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/RenderTests/RenderTest.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::RigidTransform3d;
+
+namespace
+{
+std::shared_ptr<SurgSim::Framework::SceneElement> createSphereObject()
+{
+
+ auto result = std::make_shared<SurgSim::Framework::BasicSceneElement>("Object");
+
+ auto physics = std::make_shared<SurgSim::Physics::RigidRepresentation>("Physics");
+ physics->setDensity(700.0); // Wood
+ physics->setLinearDamping(0.1);
+
+ auto sphere = std::make_shared<SurgSim::Math::SphereShape>(0.01); // 1cm Sphere
+ auto shape = std::make_shared<SurgSim::Math::CompoundShape>();
+ physics->setShape(shape);
+ result->addComponent(physics);
+
+ std::array<SurgSim::Math::Vector3d, 6> offsets =
+ {
+ Vector3d(0.1, 0.0, 0.0),
+ Vector3d(-0.1, 0.0, 0.0),
+ Vector3d(0.0, 0.1, 0.0),
+ Vector3d(0.0, -0.1, 0.0),
+ Vector3d(0.0, 0.0, 0.1),
+ Vector3d(0.0, 0.0, -0.1)
+ };
+
+ int i = 0;
+ for (auto offset : offsets)
+ {
+ auto transform = SurgSim::Math::makeRigidTranslation(offset);
+ shape->addShape(sphere, transform);
+
+ auto graphics = std::make_shared<SurgSim::Graphics::OsgSphereRepresentation>("Graphics-" + std::to_string(i));
+ graphics->setRadius(sphere->getRadius());
+
+ graphics->setLocalPose(transform);
+ result->addComponent(graphics);
+
+ ++i;
+ }
+
+ auto rigidCollision = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("Collision");
+ physics->setCollisionRepresentation(rigidCollision);
+ result->addComponent(rigidCollision);
+
+ auto transform = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(0.01, Vector3d(1.0, 1.0, 1.0)),
+ Vector3d(0.0, 1.0, 0.0));
+ result->setPose(transform);
+
+ return result;
+};
+
+std::shared_ptr<SurgSim::Framework::SceneElement> createMeshObject(
+ std::shared_ptr<SurgSim::Math::CompoundShape>* compoundShapeOut = nullptr,
+ std::shared_ptr<SurgSim::Graphics::SceneryRepresentation>* leftGraphicsOut = nullptr,
+ std::shared_ptr<SurgSim::Graphics::SceneryRepresentation>* rightGraphicsOut = nullptr)
+{
+ SurgSim::Framework::ApplicationData data("config.txt");
+
+ auto result = std::make_shared<SurgSim::Framework::BasicSceneElement>("Object");
+
+ auto physics = std::make_shared<SurgSim::Physics::RigidRepresentation>("Physics");
+ physics->setDensity(700.0); // Wood
+ physics->setLinearDamping(0.5);
+
+ auto subShape = std::make_shared<SurgSim::Math::MeshShape>();
+ subShape->load("bar.ply", data);
+ auto shape = std::make_shared<SurgSim::Math::CompoundShape>();
+
+ if (compoundShapeOut != nullptr)
+ {
+ *compoundShapeOut = shape;
+ }
+
+ physics->setShape(shape);
+ result->addComponent(physics);
+
+ auto transform = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(0.2, Vector3d(1.0, 0.0, 0.0)),
+ Vector3d(0.0, 0.0, 0.0));
+ shape->addShape(subShape, transform);
+
+ auto graphics = std::make_shared<SurgSim::Graphics::OsgSceneryRepresentation>("LeftGraphics");
+ graphics->setLocalPose(transform);
+ graphics->loadModel("bar.ply");
+ result->addComponent(graphics);
+
+ if (leftGraphicsOut != nullptr)
+ {
+ *leftGraphicsOut = graphics;
+ }
+
+ transform = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(-0.2, Vector3d(1.0, 0.0, 0.0)),
+ Vector3d(0.0, 0.0, 0.0));
+ shape->addShape(subShape, transform);
+
+ graphics = std::make_shared<SurgSim::Graphics::OsgSceneryRepresentation>("RightGraphics");
+ graphics->setLocalPose(transform);
+ graphics->loadModel("bar.ply");
+ result->addComponent(graphics);
+
+ if (rightGraphicsOut != nullptr)
+ {
+ *rightGraphicsOut = graphics;
+ }
+
+
+
+ auto rigidCollision = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("Collision");
+ physics->setCollisionRepresentation(rigidCollision);
+ result->addComponent(rigidCollision);
+
+ transform = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(0.3, Vector3d(1.0, 1.0, 1.0)),
+ Vector3d(0.0, 1.0, 0.0));
+ result->setPose(transform);
+
+ return result;
+};
+
+std::vector <std::shared_ptr<SurgSim::Framework::SceneElement>> makeScenery()
+{
+ std::vector <std::shared_ptr<SurgSim::Framework::SceneElement>> result;
+
+ {
+ // Plane
+ auto shape = std::make_shared<SurgSim::Math::PlaneShape>();
+ auto physics = std::make_shared<SurgSim::Physics::FixedRepresentation>("Physics");
+ physics->setShape(shape);
+
+ auto graphics = std::make_shared<SurgSim::Graphics::OsgPlaneRepresentation>("Graphics");
+ auto collision = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("Collision");
+ physics->setCollisionRepresentation(collision);
+
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("Plane");
+ element->addComponent(physics);
+ element->addComponent(collision);
+ element->addComponent(graphics);
+ result.push_back(element);
+ }
+
+ {
+ // Sphere
+ auto shape = std::make_shared<SurgSim::Math::SphereShape>(2.0);
+ auto physics = std::make_shared<SurgSim::Physics::FixedRepresentation>("Physics");
+ physics->setShape(shape);
+
+ auto graphics = std::make_shared<SurgSim::Graphics::OsgSphereRepresentation>("Graphics");
+ graphics->setRadius(2.0);
+ auto collision = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("Collision");
+ physics->setCollisionRepresentation(collision);
+
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("BigSphere");
+ element->addComponent(physics);
+ element->addComponent(collision);
+ element->addComponent(graphics);
+
+ auto transform = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(0.01, Vector3d(1.0, 1.0, 1.0)),
+ Vector3d(0.0, -2.0, 0.0));
+ element->setPose(transform);
+ result.push_back(element);
+ }
+
+ {
+ // Mesh Box
+ auto shape = std::make_shared<SurgSim::Math::MeshShape>();
+ shape->load("collider.ply");
+ auto physics = std::make_shared<SurgSim::Physics::FixedRepresentation>("Physics");
+ physics->setDensity(700.0); // Wood
+ physics->setLinearDamping(0.1);
+ physics->setShape(shape);
+
+ auto graphics = std::make_shared<SurgSim::Graphics::OsgSceneryRepresentation>("Graphics");
+ graphics->loadModel("collider.ply");
+ auto collision = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("Collision");
+ physics->setCollisionRepresentation(collision);
+
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("MeshBox");
+ element->addComponent(physics);
+ element->addComponent(collision);
+ element->addComponent(graphics);
+
+ result.push_back(element);
+ }
+
+
+ return result;
+}
+
+}
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+TEST_F(RenderTests, CompoundCollisionShapePlane)
+{
+ scene->addSceneElement(createSphereObject());
+ scene->addSceneElement(makeScenery()[0]);
+
+ runTest(Vector3d(0.8, 0.8, 0.8), Vector3d(0.0, 0.0, 0.0), 5000);
+
+}
+
+TEST_F(RenderTests, CompoundCollisionShapeSphere)
+{
+ scene->addSceneElement(createSphereObject());
+ scene->addSceneElement(makeScenery()[1]);
+
+ runTest(Vector3d(0.8, 0.8, 0.8), Vector3d(0.0, 0.0, 0.0), 5000);
+
+}
+
+TEST_F(RenderTests, CompoundCollsionMeshPlane)
+{
+ scene->addSceneElement(createMeshObject());
+ scene->addSceneElement(makeScenery()[0]);
+
+ runTest(Vector3d(0.8, 0.8, 0.8), Vector3d(0.0, 0.0, 0.0), 5000);
+}
+
+TEST_F(RenderTests, CompoundCollsionMeshBox)
+{
+ scene->addSceneElement(createMeshObject());
+ scene->addSceneElement(makeScenery()[2]);
+
+ runTest(Vector3d(0.8, 0.8, 0.8), Vector3d(0.0, 0.0, 0.0), 5000);
+}
+
+TEST_F(RenderTests, CompoundCollisionMoving)
+{
+ std::shared_ptr<SurgSim::Math::CompoundShape> shape;
+ std::shared_ptr<SurgSim::Graphics::SceneryRepresentation> leftGraphics;
+ std::shared_ptr<SurgSim::Graphics::SceneryRepresentation> rightGraphics;
+
+ auto element = createMeshObject(&shape, &leftGraphics, &rightGraphics);
+
+ auto transform = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(0.0, Vector3d(1.0, 0.0, 0.0)),
+ Vector3d(0.0, 0.5, 0.0));
+ element->setPose(transform);
+ scene->addSceneElement(element);
+ scene->addSceneElement(makeScenery()[2]);
+
+ viewElement->enableManipulator(true);
+ viewElement->setManipulatorParameters(Vector3d(0.8, 0.8, 0.8), Vector3d(0.0, 0.0, 0.0));
+
+ runtime->start();
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
+
+ for (double angle = 0.2; angle < 0.4; angle += 0.0025)
+ {
+ transform = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(angle, Vector3d(1.0, 0.0, 0.0)),
+ Vector3d(0.0, 0.0, 0.0));
+ shape->setPose(0, transform);
+ leftGraphics->setLocalPose(transform);
+
+ transform = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(-angle, Vector3d(1.0, 0.0, 0.0)),
+ Vector3d(0.0, 0.0, 0.0));
+
+ shape->setPose(1, transform);
+ rightGraphics->setLocalPose(transform);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(50));
+ }
+}
+
+}
+}
diff --git a/SurgSim/Physics/RenderTests/Data/bar.ply b/SurgSim/Physics/RenderTests/Data/bar.ply
new file mode 100644
index 0000000..fffa98e
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/Data/bar.ply
@@ -0,0 +1,49 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.75 (sub 0) - www.blender.org, source file: ''
+element vertex 24
+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
+-0.025470 0.035060 0.234386 -1.000000 0.000000 0.000000
+-0.025470 0.035060 -0.265614 -1.000000 0.000000 0.000000
+-0.025470 -0.014940 -0.265614 -1.000000 0.000000 0.000000
+0.024530 0.035060 0.234386 0.000000 1.000000 0.000000
+0.024530 0.035060 -0.265614 0.000000 1.000000 0.000000
+-0.025470 0.035060 -0.265614 0.000000 1.000000 0.000000
+0.024530 -0.014940 0.234386 1.000000 0.000000 -0.000000
+0.024530 -0.014940 -0.265614 1.000000 0.000000 -0.000000
+0.024530 0.035060 -0.265614 1.000000 0.000000 -0.000000
+-0.025470 -0.014940 0.234386 0.000000 -1.000000 0.000000
+-0.025470 -0.014940 -0.265614 0.000000 -1.000000 0.000000
+0.024530 -0.014940 -0.265614 0.000000 -1.000000 0.000000
+-0.025470 0.035060 -0.265614 0.000000 0.000000 -1.000000
+0.024530 0.035060 -0.265614 0.000000 0.000000 -1.000000
+0.024530 -0.014940 -0.265614 0.000000 0.000000 -1.000000
+0.024530 0.035060 0.234386 0.000000 0.000000 1.000000
+-0.025470 0.035060 0.234386 0.000000 0.000000 1.000000
+-0.025470 -0.014940 0.234386 0.000000 0.000000 1.000000
+-0.025470 -0.014940 0.234386 -1.000000 0.000000 0.000000
+-0.025470 0.035060 0.234386 0.000000 1.000000 -0.000000
+0.024530 0.035060 0.234386 1.000000 0.000000 -0.000000
+0.024530 -0.014940 0.234386 0.000000 -1.000000 0.000000
+-0.025470 -0.014940 -0.265614 -0.000000 -0.000000 -1.000000
+0.024530 -0.014940 0.234386 -0.000000 0.000000 1.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 3 5
+3 20 6 8
+3 21 9 11
+3 22 12 14
+3 23 15 17
diff --git a/SurgSim/Physics/RenderTests/Data/collider.ply b/SurgSim/Physics/RenderTests/Data/collider.ply
new file mode 100644
index 0000000..2624bf1
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/Data/collider.ply
@@ -0,0 +1,49 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.75 (sub 0) - www.blender.org, source file: ''
+element vertex 24
+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
+-0.500470 0.060060 0.484386 -1.000000 0.000000 0.000000
+-0.500470 0.060060 -0.515614 -1.000000 0.000000 0.000000
+-0.500470 -0.039940 -0.515614 -1.000000 0.000000 0.000000
+0.499530 0.060060 0.484386 0.000000 1.000000 0.000000
+0.499530 0.060060 -0.515614 0.000000 1.000000 0.000000
+-0.500470 0.060060 -0.515614 0.000000 1.000000 0.000000
+0.499530 -0.039940 0.484386 1.000000 0.000000 -0.000000
+0.499530 -0.039940 -0.515614 1.000000 0.000000 -0.000000
+0.499530 0.060060 -0.515614 1.000000 0.000000 -0.000000
+-0.500470 -0.039940 0.484386 0.000000 -1.000000 0.000000
+-0.500470 -0.039940 -0.515614 0.000000 -1.000000 0.000000
+0.499530 -0.039940 -0.515614 0.000000 -1.000000 0.000000
+-0.500470 0.060060 -0.515614 0.000000 0.000000 -1.000000
+0.499530 0.060060 -0.515614 0.000000 0.000000 -1.000000
+0.499530 -0.039940 -0.515614 0.000000 0.000000 -1.000000
+0.499530 0.060060 0.484386 0.000000 0.000000 1.000000
+-0.500470 0.060060 0.484386 0.000000 0.000000 1.000000
+-0.500470 -0.039940 0.484386 0.000000 0.000000 1.000000
+-0.500470 -0.039940 0.484386 -1.000000 0.000000 0.000000
+-0.500470 0.060060 0.484386 0.000000 1.000000 -0.000000
+0.499530 0.060060 0.484386 1.000000 0.000000 -0.000000
+0.499530 -0.039940 0.484386 0.000000 -1.000000 0.000000
+-0.500470 -0.039940 -0.515614 -0.000000 -0.000000 -1.000000
+0.499530 -0.039940 0.484386 -0.000000 0.000000 1.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 3 5
+3 20 6 8
+3 21 9 11
+3 22 12 14
+3 23 15 17
diff --git a/SurgSim/Physics/RenderTests/Data/cylinder.ply b/SurgSim/Physics/RenderTests/Data/cylinder.ply
new file mode 100644
index 0000000..4248d62
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/Data/cylinder.ply
@@ -0,0 +1,345 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.75 (sub 0) - www.blender.org, source file: ''
+element vertex 204
+property float x
+property float y
+property float z
+property float nx
+property float ny
+property float nz
+element face 128
+property list uchar uint vertex_indices
+end_header
+-0.100000 0.000000 0.000000 -1.000000 0.000000 0.000000
+-0.100000 0.010000 0.000000 -1.000000 0.000000 0.000000
+-0.100000 0.009808 -0.001951 -1.000000 0.000000 0.000000
+0.100000 0.000000 -0.000000 1.000000 0.000000 0.000000
+0.100000 0.009808 -0.001951 1.000000 0.000000 0.000000
+0.100000 0.010000 -0.000000 1.000000 0.000000 0.000000
+0.100000 0.010000 -0.000000 -0.000000 0.995185 -0.098017
+0.100000 0.009808 -0.001951 -0.000000 0.995185 -0.098017
+-0.100000 0.009808 -0.001951 -0.000000 0.995185 -0.098017
+-0.100000 0.009239 -0.003827 -1.000000 0.000000 0.000000
+0.100000 0.009239 -0.003827 1.000000 0.000000 0.000000
+0.100000 0.009808 -0.001951 -0.000000 0.956940 -0.290285
+0.100000 0.009239 -0.003827 -0.000000 0.956940 -0.290285
+-0.100000 0.009239 -0.003827 -0.000000 0.956940 -0.290285
+-0.100000 0.008315 -0.005556 -1.000000 0.000000 0.000000
+0.100000 0.008315 -0.005556 1.000000 0.000000 0.000000
+0.100000 0.009239 -0.003827 -0.000000 0.881921 -0.471396
+0.100000 0.008315 -0.005556 -0.000000 0.881921 -0.471396
+-0.100000 0.008315 -0.005556 -0.000000 0.881921 -0.471396
+-0.100000 0.007071 -0.007071 -1.000000 0.000000 0.000000
+0.100000 0.007071 -0.007071 1.000000 0.000000 0.000000
+0.100000 0.008315 -0.005556 -0.000000 0.773010 -0.634394
+0.100000 0.007071 -0.007071 -0.000000 0.773010 -0.634394
+-0.100000 0.007071 -0.007071 -0.000000 0.773010 -0.634394
+-0.100000 0.005556 -0.008315 -1.000000 0.000000 0.000000
+0.100000 0.005556 -0.008315 1.000000 0.000000 0.000000
+0.100000 0.007071 -0.007071 -0.000000 0.634393 -0.773010
+0.100000 0.005556 -0.008315 -0.000000 0.634393 -0.773010
+-0.100000 0.005556 -0.008315 -0.000000 0.634393 -0.773010
+-0.100000 0.003827 -0.009239 -1.000000 0.000000 0.000000
+0.100000 0.003827 -0.009239 1.000000 0.000000 0.000000
+0.100000 0.005556 -0.008315 -0.000000 0.471396 -0.881921
+0.100000 0.003827 -0.009239 -0.000000 0.471396 -0.881921
+-0.100000 0.003827 -0.009239 -0.000000 0.471396 -0.881921
+-0.100000 0.001951 -0.009808 -1.000000 0.000000 0.000000
+0.100000 0.001951 -0.009808 1.000000 0.000000 0.000000
+0.100000 0.003827 -0.009239 -0.000000 0.290285 -0.956940
+0.100000 0.001951 -0.009808 -0.000000 0.290285 -0.956940
+-0.100000 0.001951 -0.009808 -0.000000 0.290285 -0.956940
+-0.100000 0.000000 -0.010000 -1.000000 0.000000 0.000000
+0.100000 0.000000 -0.010000 1.000000 0.000000 0.000000
+0.100000 0.001951 -0.009808 -0.000000 0.098017 -0.995185
+0.100000 0.000000 -0.010000 -0.000000 0.098017 -0.995185
+-0.100000 0.000000 -0.010000 -0.000000 0.098017 -0.995185
+-0.100000 -0.001951 -0.009808 -1.000000 0.000000 0.000000
+0.100000 -0.001951 -0.009808 1.000000 0.000000 -0.000000
+0.100000 0.000000 -0.010000 -0.000000 -0.098017 -0.995185
+0.100000 -0.001951 -0.009808 -0.000000 -0.098017 -0.995185
+-0.100000 -0.001951 -0.009808 -0.000000 -0.098017 -0.995185
+-0.100000 -0.003827 -0.009239 -1.000000 0.000000 0.000000
+0.100000 -0.003827 -0.009239 1.000000 0.000000 -0.000000
+0.100000 -0.001951 -0.009808 -0.000000 -0.290285 -0.956940
+0.100000 -0.003827 -0.009239 -0.000000 -0.290285 -0.956940
+-0.100000 -0.003827 -0.009239 -0.000000 -0.290285 -0.956940
+-0.100000 -0.005556 -0.008315 -1.000000 0.000000 0.000000
+0.100000 -0.005556 -0.008315 1.000000 0.000000 -0.000000
+0.100000 -0.003827 -0.009239 -0.000000 -0.471396 -0.881921
+0.100000 -0.005556 -0.008315 -0.000000 -0.471396 -0.881921
+-0.100000 -0.005556 -0.008315 -0.000000 -0.471396 -0.881921
+-0.100000 -0.007071 -0.007071 -1.000000 0.000000 0.000000
+0.100000 -0.007071 -0.007071 1.000000 0.000000 -0.000000
+0.100000 -0.005556 -0.008315 -0.000000 -0.634393 -0.773010
+0.100000 -0.007071 -0.007071 -0.000000 -0.634393 -0.773010
+-0.100000 -0.007071 -0.007071 -0.000000 -0.634393 -0.773010
+-0.100000 -0.008315 -0.005556 -1.000000 0.000000 0.000000
+0.100000 -0.008315 -0.005556 1.000000 0.000000 -0.000000
+0.100000 -0.007071 -0.007071 -0.000000 -0.773010 -0.634393
+0.100000 -0.008315 -0.005556 -0.000000 -0.773010 -0.634393
+-0.100000 -0.008315 -0.005556 -0.000000 -0.773010 -0.634393
+-0.100000 -0.009239 -0.003827 -1.000000 0.000000 0.000000
+0.100000 -0.009239 -0.003827 1.000000 0.000000 -0.000000
+0.100000 -0.008315 -0.005556 -0.000000 -0.881921 -0.471397
+0.100000 -0.009239 -0.003827 -0.000000 -0.881921 -0.471397
+-0.100000 -0.009239 -0.003827 -0.000000 -0.881921 -0.471397
+-0.100000 -0.009808 -0.001951 -1.000000 0.000000 0.000000
+0.100000 -0.009808 -0.001951 1.000000 0.000000 -0.000000
+0.100000 -0.009239 -0.003827 -0.000000 -0.956940 -0.290284
+0.100000 -0.009808 -0.001951 -0.000000 -0.956940 -0.290284
+-0.100000 -0.009808 -0.001951 -0.000000 -0.956940 -0.290284
+-0.100000 -0.010000 0.000000 -1.000000 0.000000 0.000000
+0.100000 -0.010000 -0.000000 1.000000 -0.000000 -0.000000
+0.100000 -0.009808 -0.001951 -0.000000 -0.995185 -0.098017
+0.100000 -0.010000 -0.000000 -0.000000 -0.995185 -0.098017
+-0.100000 -0.010000 0.000000 -0.000000 -0.995185 -0.098017
+-0.100000 -0.009808 0.001951 -1.000000 0.000000 -0.000000
+0.100000 -0.009808 0.001951 1.000000 -0.000000 0.000000
+0.100000 -0.010000 -0.000000 0.000000 -0.995185 0.098017
+0.100000 -0.009808 0.001951 0.000000 -0.995185 0.098017
+-0.100000 -0.009808 0.001951 0.000000 -0.995185 0.098017
+-0.100000 -0.009239 0.003827 -1.000000 0.000000 -0.000000
+0.100000 -0.009239 0.003827 1.000000 -0.000000 0.000000
+0.100000 -0.009808 0.001951 0.000000 -0.956940 0.290286
+0.100000 -0.009239 0.003827 0.000000 -0.956940 0.290286
+-0.100000 -0.009239 0.003827 0.000000 -0.956940 0.290286
+-0.100000 -0.008315 0.005556 -1.000000 0.000000 -0.000000
+0.100000 -0.008315 0.005556 1.000000 -0.000000 0.000000
+0.100000 -0.009239 0.003827 0.000000 -0.881921 0.471397
+0.100000 -0.008315 0.005556 0.000000 -0.881921 0.471397
+-0.100000 -0.008315 0.005556 0.000000 -0.881921 0.471397
+-0.100000 -0.007071 0.007071 -1.000000 0.000000 -0.000000
+0.100000 -0.007071 0.007071 1.000000 -0.000000 0.000000
+0.100000 -0.008315 0.005556 0.000000 -0.773010 0.634394
+0.100000 -0.007071 0.007071 0.000000 -0.773010 0.634394
+-0.100000 -0.007071 0.007071 0.000000 -0.773010 0.634394
+-0.100000 -0.005556 0.008315 -1.000000 0.000000 -0.000000
+0.100000 -0.005556 0.008315 1.000000 -0.000000 0.000000
+0.100000 -0.007071 0.007071 0.000000 -0.634392 0.773011
+0.100000 -0.005556 0.008315 0.000000 -0.634392 0.773011
+-0.100000 -0.005556 0.008315 0.000000 -0.634392 0.773011
+-0.100000 -0.003827 0.009239 -1.000000 0.000000 -0.000000
+0.100000 -0.003827 0.009239 1.000000 -0.000000 0.000000
+0.100000 -0.005556 0.008315 0.000000 -0.471396 0.881922
+0.100000 -0.003827 0.009239 0.000000 -0.471396 0.881922
+-0.100000 -0.003827 0.009239 0.000000 -0.471396 0.881922
+-0.100000 -0.001951 0.009808 -1.000000 0.000000 -0.000000
+0.100000 -0.001951 0.009808 1.000000 -0.000000 0.000000
+0.100000 -0.003827 0.009239 0.000000 -0.290284 0.956941
+0.100000 -0.001951 0.009808 0.000000 -0.290284 0.956941
+-0.100000 -0.001951 0.009808 0.000000 -0.290284 0.956941
+-0.100000 0.000000 0.010000 -1.000000 0.000000 -0.000000
+0.100000 0.000000 0.010000 1.000000 -0.000000 0.000000
+0.100000 -0.001951 0.009808 0.000000 -0.098016 0.995185
+0.100000 0.000000 0.010000 0.000000 -0.098016 0.995185
+-0.100000 0.000000 0.010000 0.000000 -0.098016 0.995185
+-0.100000 0.001951 0.009808 -1.000000 -0.000000 0.000000
+0.100000 0.001951 0.009808 1.000000 0.000000 0.000000
+0.100000 0.000000 0.010000 0.000000 0.098018 0.995185
+0.100000 0.001951 0.009808 0.000000 0.098018 0.995185
+-0.100000 0.001951 0.009808 0.000000 0.098018 0.995185
+-0.100000 0.003827 0.009239 -1.000000 -0.000000 0.000000
+0.100000 0.003827 0.009239 1.000000 0.000000 0.000000
+0.100000 0.001951 0.009808 0.000000 0.290286 0.956940
+0.100000 0.003827 0.009239 0.000000 0.290286 0.956940
+-0.100000 0.003827 0.009239 0.000000 0.290286 0.956940
+-0.100000 0.005556 0.008315 -1.000000 -0.000000 0.000000
+0.100000 0.005556 0.008315 1.000000 0.000000 0.000000
+0.100000 0.003827 0.009239 0.000000 0.471397 0.881921
+0.100000 0.005556 0.008315 0.000000 0.471397 0.881921
+-0.100000 0.005556 0.008315 0.000000 0.471397 0.881921
+-0.100000 0.007071 0.007071 -1.000000 -0.000000 0.000000
+0.100000 0.007071 0.007071 1.000000 0.000000 0.000000
+0.100000 0.005556 0.008315 0.000000 0.634394 0.773010
+0.100000 0.007071 0.007071 0.000000 0.634394 0.773010
+-0.100000 0.007071 0.007071 0.000000 0.634394 0.773010
+-0.100000 0.008315 0.005556 -1.000000 -0.000000 0.000000
+0.100000 0.008315 0.005556 1.000000 0.000000 0.000000
+0.100000 0.007071 0.007071 0.000000 0.773011 0.634392
+0.100000 0.008315 0.005556 0.000000 0.773011 0.634392
+-0.100000 0.008315 0.005556 0.000000 0.773011 0.634392
+-0.100000 0.009239 0.003827 -1.000000 -0.000000 0.000000
+0.100000 0.009239 0.003827 1.000000 0.000000 0.000000
+0.100000 0.008315 0.005556 0.000000 0.881922 0.471395
+0.100000 0.009239 0.003827 0.000000 0.881922 0.471395
+-0.100000 0.009239 0.003827 0.000000 0.881922 0.471395
+-0.100000 0.009808 0.001951 -1.000000 -0.000000 0.000000
+0.100000 0.009808 0.001951 1.000000 0.000000 0.000000
+0.100000 0.009239 0.003827 0.000000 0.956941 0.290283
+0.100000 0.009808 0.001951 0.000000 0.956941 0.290283
+-0.100000 0.009808 0.001951 0.000000 0.956941 0.290283
+0.100000 0.009808 0.001951 0.000000 0.995185 0.098017
+0.100000 0.010000 -0.000000 0.000000 0.995185 0.098017
+-0.100000 0.010000 0.000000 0.000000 0.995185 0.098017
+-0.100000 0.010000 0.000000 -0.000000 0.995185 -0.098017
+-0.100000 0.009808 -0.001951 -0.000000 0.956940 -0.290285
+-0.100000 0.009239 -0.003827 -0.000000 0.881921 -0.471396
+-0.100000 0.008315 -0.005556 -0.000000 0.773010 -0.634394
+-0.100000 0.007071 -0.007071 -0.000000 0.634393 -0.773011
+0.100000 0.007071 -0.007071 -0.000000 0.634393 -0.773011
+-0.100000 0.005556 -0.008315 -0.000000 0.634393 -0.773011
+-0.100000 0.005556 -0.008315 -0.000000 0.471396 -0.881921
+-0.100000 0.003827 -0.009239 -0.000000 0.290285 -0.956940
+-0.100000 0.001951 -0.009808 -0.000000 0.098017 -0.995185
+-0.100000 0.000000 -0.010000 -0.000000 -0.098017 -0.995185
+-0.100000 -0.001951 -0.009808 -0.000000 -0.290285 -0.956940
+-0.100000 -0.003827 -0.009239 -0.000000 -0.471396 -0.881921
+-0.100000 -0.005556 -0.008315 -0.000000 -0.634393 -0.773011
+0.100000 -0.005556 -0.008315 -0.000000 -0.634393 -0.773011
+-0.100000 -0.007071 -0.007071 -0.000000 -0.634393 -0.773011
+-0.100000 -0.007071 -0.007071 -0.000000 -0.773010 -0.634393
+-0.100000 -0.008315 -0.005556 -0.000000 -0.881921 -0.471397
+-0.100000 -0.009239 -0.003827 -0.000000 -0.956940 -0.290284
+-0.100000 -0.009808 -0.001951 -0.000000 -0.995185 -0.098017
+-0.100000 -0.010000 0.000000 0.000000 -0.995185 0.098017
+-0.100000 -0.009808 0.001951 0.000000 -0.956940 0.290286
+-0.100000 -0.009239 0.003827 0.000000 -0.881921 0.471397
+-0.100000 -0.008315 0.005556 0.000000 -0.773010 0.634394
+-0.100000 -0.007071 0.007071 0.000000 -0.634393 0.773011
+0.100000 -0.007071 0.007071 0.000000 -0.634393 0.773011
+-0.100000 -0.005556 0.008315 0.000000 -0.634393 0.773011
+-0.100000 -0.005556 0.008315 0.000000 -0.471396 0.881922
+-0.100000 -0.003827 0.009239 0.000000 -0.290284 0.956941
+-0.100000 -0.001951 0.009808 0.000000 -0.098016 0.995185
+-0.100000 0.000000 0.010000 0.000000 0.098018 0.995185
+-0.100000 0.001951 0.009808 0.000000 0.290286 0.956940
+-0.100000 0.003827 0.009239 0.000000 0.471397 0.881921
+-0.100000 0.005556 0.008315 0.000000 0.634395 0.773009
+0.100000 0.005556 0.008315 0.000000 0.634395 0.773009
+-0.100000 0.007071 0.007071 0.000000 0.634395 0.773009
+-0.100000 0.007071 0.007071 0.000000 0.773011 0.634392
+-0.100000 0.008315 0.005556 0.000000 0.881922 0.471396
+0.100000 0.008315 0.005556 0.000000 0.881922 0.471396
+-0.100000 0.009239 0.003827 0.000000 0.881922 0.471396
+-0.100000 0.009239 0.003827 0.000000 0.956941 0.290283
+-0.100000 0.009808 0.001951 0.000000 0.995185 0.098017
+3 0 1 2
+3 3 4 5
+3 6 7 8
+3 0 2 9
+3 3 10 4
+3 11 12 13
+3 0 9 14
+3 3 15 10
+3 16 17 18
+3 0 14 19
+3 3 20 15
+3 21 22 23
+3 0 19 24
+3 3 25 20
+3 26 27 28
+3 0 24 29
+3 3 30 25
+3 31 32 33
+3 0 29 34
+3 3 35 30
+3 36 37 38
+3 0 34 39
+3 3 40 35
+3 41 42 43
+3 0 39 44
+3 3 45 40
+3 46 47 48
+3 0 44 49
+3 3 50 45
+3 51 52 53
+3 0 49 54
+3 3 55 50
+3 56 57 58
+3 0 54 59
+3 3 60 55
+3 61 62 63
+3 0 59 64
+3 3 65 60
+3 66 67 68
+3 0 64 69
+3 3 70 65
+3 71 72 73
+3 0 69 74
+3 3 75 70
+3 76 77 78
+3 0 74 79
+3 3 80 75
+3 81 82 83
+3 0 79 84
+3 3 85 80
+3 86 87 88
+3 0 84 89
+3 3 90 85
+3 91 92 93
+3 0 89 94
+3 3 95 90
+3 96 97 98
+3 0 94 99
+3 3 100 95
+3 101 102 103
+3 0 99 104
+3 3 105 100
+3 106 107 108
+3 0 104 109
+3 3 110 105
+3 111 112 113
+3 0 109 114
+3 3 115 110
+3 116 117 118
+3 0 114 119
+3 3 120 115
+3 121 122 123
+3 0 119 124
+3 3 125 120
+3 126 127 128
+3 0 124 129
+3 3 130 125
+3 131 132 133
+3 0 129 134
+3 3 135 130
+3 136 137 138
+3 0 134 139
+3 3 140 135
+3 141 142 143
+3 0 139 144
+3 3 145 140
+3 146 147 148
+3 0 144 149
+3 3 150 145
+3 151 152 153
+3 0 149 154
+3 3 155 150
+3 156 157 158
+3 0 154 1
+3 3 5 155
+3 159 160 161
+3 162 6 8
+3 163 11 13
+3 164 16 18
+3 165 21 23
+3 166 167 168
+3 169 31 33
+3 170 36 38
+3 171 41 43
+3 172 46 48
+3 173 51 53
+3 174 56 58
+3 175 176 177
+3 178 66 68
+3 179 71 73
+3 180 76 78
+3 181 81 83
+3 182 86 88
+3 183 91 93
+3 184 96 98
+3 185 101 103
+3 186 187 188
+3 189 111 113
+3 190 116 118
+3 191 121 123
+3 192 126 128
+3 193 131 133
+3 194 136 138
+3 195 196 197
+3 198 146 148
+3 199 200 201
+3 202 156 158
+3 203 159 161
diff --git a/SurgSim/Physics/RenderTests/Data/half_knot.ply b/SurgSim/Physics/RenderTests/Data/half_knot.ply
new file mode 100644
index 0000000..7c38a71
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/Data/half_knot.ply
@@ -0,0 +1,158 @@
+ply
+format ascii 1.0
+comment Prolene 3.0
+comment https://en.wikipedia.org/wiki/Prolene
+comment Prolene is a synthetic, monofilament, nonabsorbable polypropylene suture
+comment https://en.wikipedia.org/wiki/Surgical_suture
+comment Surgical suture sizes are often refered in USP unit
+comment This model is a 3.0 USP (0.2mm diameter)
+comment https://en.wikipedia.org/wiki/Young's_modulus
+comment Young modulus for polypropylene is between 1.5-2 GPa
+comment https://en.wikipedia.org/wiki/Polypropylene
+comment Mass density for polypropylene is between 855 and 946 Kg.m-3
+comment http://www.vinidex.com.au/technical/material-properties/polypropylene-properties/
+comment Poisson ratio for polypropylene is 0.45
+element vertex 63
+property double x
+property double y
+property double z
+element 1d_element 62
+property list uint uint vertex_indices
+element radius 1
+property double value
+element material 1
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+element boundary_condition 2
+property uint vertex_index
+end_header
+-0.063988 0.035971 0.001502
+-0.062314 0.035845 0.001502
+-0.058927 0.035774 0.001502
+-0.054140 0.034099 0.001500
+-0.049371 0.032492 0.001508
+-0.044793 0.030356 0.001482
+-0.040472 0.027735 0.001572
+-0.036726 0.024439 0.002441
+-0.032934 0.021236 0.003300
+-0.028799 0.018268 0.003098
+-0.024722 0.015856 0.001294
+-0.020714 0.013729 -0.000962
+-0.016169 0.011300 -0.001046
+-0.011773 0.009644 0.001057
+-0.007495 0.010778 0.003732
+-0.003107 0.013450 0.004363
+0.001183 0.015271 0.002129
+0.005421 0.014709 -0.000760
+0.009751 0.011748 -0.001268
+0.013047 0.008401 0.000717
+0.015779 0.004124 0.001146
+0.018265 -0.000243 0.001458
+0.020098 -0.004962 0.001517
+0.021334 -0.009870 0.001496
+0.021239 -0.014952 0.001504
+0.020081 -0.019876 0.001501
+0.018139 -0.024544 0.001502
+0.015443 -0.028836 0.001502
+0.011752 -0.032312 0.001502
+0.007423 -0.034932 0.001502
+0.002647 -0.036596 0.001502
+-0.002288 -0.037628 0.001502
+-0.007369 -0.037620 0.001502
+-0.012198 -0.036069 0.001502
+-0.016676 -0.033751 0.001502
+-0.020932 -0.031026 0.001502
+-0.024359 -0.027261 0.001502
+-0.026705 -0.022787 0.001502
+-0.028472 -0.018039 0.001500
+-0.029025 -0.012994 0.001507
+-0.028477 -0.007962 0.001431
+-0.026953 -0.003131 0.001173
+-0.025412 0.001636 0.001012
+-0.023105 0.006241 0.000991
+-0.020773 0.010135 0.003309
+-0.016732 0.013315 0.004273
+-0.012269 0.015101 0.002516
+-0.007868 0.014977 -0.000180
+-0.003604 0.012248 -0.001343
+0.000726 0.009916 0.000484
+0.004992 0.010415 0.003128
+0.009576 0.012768 0.004503
+0.013512 0.015055 0.002167
+0.017607 0.017447 0.000416
+0.021803 0.020336 0.000200
+0.025923 0.023100 0.001042
+0.030292 0.025627 0.001485
+0.035002 0.027418 0.001534
+0.039908 0.028661 0.001477
+0.044947 0.028927 0.001517
+0.049973 0.029365 0.001497
+0.053335 0.029135 0.001502
+0.055012 0.029068 0.001502
+2 0 1
+2 1 2
+2 2 3
+2 3 4
+2 4 5
+2 5 6
+2 6 7
+2 7 8
+2 8 9
+2 9 10
+2 10 11
+2 11 12
+2 12 13
+2 13 14
+2 14 15
+2 15 16
+2 16 17
+2 17 18
+2 18 19
+2 19 20
+2 20 21
+2 21 22
+2 22 23
+2 23 24
+2 24 25
+2 25 26
+2 26 27
+2 27 28
+2 28 29
+2 29 30
+2 30 31
+2 31 32
+2 32 33
+2 33 34
+2 34 35
+2 35 36
+2 36 37
+2 37 38
+2 38 39
+2 39 40
+2 40 41
+2 41 42
+2 42 43
+2 43 44
+2 44 45
+2 45 46
+2 46 47
+2 47 48
+2 48 49
+2 49 50
+2 50 51
+2 51 52
+2 52 53
+2 53 54
+2 54 55
+2 55 56
+2 56 57
+2 57 58
+2 58 59
+2 59 60
+2 60 61
+2 61 62
+0.0001
+900.0 0.48 1.75e7
+0
+62
diff --git a/SurgSim/Physics/RenderTests/Data/loop.ply b/SurgSim/Physics/RenderTests/Data/loop.ply
new file mode 100644
index 0000000..d13b182
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/Data/loop.ply
@@ -0,0 +1,101 @@
+ply
+format ascii 1.0
+comment Created by OpenSurgSim, www.opensurgsim.org
+element vertex 40
+property float x
+property float y
+property float z
+element 1d_element 39
+property list uint uint vertex_indices
+element radius 1
+property double value
+element material 1
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+element boundary_condition 2
+property uint vertex_index
+end_header
+-0.2 0.095 0.2
+-0.18192 0.094096 0.18192
+-0.16096 0.093048 0.16096
+-0.13904 0.091952 0.13904
+-0.11808 0.090904 0.11808
+-0.1 0.09 0.1
+-0.08192 0.089096 0.08192
+-0.06096 0.088048 0.06096
+-0.03904 0.086952 0.03904
+-0.01808 0.085904 0.01808
+0 0.085 0
+0.02064 0.084096 -0.01808
+0.04672 0.083048 -0.03904
+0.07248 0.081952 -0.06096
+0.09216 0.080904 -0.08192
+0.1 0.08 -0.1
+0.09216 0.079096 -0.12064
+0.07248 0.078048 -0.14672
+0.04672 0.076952 -0.17248
+0.02064 0.075904 -0.19216
+0 0.075 -0.2
+-0.02064 0.074096 -0.19216
+-0.04672 0.073048 -0.17248
+-0.07248 0.071952 -0.14672
+-0.09216 0.070904 -0.12064
+-0.1 0.07 -0.1
+-0.09216 0.069096 -0.08192
+-0.07248 0.068048 -0.06096
+-0.04672 0.066952 -0.03904
+-0.02064 0.065904 -0.01808
+0 0.065 0
+0.01808 0.064096 0.01808
+0.03904 0.063048 0.03904
+0.06096 0.061952 0.06096
+0.08192 0.060904 0.08192
+0.1 0.06 0.1
+0.11808 0.059096 0.11808
+0.13904 0.058048 0.13904
+0.16096 0.056952 0.16096
+0.18192 0.055904 0.18192
+2 0 1
+2 1 2
+2 2 3
+2 3 4
+2 4 5
+2 5 6
+2 6 7
+2 7 8
+2 8 9
+2 9 10
+2 10 11
+2 11 12
+2 12 13
+2 13 14
+2 14 15
+2 15 16
+2 16 17
+2 17 18
+2 18 19
+2 19 20
+2 20 21
+2 21 22
+2 22 23
+2 23 24
+2 24 25
+2 25 26
+2 26 27
+2 27 28
+2 28 29
+2 29 30
+2 30 31
+2 31 32
+2 32 33
+2 33 34
+2 34 35
+2 35 36
+2 36 37
+2 37 38
+2 38 39
+0.001
+900.0 0.45 1.75e8
+19
+38
\ No newline at end of file
diff --git a/SurgSim/Physics/RenderTests/Data/prolene 3.0-fixedExtremity.ply b/SurgSim/Physics/RenderTests/Data/prolene 3.0-fixedExtremity.ply
new file mode 100644
index 0000000..932dcb2
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/Data/prolene 3.0-fixedExtremity.ply
@@ -0,0 +1,159 @@
+ply
+format ascii 1.0
+comment Prolene 3.0
+comment https://en.wikipedia.org/wiki/Prolene
+comment Prolene is a synthetic, monofilament, nonabsorbable polypropylene suture
+comment https://en.wikipedia.org/wiki/Surgical_suture
+comment Surgical suture sizes are often refered in USP unit
+comment This model is a 3.0 USP (0.2mm diameter)
+comment https://en.wikipedia.org/wiki/Young's_modulus
+comment Young modulus for polypropylene is between 1.5-2 GPa
+comment https://en.wikipedia.org/wiki/Polypropylene
+comment Mass density for polypropylene is between 855 and 946 Kg.m-3
+comment http://www.vinidex.com.au/technical/material-properties/polypropylene-properties/
+comment Poisson ratio for polypropylene is 0.45
+element vertex 65
+property double x
+property double y
+property double z
+element 1d_element 64
+property list uint uint vertex_indices
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+element radius 1
+property double value
+element boundary_condition 1
+property uint vertex_index
+end_header
+0 0.01 -0.175
+0 0.0155 -0.17
+0 0.019 -0.165
+0 0.019 -0.16
+0 0.0155 -0.155
+0 0.01 -0.15
+0.0010452846 0.009945219 -0.1449152542
+0.0020791169 0.009781476 -0.1398305084
+0.0030901699 0.0095105652 -0.1347457626
+0.0040673664 0.0091354546 -0.1296610168
+0.005 0.008660254 -0.124576271
+0.0058778525 0.0080901699 -0.1194915252
+0.0066913061 0.0074314483 -0.1144067794
+0.0074314483 0.0066913061 -0.1093220336
+0.0080901699 0.0058778525 -0.1042372878
+0.008660254 0.005 -0.099152542
+0.0091354546 0.0040673664 -0.0940677962
+0.0095105652 0.0030901699 -0.0889830504
+0.009781476 0.0020791169 -0.0838983046
+0.009945219 0.0010452846 -0.0788135588
+0.01 2.94896168765644E-012 -0.073728813
+0.009945219 -0.0010452846 -0.0686440672
+0.009781476 -0.0020791169 -0.0635593214
+0.0095105652 -0.0030901699 -0.0584745756
+0.0091354546 -0.0040673664 -0.0533898298
+0.008660254 -0.005 -0.048305084
+0.0080901699 -0.0058778525 -0.0432203382
+0.0074314483 -0.0066913061 -0.0381355924
+0.0066913061 -0.0074314483 -0.0330508466
+0.0058778525 -0.0080901699 -0.0279661008
+0.005 -0.008660254 -0.022881355
+0.0040673664 -0.0091354546 -0.0177966092
+0.0030901699 -0.0095105652 -0.0127118634
+0.0020791169 -0.009781476 -0.0076271176
+0.0010452846 -0.009945219 -0.0025423718
+5.89791449352869E-012 -0.01 0.002542374
+-0.0010452846 -0.009945219 0.0076271198
+-0.0020791169 -0.009781476 0.0127118656
+-0.0030901699 -0.0095105652 0.0177966114
+-0.0040673664 -0.0091354546 0.0228813572
+-0.005 -0.008660254 0.027966103
+-0.0058778525 -0.0080901699 0.0330508488
+-0.0066913061 -0.0074314483 0.0381355946
+-0.0074314482 -0.0066913061 0.0432203404
+-0.0080901699 -0.0058778525 0.0483050862
+-0.008660254 -0.005 0.053389832
+-0.0091354546 -0.0040673664 0.0584745778
+-0.0095105652 -0.00309017 0.0635593236
+-0.009781476 -0.0020791169 0.0686440694
+-0.009945219 -0.0010452846 0.0737288152
+-0.01 -8.84689838564563E-012 0.078813561
+-0.009945219 0.0010452846 0.0838983068
+-0.009781476 0.0020791169 0.0889830526
+-0.0095105652 0.0030901699 0.0940677984
+-0.0091354546 0.0040673664 0.0991525442
+-0.008660254 0.005 0.10423729
+-0.0080901699 0.0058778525 0.1093220358
+-0.0074314483 0.0066913061 0.1144067816
+-0.0066913061 0.0074314482 0.1194915274
+-0.0058778525 0.0080901699 0.1245762732
+-0.005 0.008660254 0.129661019
+-0.0040673664 0.0091354546 0.1347457648
+-0.00309017 0.0095105652 0.1398305106
+-0.0020791169 0.009781476 0.1449152564
+-0.0010452846 0.009945219 0.1500000022
+2 0 1 900.0 0.45 200e9
+2 1 2 900.0 0.45 200e9
+2 2 3 900.0 0.45 200e9
+2 3 4 900.0 0.45 200e9
+2 4 5 900.0 0.45 200e9
+2 5 6 900.0 0.45 25e9
+2 6 7 900.0 0.45 12e9
+2 7 8 900.0 0.45 6e9
+2 8 9 900.0 0.45 3e9
+2 9 10 900.0 0.45 1.75e9
+2 10 11 900.0 0.45 1.75e9
+2 11 12 900.0 0.45 1.75e9
+2 12 13 900.0 0.45 1.75e9
+2 13 14 900.0 0.45 1.75e9
+2 14 15 900.0 0.45 1.75e9
+2 15 16 900.0 0.45 1.75e9
+2 16 17 900.0 0.45 1.75e9
+2 17 18 900.0 0.45 1.75e9
+2 18 19 900.0 0.45 1.75e9
+2 19 20 900.0 0.45 1.75e9
+2 20 21 900.0 0.45 1.75e9
+2 21 22 900.0 0.45 1.75e9
+2 22 23 900.0 0.45 1.75e9
+2 23 24 900.0 0.45 1.75e9
+2 24 25 900.0 0.45 1.75e9
+2 25 26 900.0 0.45 1.75e9
+2 26 27 900.0 0.45 1.75e9
+2 27 28 900.0 0.45 1.75e9
+2 28 29 900.0 0.45 1.75e9
+2 29 30 900.0 0.45 1.75e9
+2 30 31 900.0 0.45 1.75e9
+2 31 32 900.0 0.45 1.75e9
+2 32 33 900.0 0.45 1.75e9
+2 33 34 900.0 0.45 1.75e9
+2 34 35 900.0 0.45 1.75e9
+2 35 36 900.0 0.45 1.75e9
+2 36 37 900.0 0.45 1.75e9
+2 37 38 900.0 0.45 1.75e9
+2 38 39 900.0 0.45 1.75e9
+2 39 40 900.0 0.45 1.75e9
+2 40 41 900.0 0.45 1.75e9
+2 41 42 900.0 0.45 1.75e9
+2 42 43 900.0 0.45 1.75e9
+2 43 44 900.0 0.45 1.75e9
+2 44 45 900.0 0.45 1.75e9
+2 45 46 900.0 0.45 1.75e9
+2 46 47 900.0 0.45 1.75e9
+2 47 48 900.0 0.45 1.75e9
+2 48 49 900.0 0.45 1.75e9
+2 49 50 900.0 0.45 1.75e9
+2 50 51 900.0 0.45 1.75e9
+2 51 52 900.0 0.45 1.75e9
+2 52 53 900.0 0.45 1.75e9
+2 53 54 900.0 0.45 1.75e9
+2 54 55 900.0 0.45 1.75e9
+2 55 56 900.0 0.45 1.75e9
+2 56 57 900.0 0.45 1.75e9
+2 57 58 900.0 0.45 1.75e9
+2 58 59 900.0 0.45 1.75e9
+2 59 60 900.0 0.45 1.75e9
+2 60 61 900.0 0.45 1.75e9
+2 61 62 900.0 0.45 1.75e9
+2 62 63 900.0 0.45 1.75e9
+2 63 64 900.0 0.45 1.75e9
+0.0001
+0
\ No newline at end of file
diff --git a/SurgSim/Physics/RenderTests/Fem3DMeshRenderTest.cpp b/SurgSim/Physics/RenderTests/Fem3DMeshRenderTest.cpp
index 6f03e57..57616dd 100644
--- a/SurgSim/Physics/RenderTests/Fem3DMeshRenderTest.cpp
+++ b/SurgSim/Physics/RenderTests/Fem3DMeshRenderTest.cpp
@@ -26,11 +26,17 @@
#include "SurgSim/Graphics/OsgAxesRepresentation.h"
#include "SurgSim/Graphics/OsgMeshRepresentation.h"
#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Graphics/OsgSphereRepresentation.h"
+#include "SurgSim/Math/MeshShape.h"
#include "SurgSim/Math/OdeSolver.h"
+#include "SurgSim/Math/SphereShape.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
#include "SurgSim/Physics/Fem3DPlyReaderDelegate.h"
#include "SurgSim/Physics/Fem3DRepresentation.h"
#include "SurgSim/Physics/RenderTests/RenderTest.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
using SurgSim::Math::Vector3d;
@@ -50,13 +56,20 @@ static std::shared_ptr<SurgSim::Framework::SceneElement> createFemSceneElement(
// 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->loadFem(filename);
fem->setIntegrationScheme(integrationScheme);
sceneElement->addComponent(fem);
+ auto collision = std::make_shared<SurgSim::Physics::DeformableCollisionRepresentation>("Collision");
+ auto shape = std::make_shared<SurgSim::Math::MeshShape>();
+ shape->load(filename);
+ collision->setShape(shape);
+ fem->setCollisionRepresentation(collision);
+ sceneElement->addComponent(collision);
+
// Add the graphics mesh used to display the Fem3d
auto graphics = std::make_shared<SurgSim::Graphics::OsgMeshRepresentation>("fem graphics");
- graphics->setFilename(filename);
+ graphics->loadMesh(filename);
graphics->setDrawAsWireFrame(true);
sceneElement->addComponent(graphics);
@@ -72,7 +85,6 @@ static std::shared_ptr<SurgSim::Framework::SceneElement> createFemSceneElement(
= 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
@@ -84,11 +96,77 @@ static std::shared_ptr<SurgSim::Framework::SceneElement> createFemSceneElement(
return sceneElement;
}
+std::shared_ptr<SurgSim::Framework::SceneElement> createMeshSphere()
+{
+ SurgSim::Math::RigidTransform3d pose =
+ SurgSim::Math::makeRigidTranslation(SurgSim::Math::Vector3d(0.0, 0.025, 0.0));
+
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("RigidMesh");
+ element->setPose(pose);
+
+ auto shape = std::make_shared<SurgSim::Math::MeshShape>();
+ shape->load("Geometry/sphere0_025.ply");
+
+ auto rigid = std::make_shared<SurgSim::Physics::RigidRepresentation>("Physics");
+ rigid->setIsGravityEnabled(true);
+ // http://www.engineeringtoolbox.com/wood-density-d_40.html
+ rigid->setDensity(5800.0); // Cedar of Lebanon wood density 5800.0 Kg/m-3
+ rigid->setShape(shape);
+ element->addComponent(rigid);
+
+ auto collision = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("Collision");
+ rigid->setCollisionRepresentation(collision);
+ collision->setShape(shape);
+ element->addComponent(collision);
+
+ std::shared_ptr<SurgSim::Graphics::OsgMeshRepresentation> osgRepresentation =
+ std::make_shared<SurgSim::Graphics::OsgMeshRepresentation>("Graphics");
+ osgRepresentation->setShape(shape);
+ element->addComponent(osgRepresentation);
+
+ return element;
+}
+
+std::shared_ptr<SurgSim::Framework::SceneElement> createShapeSphere()
+{
+ SurgSim::Math::RigidTransform3d pose = SurgSim::Math::makeRigidTranslation(SurgSim::Math::Vector3d(0.0, 0.05, 0.0));
+
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("Sphere");
+ element->setPose(pose);
+
+ auto physics = std::make_shared<RigidRepresentation>("Physics");
+ physics->setDensity(5800.0);
+ auto shape = std::make_shared<SurgSim::Math::SphereShape>(0.025);
+ physics->setShape(shape);
+ element->addComponent(physics);
+
+ auto collision = std::make_shared<RigidCollisionRepresentation>("Collision");
+ physics->setCollisionRepresentation(collision);
+ element->addComponent(collision);
+
+ auto graphics = std::make_shared<SurgSim::Graphics::OsgSphereRepresentation>("Graphics");
+ graphics->setRadius(shape->getRadius());
+ element->addComponent(graphics);
+
+ return element;
+}
+
TEST_F(RenderTests, SimulatedWoundRenderTest)
{
runtime->getScene()->addSceneElement(createFemSceneElement("Fem",
"Geometry/wound_deformable.ply",
- SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER));
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT));
+
+ runTest(Vector3d(0.0, 0.0, 0.2), Vector3d::Zero(), 5000.0);
+}
+
+TEST_F(RenderTests, Fem3dMeshCollision)
+{
+ runtime->getScene()->addSceneElement(createMeshSphere());
+
+ runtime->getScene()->addSceneElement(createFemSceneElement("Fem",
+ "Geometry/wound_deformable.ply",
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT));
runTest(Vector3d(0.0, 0.0, 0.2), Vector3d::Zero(), 5000.0);
}
diff --git a/SurgSim/Physics/RenderTests/Fem3DvsTruthCubeRenderTest.cpp b/SurgSim/Physics/RenderTests/Fem3DvsTruthCubeRenderTest.cpp
index 5d620e9..b8fd358 100644
--- a/SurgSim/Physics/RenderTests/Fem3DvsTruthCubeRenderTest.cpp
+++ b/SurgSim/Physics/RenderTests/Fem3DvsTruthCubeRenderTest.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -392,7 +392,7 @@ private:
/// \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)
+ SurgSim::Math::Matrix* A, SurgSim::Math::Vector* B)
{
// The static system with constraints is defined as follow:
// (K H^T).(U ) = (F)
@@ -400,26 +400,27 @@ void buildConstrainedSystem(std::shared_ptr<TruthCubeRepresentation> truthCubeRe
size_t numConstraints = truthCubeRepresentation->getBoundaryConditions().size();
size_t numDof = truthCubeRepresentation->getNumDof();
- A.resize(numDof + numConstraints, numDof + numConstraints);
- B.resize(numDof + numConstraints);
+ A->resize(numDof + numConstraints, numDof + numConstraints);
+ B->resize(numDof + numConstraints);
SurgSim::Math::Matrix H(numConstraints, truthCubeRepresentation->getNumDof());
H.setZero();
- B.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];
+ (*B)[numDof + i] = truthCubeRepresentation->getBoundaryConditionsDisplacement()[i];
}
- A.setZero();
+ A->setZero();
// Copy K into A
- A.block(0,0, numDof, numDof) = truthCubeRepresentation->computeK(*truthCubeRepresentation->getInitialState());
+ truthCubeRepresentation->updateFMDK(*truthCubeRepresentation->getInitialState(), Math::ODEEQUATIONUPDATE_K);
+ A->block(0,0, numDof, numDof) = truthCubeRepresentation->getK();
// Copy H into A
- A.block(numDof,0, numConstraints, numDof) = H;
+ A->block(numDof, 0, numConstraints, numDof) = H;
//Copy H^T into A
- A.block(0, numDof, numDof, numConstraints) = H.transpose();
+ A->block(0, numDof, numDof, numConstraints) = H.transpose();
}
/// Using static solver to find the displacement of truth cube
@@ -432,10 +433,10 @@ SurgSim::Math::Vector staticSolver(std::shared_ptr<TruthCubeRepresentation> trut
// Build the constrained system A.X = B
SurgSim::Math::Matrix A;
SurgSim::Math::Vector B;
- buildConstrainedSystem(truthCubeRepresentation, A, B);
+ buildConstrainedSystem(truthCubeRepresentation, &A, &B);
// Solve the constrained system A.X = B
- SurgSim::Math::Vector X = A.inverse() * B;
+ SurgSim::Math::Vector X = A.partialPivLu().solve(B);
// Extract the dof displacement vector from the solution X
return X.segment(0, numDof);
@@ -540,7 +541,7 @@ struct Fem3DVSTruthCubeRenderTests : public RenderTests
viewElement->addComponent(component);
}
- virtual void SetUp() override
+ void SetUp() override
{
RenderTests::SetUp();
diff --git a/SurgSim/Physics/RenderTests/RenderTestCcdSuture.cpp b/SurgSim/Physics/RenderTests/RenderTestCcdSuture.cpp
new file mode 100644
index 0000000..b53e0ea
--- /dev/null
+++ b/SurgSim/Physics/RenderTests/RenderTestCcdSuture.cpp
@@ -0,0 +1,341 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+///\file RenderTestCcdSuture.cpp render test for ccd with Fem1D
+
+#include <memory>
+
+#include "SurgSim/Blocks/CompoundShapeToGraphics.h"
+#include "SurgSim/Blocks/TransferPhysicsToVerticesBehavior.h"
+#include "SurgSim/Blocks/VisualizeConstraints.h"
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Graphics/OsgAxesRepresentation.h"
+#include "SurgSim/Graphics/OsgCurveRepresentation.h"
+#include "SurgSim/Graphics/OsgMeshRepresentation.h"
+#include "SurgSim/Graphics/OsgSceneryRepresentation.h"
+#include "SurgSim/Math/CompoundShape.h"
+#include "SurgSim/Math/LinearSparseSolveAndInverse.h"
+#include "SurgSim/Math/MeshShape.h"
+#include "SurgSim/Math/OdeSolver.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SegmentMeshShape.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/PhysicsManager.h"
+#include "SurgSim/Physics/RenderTests/RenderTest.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+
+namespace
+{
+std::shared_ptr<SurgSim::Framework::SceneElement> makeSuture(const std::string& filename)
+{
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("Suture");
+
+ // Physics
+ auto physics = std::make_shared<SurgSim::Physics::Fem1DRepresentation>("Physics");
+ physics->setFemElementType("SurgSim::Physics::Fem1DElementBeam");
+ physics->setLocalPose(SurgSim::Math::RigidTransform3d::Identity());
+ physics->loadFem(filename);
+ physics->setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT);
+ physics->setLinearSolver(SurgSim::Math::LINEARSOLVER_LU);
+ physics->setRayleighDampingMass(5.0);
+ physics->setRayleighDampingStiffness(0.001);
+ physics->setIsGravityEnabled(true);
+ element->addComponent(physics);
+
+ // Graphics
+ auto gfx = std::make_shared<SurgSim::Graphics::OsgCurveRepresentation>("Graphics");
+ gfx->setColor(SurgSim::Math::Vector4d(0.0, 0.0, 1.0, 1.0));
+ gfx->setAntiAliasing(true);
+ gfx->setWidth(0.9);
+ element->addComponent(gfx);
+
+ auto collision = std::make_shared<SurgSim::Physics::DeformableCollisionRepresentation>("Collision");
+ auto shape = std::make_shared<SurgSim::Math::SegmentMeshShape>();
+ shape->load(filename);
+ shape->setRadius(0.001);
+ collision->setShape(shape);
+ collision->setCollisionDetectionType(SurgSim::Collision::COLLISION_DETECTION_TYPE_CONTINUOUS);
+ collision->setSelfCollisionDetectionType(SurgSim::Collision::COLLISION_DETECTION_TYPE_CONTINUOUS);
+ physics->setCollisionRepresentation(collision);
+ element->addComponent(collision);
+
+ auto copier = std::make_shared<SurgSim::Blocks::TransferPhysicsToVerticesBehavior>("Copier");
+ copier->setSource(physics);
+ copier->setTarget(gfx);
+ element->addComponent(copier);
+
+ return element;
+}
+
+std::shared_ptr<SurgSim::Framework::SceneElement> makeRigid(const std::string& filename)
+{
+ SurgSim::Math::RigidTransform3d pose = SurgSim::Math::makeRigidTranslation(SurgSim::Math::Vector3d(0.0, -0.1, 0.0));
+
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("RigidMesh");
+ element->setPose(pose);
+ auto shape = std::make_shared<SurgSim::Math::MeshShape>();
+ shape->load(filename);
+
+ auto rigid = std::make_shared<SurgSim::Physics::RigidRepresentation>("Physics");
+ rigid->setIsGravityEnabled(false);
+ // http://www.engineeringtoolbox.com/wood-density-d_40.html
+ rigid->setDensity(5800.0); // Cedar of Lebanon wood density 5800.0 Kg/m-3
+ rigid->setShape(shape);
+ element->addComponent(rigid);
+
+ auto collision = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("Collision");
+ collision->setCollisionDetectionType(SurgSim::Collision::COLLISION_DETECTION_TYPE_CONTINUOUS);
+ rigid->setCollisionRepresentation(collision);
+ collision->setShape(shape);
+ element->addComponent(collision);
+
+ auto osgRepresentation = std::make_shared<SurgSim::Graphics::OsgMeshRepresentation>("Graphics");
+ osgRepresentation->setShape(shape);
+ element->addComponent(osgRepresentation);
+
+ return element;
+}
+
+std::shared_ptr<SurgSim::Framework::SceneElement> makeCompound()
+{
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("Compound");
+ using SurgSim::Math::Vector3d;
+
+ auto physics = std::make_shared<SurgSim::Physics::RigidRepresentation>("Physics");
+ physics->setDensity(700.0); // Wood
+ physics->setLinearDamping(0.5);
+ physics->setIsGravityEnabled(false);
+
+ auto subShape = std::make_shared<SurgSim::Math::MeshShape>();
+ subShape->load("bar.ply");
+ auto shape = std::make_shared<SurgSim::Math::CompoundShape>();
+
+ physics->setShape(shape);
+ element->addComponent(physics);
+
+ auto transform = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(0.2, Vector3d(1.0, 0.0, 0.0)),
+ Vector3d(0.0, 0.0, 0.0));
+ shape->addShape(subShape, transform);
+
+ auto copier = std::make_shared<SurgSim::Blocks::CompoundShapeToGraphics>("Copier");
+ copier->setSource(physics);
+ element->addComponent(copier);
+
+ auto graphics = std::make_shared<SurgSim::Graphics::OsgSceneryRepresentation>("LeftGraphics");
+ graphics->setLocalPose(transform);
+ graphics->loadModel("bar.ply");
+ element->addComponent(graphics);
+ copier->addTarget(graphics);
+
+
+ transform = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(-0.2, Vector3d(1.0, 0.0, 0.0)),
+ Vector3d(0.0, 0.0, 0.0));
+ shape->addShape(subShape, transform);
+
+ graphics = std::make_shared<SurgSim::Graphics::OsgSceneryRepresentation>("RightGraphics");
+ graphics->setLocalPose(transform);
+ graphics->loadModel("bar.ply");
+ element->addComponent(graphics);
+ copier->addTarget(graphics);
+
+ auto rigidCollision = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("Collision");
+ physics->setCollisionRepresentation(rigidCollision);
+ rigidCollision->setCollisionDetectionType(SurgSim::Collision::COLLISION_DETECTION_TYPE_CONTINUOUS);
+
+ element->addComponent(rigidCollision);
+
+ transform = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(M_PI_2, Vector3d(0.0, 1.0, 0.0)),
+ Vector3d(0.0, -0.2, 0.0));
+ element->setPose(transform);
+
+ return element;
+}
+
+}
+
+class CcdSutureTest : public SurgSim::Physics::RenderTests
+{
+public:
+ void SetUp() override
+ {
+ SurgSim::Physics::RenderTests::SetUp();
+ scene->addSceneElement(std::make_shared<SurgSim::Blocks::VisualizeConstraints>());
+
+ SurgSim::Framework::Logger::getLoggerManager()->setThreshold(SurgSim::Framework::LOG_LEVEL_DEBUG);
+ physicsManager->setComputations(SurgSim::Physics::createCcdPipeline());
+ physicsManager->setRate(150.0);
+ }
+};
+
+TEST_F(CcdSutureTest, SutureVsMeshedCylinder)
+{
+ scene->addSceneElement(makeSuture("prolene 3.0-fixedExtremity.ply"));
+ scene->addSceneElement(makeRigid("cylinder.ply"));
+
+ SurgSim::Math::Vector3d cameraPosition(0.25, 0.0, 0.1);
+ SurgSim::Math::Vector3d cameraLookAt(0.0, -0.1, 0.0);
+ physicsManager->setRate(100.0);
+ double milliseconds = 5000.0;
+ runTest(cameraPosition, cameraLookAt, milliseconds);
+}
+
+TEST_F(CcdSutureTest, Fem1DHalfKnot)
+{
+ scene->addSceneElement(makeSuture("half_knot.ply"));
+
+ SurgSim::Math::Vector3d cameraPosition(1.0, 0.0, 1.0);
+ SurgSim::Math::Vector3d cameraLookAt(0.0, 0.0, 0.0);
+ physicsManager->setRate(150.0);
+ double milliseconds = 5000.0;
+ runTest(cameraPosition, cameraLookAt, milliseconds);
+}
+
+TEST_F(CcdSutureTest, Fem1DLoop)
+{
+ scene->addSceneElement(makeSuture("loop.ply"));
+
+ SurgSim::Math::Vector3d cameraPosition(0.25, 0.0, 0.25);
+ SurgSim::Math::Vector3d cameraLookAt(0.0, 0.0, 0.0);
+ physicsManager->setRate(50.0);
+ double milliseconds = 5000.0;
+ runTest(cameraPosition, cameraLookAt, milliseconds);
+}
+
+TEST_F(CcdSutureTest, Fem1DBlock)
+{
+
+ std::shared_ptr<SurgSim::Framework::SceneElement> element =
+ std::make_shared<SurgSim::Framework::BasicSceneElement>("Axis");
+ element->addComponent(std::make_shared<SurgSim::Graphics::OsgAxesRepresentation>("Axis"));
+ scene->addSceneElement(element);
+
+ element = makeRigid("bar.ply");
+ auto transform = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(M_PI_2, SurgSim::Math::Vector3d(0.0, 1.0, 0.0)),
+ SurgSim::Math::Vector3d(0.0, -0.2, 0.0));
+ element->setPose(transform);
+ scene->addSceneElement(element);
+ scene->addSceneElement(makeSuture("prolene 3.0-fixedExtremity.ply"));
+
+ physicsManager->setRate(100.0);
+ physicsManager->setComputations(SurgSim::Physics::createCcdPipeline());
+ scene->addSceneElement(std::make_shared<SurgSim::Blocks::VisualizeConstraints>());
+
+ SurgSim::Math::Vector3d cameraPosition(0.25, 0.0, 0.25);
+ SurgSim::Math::Vector3d cameraLookAt(0.0, -0.1, 0.0);
+ double miliseconds = 5000.0;
+
+ runTest(cameraPosition, cameraLookAt, miliseconds);
+}
+
+
+TEST_F(CcdSutureTest, Fem1DCompound)
+{
+
+ auto element = std::make_shared<SurgSim::Framework::BasicSceneElement>("Axis");
+ element->addComponent(std::make_shared<SurgSim::Graphics::OsgAxesRepresentation>("Axis"));
+ scene->addSceneElement(element);
+
+ scene->addSceneElement(makeCompound());
+
+ scene->addSceneElement(makeSuture("prolene 3.0-fixedExtremity.ply"));
+
+ physicsManager->setRate(100.0);
+ physicsManager->setComputations(SurgSim::Physics::createCcdPipeline());
+ scene->addSceneElement(std::make_shared<SurgSim::Blocks::VisualizeConstraints>());
+
+ SurgSim::Math::Vector3d cameraPosition(0.25, 0.0, 0.25);
+ SurgSim::Math::Vector3d cameraLookAt(0.0, -0.1, 0.0);
+ double miliseconds = 5000.0;
+
+ runTest(cameraPosition, cameraLookAt, miliseconds);
+}
+
+
+TEST_F(CcdSutureTest, Fem1DMovingCompound)
+{
+
+ using SurgSim::Math::Vector3d;
+ std::shared_ptr<SurgSim::Framework::SceneElement> element =
+ std::make_shared<SurgSim::Framework::BasicSceneElement>("Axis");
+ element->addComponent(std::make_shared<SurgSim::Graphics::OsgAxesRepresentation>("Axis"));
+ scene->addSceneElement(element);
+
+ element = makeCompound();
+
+ auto leftGraphics =
+ std::dynamic_pointer_cast<SurgSim::Graphics::Representation>(element->getComponent("LeftGraphics"));
+ auto rightGraphics =
+ std::dynamic_pointer_cast<SurgSim::Graphics::Representation>(element->getComponent("RightGraphics"));
+
+ auto physics = std::dynamic_pointer_cast<SurgSim::Physics::RigidRepresentation>(element->getComponent("Physics"));
+
+ ASSERT_NE(nullptr, leftGraphics);
+ ASSERT_NE(nullptr, rightGraphics);
+ ASSERT_NE(nullptr, physics);
+
+
+ auto shape = std::dynamic_pointer_cast<SurgSim::Math::CompoundShape>(physics->getShape());
+
+ scene->addSceneElement(element);
+
+ scene->addSceneElement(makeSuture("prolene 3.0-fixedExtremity.ply"));
+
+ physicsManager->setRate(300.0);
+ physicsManager->setComputations(SurgSim::Physics::createCcdPipeline());
+ scene->addSceneElement(std::make_shared<SurgSim::Blocks::VisualizeConstraints>());
+
+ SurgSim::Math::Vector3d cameraPosition(0.25, 0.0, 0.25);
+ SurgSim::Math::Vector3d cameraLookAt(0.0, -0.1, 0.0);
+
+
+ runtime->start();
+ viewElement->enableManipulator(true);
+ viewElement->setManipulatorParameters(cameraPosition, cameraLookAt);
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+
+ for (double angle = 0.2, offset = -0.05; angle < 1.0; angle += 0.00125, offset += 0.0005)
+ {
+ auto transform = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(angle, Vector3d(1.0, 0.0, 0.0)),
+ Vector3d(0.0, offset, 0.0));
+ shape->setPose(0, transform);
+ leftGraphics->setLocalPose(transform);
+
+ transform = SurgSim::Math::makeRigidTransform(
+ SurgSim::Math::makeRotationQuaternion(-angle, Vector3d(1.0, 0.0, 0.0)),
+ Vector3d(0.0, offset, 0.0));
+
+ shape->setPose(1, transform);
+ rightGraphics->setLocalPose(transform);
+ boost::this_thread::sleep(boost::posix_time::milliseconds(50));
+ }
+}
+
+
+
diff --git a/SurgSim/Physics/RenderTests/RenderTestFem1D.cpp b/SurgSim/Physics/RenderTests/RenderTestFem1D.cpp
index 62c9d37..d634e62 100644
--- a/SurgSim/Physics/RenderTests/RenderTestFem1D.cpp
+++ b/SurgSim/Physics/RenderTests/RenderTestFem1D.cpp
@@ -17,9 +17,9 @@
#include <memory>
-#include "SurgSim/Blocks/TransferPhysicsToPointCloudBehavior.h"
+#include "SurgSim/Blocks/TransferPhysicsToVerticesBehavior.h"
#include "SurgSim/Framework/BasicSceneElement.h"
-#include "SurgSim/Graphics/OsgPointCloudRepresentation.h"
+#include "SurgSim/Graphics/OsgCurveRepresentation.h"
#include "SurgSim/Math/OdeState.h"
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/RigidTransform.h"
@@ -28,9 +28,9 @@
#include "SurgSim/Physics/Fem1DElementBeam.h"
#include "SurgSim/Physics/RenderTests/RenderTest.h"
-using SurgSim::Blocks::TransferPhysicsToPointCloudBehavior;
+using SurgSim::Blocks::TransferPhysicsToVerticesBehavior;
using SurgSim::Framework::BasicSceneElement;
-using SurgSim::Graphics::OsgPointCloudRepresentation;
+using SurgSim::Graphics::OsgCurveRepresentation;
using SurgSim::Math::Vector3d;
using SurgSim::Physics::Fem1DRepresentation;
using SurgSim::Physics::Fem1DElementBeam;
@@ -81,8 +81,7 @@ std::shared_ptr<SurgSim::Framework::SceneElement> createFem1D(const std::string&
const SurgSim::Math::Vector4d& color,
SurgSim::Math::IntegrationScheme integrationScheme)
{
- std::shared_ptr<Fem1DRepresentation> physicsRepresentation
- = std::make_shared<Fem1DRepresentation>("Physics Representation: " + name);
+ auto 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);
@@ -91,23 +90,19 @@ std::shared_ptr<SurgSim::Framework::SceneElement> createFem1D(const std::string&
physicsRepresentation->setRayleighDampingMass(5e-2);
physicsRepresentation->setRayleighDampingStiffness(5e-3);
- std::shared_ptr<BasicSceneElement> femSceneElement = std::make_shared<BasicSceneElement>(name);
+ auto femSceneElement = std::make_shared<BasicSceneElement>(name);
femSceneElement->addComponent(physicsRepresentation);
- std::shared_ptr<SurgSim::Graphics::PointCloudRepresentation> graphicsRepresentation
- = std::make_shared<OsgPointCloudRepresentation>("Graphics Representation: " + name);
+ auto graphicsRepresentation = std::make_shared<OsgCurveRepresentation>("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);
+ auto copier = std::make_shared<SurgSim::Blocks::TransferPhysicsToVerticesBehavior>("Copier");
+ copier->setSource(physicsRepresentation);
+ copier->setTarget(graphicsRepresentation);
+ femSceneElement->addComponent(copier);
return femSceneElement;
}
@@ -129,13 +124,13 @@ TEST_F(RenderTests, VisualTestFem1D)
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
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT)); // 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));
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT));
scene->addSceneElement(
createFem1D("Runge Kutta 4",
@@ -147,7 +142,7 @@ TEST_F(RenderTests, VisualTestFem1D)
createFem1D("Euler Implicit",
makeRigidTranslation(Vector3d(0.0, -0.25, 0.0)),
Vector4d(0, 0, 1, 1),
- SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER));
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT));
scene->addSceneElement(
createFem1D("Static",
diff --git a/SurgSim/Physics/RenderTests/RenderTestFem2D.cpp b/SurgSim/Physics/RenderTests/RenderTestFem2D.cpp
index c6a4e90..6f3744f 100644
--- a/SurgSim/Physics/RenderTests/RenderTestFem2D.cpp
+++ b/SurgSim/Physics/RenderTests/RenderTestFem2D.cpp
@@ -185,7 +185,7 @@ std::shared_ptr<SurgSim::Framework::SceneElement> createFem2D(const std::string&
graphicsPointCloudRepresentation->setLocalPose(gfxPose);
graphicsPointCloudRepresentation->setColor(color);
graphicsPointCloudRepresentation->setPointSize(3.0f);
- graphicsPointCloudRepresentation->setVisible(true);
+ graphicsPointCloudRepresentation->setLocalActive(true);
femSceneElement->addComponent(graphicsPointCloudRepresentation);
auto physicsToPointCloud =
@@ -216,13 +216,13 @@ TEST_F(RenderTests, VisualTestFem2D)
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
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT)); // 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));
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT_MODIFIED));
scene->addSceneElement(
createFem2D("Runge Kutta 4",
@@ -234,7 +234,7 @@ TEST_F(RenderTests, VisualTestFem2D)
createFem2D("Euler Implicit",
makeRigidTransform(quaternion, Vector3d(0.0, -0.3, 0.0)),
Vector4d(0, 0, 1, 1),
- SurgSim::Math::INTEGRATIONSCHEME_LINEAR_IMPLICIT_EULER));
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT));
scene->addSceneElement(
createFem2D("Static",
diff --git a/SurgSim/Physics/RenderTests/RenderTestFem3D.cpp b/SurgSim/Physics/RenderTests/RenderTestFem3D.cpp
index fcac735..9b59691 100644
--- a/SurgSim/Physics/RenderTests/RenderTestFem3D.cpp
+++ b/SurgSim/Physics/RenderTests/RenderTestFem3D.cpp
@@ -106,7 +106,7 @@ std::shared_ptr<SurgSim::Framework::SceneElement> createTetrahedronFem3D(const s
graphicsRepresentation->setLocalPose(pose);
graphicsRepresentation->setColor(color);
graphicsRepresentation->setPointSize(3.0f);
- graphicsRepresentation->setVisible(true);
+ graphicsRepresentation->setLocalActive(true);
// Scene Element
std::shared_ptr<BasicSceneElement> femSceneElement = std::make_shared<BasicSceneElement>(name);
@@ -173,7 +173,7 @@ std::shared_ptr<SurgSim::Framework::SceneElement> createCubeFem3D(const std::str
graphicsRepresentation->setLocalPose(pose);
graphicsRepresentation->setColor(color);
graphicsRepresentation->setPointSize(3.0f);
- graphicsRepresentation->setVisible(true);
+ graphicsRepresentation->setLocalActive(true);
// Scene Element
std::shared_ptr<BasicSceneElement> femSceneElement = std::make_shared<BasicSceneElement>(name);
@@ -206,12 +206,12 @@ TEST_F(RenderTests, VisualTestFem3D)
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));
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT));
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));
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT_MODIFIED));
scene->addSceneElement(createCubeFem3D("CubeElement Runge Kutta 4",
makeRigidTranslation(Vector3d(0.0, 2.0, -2.0)),
@@ -221,7 +221,7 @@ TEST_F(RenderTests, VisualTestFem3D)
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));
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT));
scene->addSceneElement(createCubeFem3D("CubeElement Static",
makeRigidTranslation(Vector3d(4.0, 2.0, -2.0)),
@@ -232,12 +232,12 @@ TEST_F(RenderTests, VisualTestFem3D)
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));
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT));
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));
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT_MODIFIED));
scene->addSceneElement(createTetrahedronFem3D("TetrahedronElement Runge Kutta 4",
makeRigidTranslation(Vector3d(0.0, -2.0, -2.0)),
@@ -247,7 +247,7 @@ TEST_F(RenderTests, VisualTestFem3D)
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));
+ SurgSim::Math::INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT));
scene->addSceneElement(createTetrahedronFem3D("TetrahedronElement Static",
makeRigidTranslation(Vector3d(4.0, -2.0, -2.0)),
diff --git a/SurgSim/Physics/RenderTests/RenderTestFem3DCorotational.cpp b/SurgSim/Physics/RenderTests/RenderTestFem3DCorotational.cpp
index 9c3f9df..3b81016 100644
--- a/SurgSim/Physics/RenderTests/RenderTestFem3DCorotational.cpp
+++ b/SurgSim/Physics/RenderTests/RenderTestFem3DCorotational.cpp
@@ -104,7 +104,7 @@ std::shared_ptr<SurgSim::Framework::SceneElement> createTetrahedronFem3D(const s
graphicsRepresentation->setLocalPose(pose);
graphicsRepresentation->setColor(color);
graphicsRepresentation->setPointSize(3.0f);
- graphicsRepresentation->setVisible(true);
+ graphicsRepresentation->setLocalActive(true);
// Scene Element
std::shared_ptr<BasicSceneElement> femSceneElement = std::make_shared<BasicSceneElement>(name);
@@ -128,7 +128,7 @@ namespace SurgSim
namespace Physics
{
-TEST_F(RenderTests, VisualTestFem3DCorotatioal)
+TEST_F(RenderTests, VisualTestFem3DCorotational)
{
using SurgSim::Math::makeRigidTranslation;
@@ -136,12 +136,12 @@ TEST_F(RenderTests, VisualTestFem3DCorotatioal)
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));
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT));
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));
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED));
scene->addSceneElement(createTetrahedronFem3D("CorotatinoalTetrahedronElement Runge Kutta 4",
makeRigidTranslation(Vector3d(0.0, 1.0, -1.0)),
@@ -151,7 +151,7 @@ TEST_F(RenderTests, VisualTestFem3DCorotatioal)
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));
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT));
scene->addSceneElement(createTetrahedronFem3D("CorotatinoalTetrahedronElement Fem 3D Static",
makeRigidTranslation(Vector3d(4.0, 1.0, -1.0)),
diff --git a/SurgSim/Physics/RenderTests/RenderTestMassSprings.cpp b/SurgSim/Physics/RenderTests/RenderTestMassSprings.cpp
index ec3926a..26bc9c6 100644
--- a/SurgSim/Physics/RenderTests/RenderTestMassSprings.cpp
+++ b/SurgSim/Physics/RenderTests/RenderTestMassSprings.cpp
@@ -37,7 +37,6 @@ using SurgSim::Framework::BasicSceneElement;
using SurgSim::Graphics::OsgPointCloudRepresentation;
using SurgSim::Math::Vector3d;
-
namespace
{
@@ -53,6 +52,10 @@ std::shared_ptr<SurgSim::Framework::SceneElement> createMassSpring1D(const std::
std::vector<size_t> nodeBoundaryConditions;
nodeBoundaryConditions.push_back(0);
+ // Adding in this boundary condition is not necessary for the physics of the system, but it allows us
+ // to better control the condition number for the linear system that the OdeStaticSolver
+ // generates. This results in a more stable and more accurate test.
+ nodeBoundaryConditions.push_back(1);
// 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
@@ -91,11 +94,11 @@ std::shared_ptr<SurgSim::Framework::SceneElement> createMassSpring1D(const std::
massSpringElement->addComponent(physicsRepresentation);
std::shared_ptr<OsgPointCloudRepresentation> graphicsRepresentation =
- std::make_shared<OsgPointCloudRepresentation>("Graphics object");
+ std::make_shared<OsgPointCloudRepresentation>("Graphics object");
graphicsRepresentation->setLocalPose(gfxPose);
graphicsRepresentation->setColor(color);
graphicsRepresentation->setPointSize(3.0f);
- graphicsRepresentation->setVisible(true);
+ graphicsRepresentation->setLocalActive(true);
massSpringElement->addComponent(graphicsRepresentation);
auto physicsToGraphics =
@@ -161,11 +164,11 @@ std::shared_ptr<SurgSim::Framework::SceneElement> createMassSpring2D(const std::
massSpringElement->addComponent(physicsRepresentation);
std::shared_ptr<OsgPointCloudRepresentation> graphicsRepresentation =
- std::make_shared<OsgPointCloudRepresentation>("Graphics object");
+ std::make_shared<OsgPointCloudRepresentation>("Graphics object");
graphicsRepresentation->setLocalPose(gfxPose);
graphicsRepresentation->setColor(color);
graphicsRepresentation->setPointSize(3.0f);
- graphicsRepresentation->setVisible(true);
+ graphicsRepresentation->setLocalActive(true);
massSpringElement->addComponent(graphicsRepresentation);
auto physicsToGraphics =
std::make_shared<TransferPhysicsToPointCloudBehavior>("Physics to Graphics deformable points");
@@ -189,6 +192,11 @@ std::shared_ptr<SurgSim::Framework::SceneElement> createMassSpring3D(const std::
std::vector<size_t> nodeBoundaryConditions;
nodeBoundaryConditions.push_back(0);
nodeBoundaryConditions.push_back(1);
+ // Adding in these two boundary conditions is not necessary for the physics of the system, but it allows us
+ // to better control the condition number for the linear system that the OdeStaticSolver
+ // generates. This results in a more stable and more accurate test.
+ nodeBoundaryConditions.push_back(2);
+ nodeBoundaryConditions.push_back(3);
std::array<std::array<std::array<SurgSim::Math::Vector3d, 2>, 2>, 2> extremities =
{
{
@@ -233,11 +241,11 @@ std::shared_ptr<SurgSim::Framework::SceneElement> createMassSpring3D(const std::
massSpringElement->addComponent(physicsRepresentation);
std::shared_ptr<OsgPointCloudRepresentation> graphicsRepresentation =
- std::make_shared<OsgPointCloudRepresentation>("Graphics object");
+ std::make_shared<OsgPointCloudRepresentation>("Graphics object");
graphicsRepresentation->setLocalPose(gfxPose);
graphicsRepresentation->setColor(color);
graphicsRepresentation->setPointSize(3.0f);
- graphicsRepresentation->setVisible(true);
+ graphicsRepresentation->setLocalActive(true);
massSpringElement->addComponent(graphicsRepresentation);
auto physicsToGraphics =
@@ -266,11 +274,11 @@ TEST_F(RenderTests, VisualTestMassSprings)
{
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));
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT));
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));
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED));
scene->addSceneElement(createMassSpring1D("MassSpring 1D Runge Kutta 4",
makeRigidTranslation(Vector3d(0.5, 3.0, 0.0)), Vector4d(0, 1, 0, 1),
@@ -278,7 +286,7 @@ TEST_F(RenderTests, VisualTestMassSprings)
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));
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT));
scene->addSceneElement(createMassSpring1D("MassSpring 1D Static",
makeRigidTranslation(Vector3d(4.0, 3.0, 0.0)), Vector4d(1, 1, 1, 1),
@@ -289,11 +297,11 @@ TEST_F(RenderTests, VisualTestMassSprings)
{
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));
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT));
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));
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED));
scene->addSceneElement(createMassSpring2D("MassSpring 2D Runge Kutta 4",
makeRigidTranslation(Vector3d(0.5, 0.5, 0.0)), Vector4d(0, 1, 0, 1),
@@ -301,7 +309,7 @@ TEST_F(RenderTests, VisualTestMassSprings)
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));
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT));
scene->addSceneElement(createMassSpring2D("MassSpring 2D Static",
makeRigidTranslation(Vector3d(4.0, 0.5, 0.0)), Vector4d(1, 1, 1, 1),
@@ -312,11 +320,11 @@ TEST_F(RenderTests, VisualTestMassSprings)
{
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));
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT));
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));
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED));
scene->addSceneElement(createMassSpring3D("MassSpring 3D Runge Kutta 4",
makeRigidTranslation(Vector3d(0.5, -1.5, 0.0)), Vector4d(0, 1, 0, 1),
@@ -324,14 +332,14 @@ TEST_F(RenderTests, VisualTestMassSprings)
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));
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT));
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);
+ runTest(Vector3d(0.0, 0.0, 8.5), Vector3d::Zero(), 5000.0);
}
}; // namespace Physics
diff --git a/SurgSim/Physics/RenderTests/RenderTestRigidBodies.cpp b/SurgSim/Physics/RenderTests/RenderTestRigidBodies.cpp
index 21824da..ca3ebfe 100644
--- a/SurgSim/Physics/RenderTests/RenderTestRigidBodies.cpp
+++ b/SurgSim/Physics/RenderTests/RenderTestRigidBodies.cpp
@@ -17,8 +17,7 @@
#include <memory>
-#include "SurgSim/DataStructures/TriangleMeshBase.h"
-#include "SurgSim/DataStructures/TriangleMeshUtilities.h"
+#include "SurgSim/DataStructures/TriangleMesh.h"
#include "SurgSim/Framework/ApplicationData.h"
#include "SurgSim/Framework/BasicSceneElement.h"
#include "SurgSim/Graphics/Mesh.h"
@@ -35,7 +34,6 @@
#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;
@@ -45,6 +43,7 @@ using SurgSim::Math::BoxShape;
using SurgSim::Math::MeshShape;
using SurgSim::Math::PlaneShape;
using SurgSim::Math::SphereShape;
+using SurgSim::Math::SurfaceMeshShape;
using SurgSim::Math::Vector3d;
using SurgSim::Physics::FixedRepresentation;
using SurgSim::Physics::RigidCollisionRepresentation;
@@ -139,21 +138,17 @@ std::shared_ptr<SurgSim::Framework::SceneElement> createFixedPlaneSceneElement(c
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)
+ auto mesh = std::make_shared<MeshShape>();
+ mesh->load(plyFilename);
+ for (auto& vertex : mesh->getVertices())
{
- mesh->setVertexPosition(vertexId, mesh->getVertexPosition(vertexId) * scale);
+ vertex.position *= 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->setShape(mesh);
physicsRepresentation->setDensity(750.0); // Average mass density of oak wood
physicsRepresentation->setLinearDamping(1e-2);
physicsRepresentation->setAngularDamping(1e-4);
@@ -166,7 +161,42 @@ std::shared_ptr<SurgSim::Framework::SceneElement> createRigidMeshSceneElement(
// 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->setShape(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;
+}
+
+std::shared_ptr<SurgSim::Framework::SceneElement> createFixedSurfaceMeshSceneElement(
+ const std::string& name, std::string plyFilename, double scale = 1.0)
+{
+ auto mesh = std::make_shared<SurfaceMeshShape>();
+ mesh->load(plyFilename);
+ for (auto& vertex : mesh->getVertices())
+ {
+ vertex.position *= scale;
+ }
+
+ // Physics representation
+ std::shared_ptr<FixedRepresentation> physicsRepresentation =
+ std::make_shared<FixedRepresentation>("Physics");
+ physicsRepresentation->setShape(mesh);
+ physicsRepresentation->setDensity(750.0); // Average mass density of oak wood
+
+ // 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->setShape(mesh);
osgRepresentation->setDrawAsWireFrame(true);
std::shared_ptr<BasicSceneElement> sceneElement = std::make_shared<BasicSceneElement>(name);
@@ -191,20 +221,20 @@ TEST_F(RenderTests, VisualTestFallingRigidBodies)
// Mesh-base objects
std::shared_ptr<SurgSim::Framework::SceneElement> boxMesh =
- createRigidMeshSceneElement("boxMesh1", "box.ply", 1.0);
+ createRigidMeshSceneElement("boxMesh1", "Geometry/box.ply", 1.0);
scene->addSceneElement(boxMesh);
boxMesh->setPose(makeRigidTranslation(Vector3d(-0.35, 0.3, 0.0)));
- boxMesh = createRigidMeshSceneElement("boxMesh2", "box.ply", 0.5);
+ boxMesh = createRigidMeshSceneElement("boxMesh2", "Geometry/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);
+ createRigidMeshSceneElement("sphereMesh1", "Geometry/sphere.ply", 1.0);
scene->addSceneElement(sphereMesh);
sphereMesh->setPose(makeRigidTranslation(Vector3d(-0.15, 0.3, 0.0)));
- sphereMesh = createRigidMeshSceneElement("sphereMesh2", "sphere.ply", 0.5);
+ sphereMesh = createRigidMeshSceneElement("sphereMesh2", "Geometry/sphere.ply", 0.5);
scene->addSceneElement(sphereMesh);
sphereMesh->setPose(makeRigidTranslation(Vector3d(-0.05, 0.3, 0.0)));
@@ -240,12 +270,12 @@ TEST_F(RenderTests, VisualTestRigidBodiesSlidingOnPlanes)
// Mesh-base objects
std::shared_ptr<SurgSim::Framework::SceneElement> boxMesh =
- createRigidMeshSceneElement("boxMesh", "box.ply");
+ createRigidMeshSceneElement("boxMesh", "Geometry/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");
+ createRigidMeshSceneElement("sphereMesh", "Geometry/sphere.ply");
scene->addSceneElement(sphereMesh);
sphereMesh->setPose(makeRigidTranslation(Vector3d(-0.15, 0.3, 0.0)));
@@ -286,7 +316,7 @@ TEST_F(RenderTests, VisualTestRigidBodiesStacking)
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);
+ createRigidMeshSceneElement(ss.str(), "Geometry/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)));
@@ -298,7 +328,7 @@ TEST_F(RenderTests, VisualTestRigidBodiesStacking)
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);
+ createRigidMeshSceneElement(ss.str(), "Geometry/sphere.ply", scale);
scene->addSceneElement(sphereMesh);
sphereMesh->setPose(makeRigidTranslation(Vector3d(-0.15, 0.3 + 0.15 * i, 0.0)));
}
@@ -349,7 +379,7 @@ TEST_F(RenderTests, VisualTestRigidBodiesStackingReversed)
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);
+ createRigidMeshSceneElement(ss.str(), "Geometry/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)));
@@ -362,7 +392,7 @@ TEST_F(RenderTests, VisualTestRigidBodiesStackingReversed)
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);
+ createRigidMeshSceneElement(ss.str(), "Geometry/sphere.ply", scale);
scene->addSceneElement(sphereMesh);
sphereMesh->setPose(makeRigidTranslation(Vector3d(-0.15, 0.3 + 0.15 * i, 0.0)));
}
@@ -400,6 +430,75 @@ TEST_F(RenderTests, VisualTestRigidBodiesStackingReversed)
runTest(Vector3d(0.0, 0.0, 1.0), Vector3d::Zero(), 10000.0);
}
+TEST_F(RenderTests, VisualTestFallingRigidShapesOnPlane)
+{
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::makeRigidTranslation;
+
+ const size_t numSphere = 10;
+ const double radius = 0.05;
+ const double distanceBetweenSphere = radius / 2.0;
+
+ // Shape-base objects
+ for (size_t sphere = 0; sphere < numSphere; ++sphere)
+ {
+ std::stringstream ss;
+ ss << "sphereShape " << sphere;
+ std::shared_ptr<SurgSim::Framework::SceneElement> sphereShape =
+ createRigidSphereSceneElement(ss.str(), radius);
+ scene->addSceneElement(sphereShape);
+ sphereShape->setPose(makeRigidTranslation(Vector3d(0.0, (2.0 * radius + distanceBetweenSphere) * sphere, 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, -radius - distanceBetweenSphere, 0.0)));
+
+ runTest(Vector3d(0.0, 0.0, 1.0), Vector3d::Zero(), 10000.0);
+}
+
+TEST_F(RenderTests, VisualTestFallingRigidMeshesOnPlane)
+{
+ using SurgSim::Math::makeRigidTransform;
+ using SurgSim::Math::makeRigidTranslation;
+
+ const size_t numSphere = 10;
+ const double radius = 0.05;
+ const double distanceBetweenSphere = radius / 2.0;
+
+ double xAxis[2] = {-0.26, 0.26};
+ double xAngle[2] = {-M_PI_2, M_PI_2};
+
+ for (int i = 0; i < 2; ++i)
+ {
+ // Mesh-base objects
+ for (size_t sphere = 0; sphere < numSphere; ++sphere)
+ {
+ std::stringstream ss;
+ ss << "sphereShape " << sphere;
+ std::shared_ptr<SurgSim::Framework::SceneElement> sphereShape =
+ createRigidMeshSceneElement(ss.str(), "Geometry/sphere.ply", 10.0 * radius);
+ scene->addSceneElement(sphereShape);
+ sphereShape->setPose(
+ makeRigidTranslation(Vector3d(xAxis[i], (2.0 * radius + distanceBetweenSphere) * sphere, 0.0)));
+ }
+
+ // Floor on which the objects are falling
+ {
+ std::shared_ptr<SurgSim::Framework::SceneElement> planeMesh =
+ createFixedSurfaceMeshSceneElement("floor", "Geometry/plane.ply", 0.25);
+ scene->addSceneElement(planeMesh);
+ Eigen::AngleAxisd aa(xAngle[i], Vector3d(1.0, 0.0, 0.0));
+ planeMesh->setPose(
+ makeRigidTransform(SurgSim::Math::Quaterniond(aa),
+ Vector3d(xAxis[i], -radius - distanceBetweenSphere, 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
index c24d2f9..07a5228 100644
--- a/SurgSim/Physics/RenderTests/config.txt.in
+++ b/SurgSim/Physics/RenderTests/config.txt.in
@@ -1,2 +1,2 @@
${CMAKE_CURRENT_SOURCE_DIR}/Data/
-${PROJECT_BINARY_DIR}/SurgSim/Testing/Data
\ No newline at end of file
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data/
diff --git a/SurgSim/Physics/Representation.cpp b/SurgSim/Physics/Representation.cpp
index 9473952..ee0420d 100644
--- a/SurgSim/Physics/Representation.cpp
+++ b/SurgSim/Physics/Representation.cpp
@@ -15,8 +15,12 @@
#include "SurgSim/Collision/Representation.h"
#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/PoseComponent.h"
#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/ConstraintImplementation.h"
#include "SurgSim/Physics/Localization.h"
#include "SurgSim/Physics/Representation.h"
@@ -31,7 +35,8 @@ Representation::Representation(const std::string& name) :
m_gravity(0.0, -9.81, 0.0),
m_numDof(0),
m_isGravityEnabled(true),
- m_isDrivingSceneElementPose(true)
+ m_isDrivingSceneElementPose(true),
+ m_logger(SurgSim::Framework::Logger::getLogger("Physics/Representation"))
{
SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, size_t, NumDof, getNumDof, setNumDof);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(Representation, bool, IsGravityEnabled, isGravityEnabled, setIsGravityEnabled);
@@ -47,10 +52,6 @@ void Representation::resetState()
{
}
-void Representation::resetParameters()
-{
-}
-
size_t Representation::getNumDof() const
{
return m_numDof;
@@ -117,6 +118,17 @@ void Representation::setCollisionRepresentation(std::shared_ptr<SurgSim::Collisi
m_collisionRepresentation = val;
}
+std::shared_ptr<ConstraintImplementation> Representation::getConstraintImplementation(
+ SurgSim::Physics::ConstraintType type)
+{
+ auto implementation = ConstraintImplementation::getFactory().getImplementation(typeid(*this), type);
+ if (implementation == nullptr)
+ {
+ SURGSIM_LOG_SEVERE(m_logger) << getClassName() << ": Does not support constraint type (" << type << ").";
+ }
+ return implementation;
+}
+
void Representation::driveSceneElementPose(const SurgSim::Math::RigidTransform3d& pose)
{
if (isDrivingSceneElementPose())
diff --git a/SurgSim/Physics/Representation.h b/SurgSim/Physics/Representation.h
index 6496d48..b050b5a 100644
--- a/SurgSim/Physics/Representation.h
+++ b/SurgSim/Physics/Representation.h
@@ -17,9 +17,11 @@
#define SURGSIM_PHYSICS_REPRESENTATION_H
#include <string>
+#include <typeindex>
#include "SurgSim/Framework/Representation.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/ConstraintType.h"
namespace SurgSim
{
@@ -29,6 +31,11 @@ namespace DataStructures
struct Location;
}
+namespace Framework
+{
+class Logger;
+}
+
namespace Collision
{
class Representation;
@@ -37,21 +44,11 @@ class Representation;
namespace Physics
{
+class Constraint;
+class ConstraintData;
+class ConstraintImplementation;
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
{
@@ -63,16 +60,9 @@ public:
/// 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;
@@ -131,6 +121,11 @@ public:
/// \param representation The appropriate collision representation for this object.
virtual void setCollisionRepresentation(std::shared_ptr<SurgSim::Collision::Representation> representation);
+ /// Get a constraint implementation of the given type for this representation.
+ /// \param type The type of constraint.
+ /// \return A contact constraint implementation or nullptr.
+ std::shared_ptr<ConstraintImplementation> getConstraintImplementation(SurgSim::Physics::ConstraintType type);
+
protected:
/// Set the number of degrees of freedom
/// \param numDof The number of degrees of freedom
@@ -169,6 +164,8 @@ private:
/// Is this representation driving the sceneElement pose
bool m_isDrivingSceneElementPose;
+ /// Logger for this class.
+ std::shared_ptr<SurgSim::Framework::Logger> m_logger;
};
}; // namespace Physics
diff --git a/SurgSim/Physics/RigidCollisionRepresentation.cpp b/SurgSim/Physics/RigidCollisionRepresentation.cpp
index 209dccf..750ff47 100644
--- a/SurgSim/Physics/RigidCollisionRepresentation.cpp
+++ b/SurgSim/Physics/RigidCollisionRepresentation.cpp
@@ -13,11 +13,13 @@
// 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/Framework/Log.h"
+#include "SurgSim/Framework/SceneElement.h"
#include "SurgSim/Math/MathConvert.h"
+#include "SurgSim/Math/MeshShape.h"
#include "SurgSim/Math/Shape.h"
#include "SurgSim/Physics/RigidRepresentationBase.h"
@@ -39,15 +41,6 @@ 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)
{
@@ -93,5 +86,60 @@ SurgSim::Math::RigidTransform3d RigidCollisionRepresentation::getPose() const
return physicsPose * physicsRepresentation->getLocalPose().inverse() * getLocalPose();
}
+void RigidCollisionRepresentation::updateShapeData()
+{
+ // All we want to do is to transform the shape into the current pose
+ getPosedShape();
+}
+
+
+void RigidCollisionRepresentation::updateDcdData()
+{
+ // HS-2-Mar-2016
+ // #todo need to trigger the aabb tree build/update here
+}
+
+
+void RigidCollisionRepresentation::updateCcdData(double timeOfImpact)
+{
+ using Math::PosedShape;
+ using Math::PosedShapeMotion;
+ using Math::Shape;
+
+ Math::RigidTransform3d previousPose;
+ Math::RigidTransform3d currentPose;
+ {
+ auto physicsRepresentation = m_physicsRepresentation.lock();
+ SURGSIM_ASSERT(physicsRepresentation != nullptr) <<
+ "PhysicsRepresentation went out of scope for Collision Representation " << getName();
+ const Math::RigidTransform3d& physicsCurrentPose = physicsRepresentation->getCurrentState().getPose();
+ const Math::RigidTransform3d& physicsPreviousPose = physicsRepresentation->getPreviousState().getPose();
+
+ Math::RigidTransform3d transform = physicsRepresentation->getLocalPose().inverse() * getLocalPose();
+ previousPose = physicsPreviousPose * transform;
+ currentPose = physicsCurrentPose * transform;
+ }
+
+ std::shared_ptr<Shape> previousShape = getShape();
+ std::shared_ptr<Shape> currentShape = getShape();
+ if (getShape()->isTransformable())
+ {
+ previousShape = getShape()->getTransformed(previousPose);
+ currentShape = getShape()->getTransformed(currentPose);
+ }
+
+ PosedShape<std::shared_ptr<Shape>> posedShape1(previousShape, previousPose);
+ PosedShape<std::shared_ptr<Shape>> posedShape2(currentShape, currentPose);
+ PosedShapeMotion<std::shared_ptr<Shape>> posedShapeMotion(posedShape1, posedShape2);
+
+ setPosedShapeMotion(posedShapeMotion);
+
+ // HS-2-Mar-2016
+ // #todo Add AABB tree for the posedShapeMotion (i.e. that is the tree where each bounding box consists of the
+ // corresponding elements from posedShape1 and posedShape2
+
+}
+
+
}; // namespace Collision
}; // namespace SurgSim
diff --git a/SurgSim/Physics/RigidCollisionRepresentation.h b/SurgSim/Physics/RigidCollisionRepresentation.h
index c843268..cf9604b 100644
--- a/SurgSim/Physics/RigidCollisionRepresentation.h
+++ b/SurgSim/Physics/RigidCollisionRepresentation.h
@@ -44,19 +44,17 @@ public:
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;
+ 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;
+ 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;
+ 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
@@ -72,6 +70,10 @@ public:
/// \return The physics representation.
std::shared_ptr<SurgSim::Physics::RigidRepresentationBase> getRigidRepresentation();
+ void updateShapeData() override;
+ void updateDcdData() override;
+ void updateCcdData(double timeOfImpact) override;
+
private:
std::weak_ptr<SurgSim::Physics::RigidRepresentationBase> m_physicsRepresentation;
std::shared_ptr<SurgSim::Math::Shape> m_shape;
diff --git a/SurgSim/Physics/RigidConstraintFixedPoint.cpp b/SurgSim/Physics/RigidConstraintFixedPoint.cpp
new file mode 100644
index 0000000..14b6d7c
--- /dev/null
+++ b/SurgSim/Physics/RigidConstraintFixedPoint.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/Physics/Localization.h"
+#include "SurgSim/Physics/RigidConstraintFixedPoint.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+RigidConstraintFixedPoint::RigidConstraintFixedPoint()
+{
+}
+
+RigidConstraintFixedPoint::~RigidConstraintFixedPoint()
+{
+}
+
+void RigidConstraintFixedPoint::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()*m_newH.transpose(), 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()*m_newH.transpose(), 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()*m_newH.transpose(), indexOfRepresentation,
+ indexOfConstraint + 2);
+}
+
+SurgSim::Physics::ConstraintType RigidConstraintFixedPoint::getConstraintType() const
+{
+ return SurgSim::Physics::FIXED_3DPOINT;
+}
+
+size_t RigidConstraintFixedPoint::doGetNumDof() const
+{
+ return 3;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RigidConstraintFixedPoint.h b/SurgSim/Physics/RigidConstraintFixedPoint.h
new file mode 100644
index 0000000..78e11be
--- /dev/null
+++ b/SurgSim/Physics/RigidConstraintFixedPoint.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_RIGIDCONSTRAINTFIXEDPOINT_H
+#define SURGSIM_PHYSICS_RIGIDCONSTRAINTFIXEDPOINT_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// RigidRepresentation bilateral 3d constraint implementation.
+///
+/// The family of FixedPoint constraints enforce equality between two points.
+class RigidConstraintFixedPoint : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ RigidConstraintFixedPoint();
+
+ /// Destructor
+ virtual ~RigidConstraintFixedPoint();
+
+ /// Gets the constraint type for this ConstraintImplementation
+ /// \return The constraint type corresponding to this constraint implementation
+ SurgSim::Physics::ConstraintType getConstraintType() 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_RIGIDCONSTRAINTFIXEDPOINT_H
diff --git a/SurgSim/Physics/RigidConstraintFixedRotationVector.cpp b/SurgSim/Physics/RigidConstraintFixedRotationVector.cpp
new file mode 100644
index 0000000..4298c10
--- /dev/null
+++ b/SurgSim/Physics/RigidConstraintFixedRotationVector.cpp
@@ -0,0 +1,73 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/RigidConstraintFixedRotationVector.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RotationVectorConstraintData.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+RigidConstraintFixedRotationVector::RigidConstraintFixedRotationVector()
+{
+}
+
+RigidConstraintFixedRotationVector::~RigidConstraintFixedRotationVector()
+{
+}
+
+void RigidConstraintFixedRotationVector::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;
+ const auto& constRotVecData = static_cast<const RotationVectorRigidFem1DConstraintData&>(data);
+ SurgSim::Math::Vector3d rotationVector = constRotVecData.getCurrentRotationVector();
+
+ // Fill up b with the constraint violation
+ mlcp->b.segment<3>(indexOfConstraint) += rotationVector * scale;
+}
+
+SurgSim::Physics::ConstraintType RigidConstraintFixedRotationVector::getConstraintType() const
+{
+ return SurgSim::Physics::FIXED_3DROTATION_VECTOR;
+}
+
+size_t RigidConstraintFixedRotationVector::doGetNumDof() const
+{
+ return 3;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RigidConstraintFixedRotationVector.h b/SurgSim/Physics/RigidConstraintFixedRotationVector.h
new file mode 100644
index 0000000..85cda97
--- /dev/null
+++ b/SurgSim/Physics/RigidConstraintFixedRotationVector.h
@@ -0,0 +1,68 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_RIGIDCONSTRAINTFIXEDROTATIONVECTOR_H
+#define SURGSIM_PHYSICS_RIGIDCONSTRAINTFIXEDROTATIONVECTOR_H
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// RigidRepresentationBase fixed rotation vector constraint
+///
+/// This implementation simply fixes the rotational dof of the constraint, effectively controlling the
+/// other representation orientation.
+class RigidConstraintFixedRotationVector : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ RigidConstraintFixedRotationVector();
+
+ /// Destructor
+ virtual ~RigidConstraintFixedRotationVector();
+
+ SurgSim::Physics::ConstraintType getConstraintType() const override;
+
+private:
+ size_t doGetNumDof() const override;
+
+ /// \note This is only setting the mlcp->b violation to the rigid representation current rotation vector.
+ /// It means that the rigid won't receive any forces back, it simply will control the other representation's
+ /// orientation in this constraint.
+ ///
+ /// \note The rotation vector constraint violation being calculated based on a quaternion interpolation (slerp),
+ /// and this type of interpolation being highly non-linear, the classical way of using the implementation one after
+ /// the other one won't work.
+ /// Therefore, the RotationVectorConstraint will use the vector mlcp->b to retrieve both representation's
+ /// rotation vector, then calculate the proper slerp and set the violation back in mlcp->b
+ 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_RIGIDCONSTRAINTFIXEDROTATIONVECTOR_H
diff --git a/SurgSim/Physics/RigidConstraintFrictionlessContact.cpp b/SurgSim/Physics/RigidConstraintFrictionlessContact.cpp
new file mode 100644
index 0000000..30640e8
--- /dev/null
+++ b/SurgSim/Physics/RigidConstraintFrictionlessContact.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/RigidConstraintFrictionlessContact.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/ConstraintImplementation.h"
+#include "SurgSim/Physics/Localization.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+RigidConstraintFrictionlessContact::RigidConstraintFrictionlessContact()
+{
+
+}
+
+RigidConstraintFrictionlessContact::~RigidConstraintFrictionlessContact()
+{
+
+}
+
+void RigidConstraintFrictionlessContact::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();
+
+ // 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
+ // Since the d term will be added to the constraint for one side of the contact and subtracted from the other,
+ // and because it is not clear which distance should be used, we leave it out.
+
+ Vector3d globalPosition = localization->calculatePosition();
+ Vector3d GP = globalPosition - rigid->getCurrentState().getPose() * rigid->getMassCenter();
+
+ // Fill up b with the constraint equation...
+ double violation = n.dot(globalPosition);
+ 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 * m_newH.transpose(), indexOfRepresentation, indexOfConstraint);
+}
+
+SurgSim::Physics::ConstraintType RigidConstraintFrictionlessContact::getConstraintType() const
+{
+ return SurgSim::Physics::FRICTIONLESS_3DCONTACT;
+}
+
+size_t RigidConstraintFrictionlessContact::doGetNumDof() const
+{
+ return 1;
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RigidConstraintFrictionlessContact.h b/SurgSim/Physics/RigidConstraintFrictionlessContact.h
new file mode 100644
index 0000000..a7c0cdd
--- /dev/null
+++ b/SurgSim/Physics/RigidConstraintFrictionlessContact.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_PHYSICS_RIGIDCONSTRAINTFRICTIONLESSCONTACT_H
+#define SURGSIM_PHYSICS_RIGIDCONSTRAINTFRICTIONLESSCONTACT_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 RigidConstraintFrictionlessContact : public ConstraintImplementation
+{
+public:
+ /// Constructor
+ RigidConstraintFrictionlessContact();
+
+ /// Destructor
+ virtual ~RigidConstraintFrictionlessContact();
+
+ /// Gets the constraint type for this ConstraintImplementation
+ /// \return The constraint type corresponding to this constraint implementation
+ SurgSim::Physics::ConstraintType getConstraintType() 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_RIGIDCONSTRAINTFRICTIONLESSCONTACT_H
diff --git a/SurgSim/Physics/RigidLocalization.cpp b/SurgSim/Physics/RigidLocalization.cpp
new file mode 100644
index 0000000..29d613e
--- /dev/null
+++ b/SurgSim/Physics/RigidLocalization.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/Physics/RigidLocalization.h"
+
+#include "SurgSim/Physics/RigidRepresentationBase.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+RigidLocalization::RigidLocalization() :
+ Localization()
+{
+
+}
+
+RigidLocalization::RigidLocalization(std::shared_ptr<Representation>
+ representation) :
+ Localization()
+{
+ setRepresentation(representation);
+}
+
+RigidLocalization::~RigidLocalization()
+{
+
+}
+
+void RigidLocalization::setLocalPosition(const SurgSim::Math::Vector3d& p)
+{
+ m_position = p;
+}
+
+const SurgSim::Math::Vector3d& RigidLocalization::getLocalPosition() const
+{
+ return m_position;
+}
+
+SurgSim::Math::Vector3d RigidLocalization::doCalculatePosition(double time) const
+{
+ 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 <= std::numeric_limits<double>::epsilon())
+ {
+ return rigidRepresentation->getPreviousState().getPose() * m_position;
+ }
+ else if (time >= 1.0 - std::numeric_limits<double>::epsilon())
+ {
+ 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;
+}
+
+SurgSim::Math::Vector3d RigidLocalization::doCalculateVelocity(double time) const
+{
+ std::shared_ptr<RigidRepresentationBase> rigidRepresentation =
+ std::static_pointer_cast<RigidRepresentationBase>(getRepresentation());
+
+ SURGSIM_ASSERT(rigidRepresentation != nullptr) << "RigidRepresentation is null, it was probably not" <<
+ " initialized";
+
+ auto& previousR = rigidRepresentation->getPreviousState().getPose().linear();
+ auto& currentR = rigidRepresentation->getCurrentState().getPose().linear();
+
+ if (time <= std::numeric_limits<double>::epsilon())
+ {
+ return rigidRepresentation->getPreviousState().getLinearVelocity() +
+ rigidRepresentation->getPreviousState().getAngularVelocity().cross(previousR * m_position);
+ }
+ else if (time >= 1.0 - std::numeric_limits<double>::epsilon())
+ {
+ return rigidRepresentation->getCurrentState().getLinearVelocity() +
+ rigidRepresentation->getCurrentState().getAngularVelocity().cross(currentR * m_position);
+ }
+
+ Math::Vector3d currentVelocity = rigidRepresentation->getCurrentState().getLinearVelocity() +
+ rigidRepresentation->getCurrentState().getAngularVelocity().cross(currentR * m_position);
+ Math::Vector3d previousVelocity = rigidRepresentation->getPreviousState().getLinearVelocity() +
+ rigidRepresentation->getPreviousState().getAngularVelocity().cross(previousR * m_position);
+
+ return Math::interpolate(previousVelocity, currentVelocity, time);
+}
+
+bool RigidLocalization::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/RigidLocalization.h b/SurgSim/Physics/RigidLocalization.h
new file mode 100644
index 0000000..61e3dae
--- /dev/null
+++ b/SurgSim/Physics/RigidLocalization.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_RIGIDLOCALIZATION_H
+#define SURGSIM_PHYSICS_RIGIDLOCALIZATION_H
+
+#include "SurgSim/Physics/Localization.h"
+
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// This class implements the localization on a RigidRepresentationBase, as a local position.
+class RigidLocalization: public Localization
+{
+public:
+ /// Default constructor
+ RigidLocalization();
+
+ /// Constructor
+ /// \param representation The representation to assign to this localization.
+ explicit RigidLocalization(std::shared_ptr<Representation> representation);
+
+ /// Destructor
+ virtual ~RigidLocalization();
+
+ /// 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.
+ 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) const;
+
+ SurgSim::Math::Vector3d doCalculateVelocity(double time) const;
+
+ /// 3D position in local coordinates.
+ SurgSim::Math::Vector3d m_position;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_RIGIDLOCALIZATION_H
diff --git a/SurgSim/Physics/RigidRepresentation.cpp b/SurgSim/Physics/RigidRepresentation.cpp
index 053ee8a..acf305e 100644
--- a/SurgSim/Physics/RigidRepresentation.cpp
+++ b/SurgSim/Physics/RigidRepresentation.cpp
@@ -15,13 +15,12 @@
#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/DataStructures/BufferedValue.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"
+#include "SurgSim/Physics/RigidState.h"
namespace
{
@@ -41,6 +40,7 @@ RigidRepresentation::RigidRepresentation(const std::string& name) :
m_force(SurgSim::Math::Vector3d::Zero()),
m_torque(SurgSim::Math::Vector3d::Zero()),
m_C(SurgSim::Math::Matrix66d::Zero()),
+ m_hasExternalGeneralizedForce(false),
m_externalGeneralizedForce(SurgSim::Math::Vector6d::Zero()),
m_externalGeneralizedStiffness(SurgSim::Math::Matrix66d::Zero()),
m_externalGeneralizedDamping(SurgSim::Math::Matrix66d::Zero())
@@ -54,24 +54,20 @@ 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)
+ const SurgSim::Math::Matrix66d& K,
+ const SurgSim::Math::Matrix66d& D)
{
- m_externalGeneralizedForce += generalizedForce;
+ m_externalGeneralizedForce.unsafeGet() += generalizedForce;
m_externalGeneralizedStiffness += K;
m_externalGeneralizedDamping += D;
+ m_hasExternalGeneralizedForce = true;
}
void RigidRepresentation::addExternalGeneralizedForce(const SurgSim::DataStructures::Location& location,
- const SurgSim::Math::Vector6d& generalizedForce,
- const SurgSim::Math::Matrix66d& K,
- const SurgSim::Math::Matrix66d& D)
+ const SurgSim::Math::Vector6d& generalizedForce,
+ const SurgSim::Math::Matrix66d& K,
+ const SurgSim::Math::Matrix66d& D)
{
using SurgSim::Math::Matrix33d;
using SurgSim::Math::Matrix66d;
@@ -80,7 +76,7 @@ void RigidRepresentation::addExternalGeneralizedForce(const SurgSim::DataStructu
SURGSIM_ASSERT(location.rigidLocalPosition.hasValue()) << "Invalid location (no rigid local position)";
- RigidRepresentationLocalization localization;
+ RigidLocalization localization;
localization.setRepresentation(std::static_pointer_cast<Representation>(shared_from_this()));
localization.setLocalPosition(location.rigidLocalPosition.getValue());
const Vector3d point = localization.calculatePosition();
@@ -90,14 +86,14 @@ void RigidRepresentation::addExternalGeneralizedForce(const SurgSim::DataStructu
const Vector3d torque = lever.cross(force);
// add the generalized force
- m_externalGeneralizedForce += generalizedForce;
+ m_externalGeneralizedForce.unsafeGet() += 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;
+ m_externalGeneralizedForce.unsafeGet().segment<3>(3) += torque;
// add the extra terms in the stiffness matrix
const Vector3d leverInLocalSpace = getCurrentState().getPose().rotation().inverse() * lever;
@@ -117,14 +113,14 @@ void RigidRepresentation::addExternalGeneralizedForce(const SurgSim::DataStructu
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;
+ 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;
+ 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];
+ 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)
{
@@ -144,16 +140,16 @@ void RigidRepresentation::addExternalGeneralizedForce(const SurgSim::DataStructu
dAxisZdRotationVector = Vector3d::UnitZ();
dRdAxisX << 0.0, 0.0, 0.0,
- 0.0, 0.0, -1.0,
- 0.0, 1.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;
+ 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;
+ 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)
@@ -167,9 +163,9 @@ void RigidRepresentation::addExternalGeneralizedForce(const SurgSim::DataStructu
{
m_externalGeneralizedStiffness.block<3, 1>(3, 3 + i) +=
-((dRdAngle * dAngledRotationVector[i] +
- dRdAxisX * dAxisXdRotationVector[i] +
- dRdAxisY * dAxisYdRotationVector[i] +
- dRdAxisZ * dAxisZdRotationVector[i]) * leverInLocalSpace).cross(force);
+ dRdAxisX * dAxisXdRotationVector[i] +
+ dRdAxisY * dAxisYdRotationVector[i] +
+ dRdAxisZ * dAxisZdRotationVector[i]) * leverInLocalSpace).cross(force);
}
// add the extra damping terms produced by the lever
@@ -177,9 +173,12 @@ void RigidRepresentation::addExternalGeneralizedForce(const SurgSim::DataStructu
{
m_externalGeneralizedDamping.block<3, 1>(3, column) += lever.cross(D.block<3, 1>(0, column));
}
+
+ m_hasExternalGeneralizedForce = true;
}
-const SurgSim::Math::Vector6d& RigidRepresentation::getExternalGeneralizedForce() const
+SurgSim::DataStructures::BufferedValue<SurgSim::Math::Vector6d>&
+RigidRepresentation::getExternalGeneralizedForce()
{
return m_externalGeneralizedForce;
}
@@ -199,7 +198,7 @@ void RigidRepresentation::beforeUpdate(double dt)
RigidRepresentationBase::beforeUpdate(dt);
SURGSIM_LOG_IF(!m_parametersValid,
- SurgSim::Framework::Logger::getDefaultLogger(), WARNING) << getName() <<
+ SurgSim::Framework::Logger::getDefaultLogger(), WARNING) << getFullName() <<
" deactivated in beforeUpdate because parameters are not valid." << std::endl;
if (!m_parametersValid)
{
@@ -232,7 +231,7 @@ void RigidRepresentation::update(double dt)
Quaterniond dq;
double qNorm = q.norm(); // Norm of q before normalization
- // Rigid body dynamics (using backward euler numerical integration scheme):
+ // 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))
//
@@ -243,8 +242,11 @@ void RigidRepresentation::update(double dt)
// 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 (m_hasExternalGeneralizedForce)
+ {
+ m_force += m_externalGeneralizedForce.unsafeGet().segment<3>(0);
+ m_torque += m_externalGeneralizedForce.unsafeGet().segment<3>(3);
+ }
if (isGravityEnabled())
{
m_force += getGravity() * getMass();
@@ -280,7 +282,7 @@ void RigidRepresentation::update(double dt)
q.normalize();
}
R = q.matrix();
- m_currentState.setPose(SurgSim::Math::makeRigidTransform(R, G-R*getMassCenter()));
+ m_currentState.setPose(SurgSim::Math::makeRigidTransform(R, G - R * getMassCenter()));
// Compute the global inertia matrix with the current state
updateGlobalInertiaMatrices(m_currentState);
@@ -311,6 +313,8 @@ void RigidRepresentation::update(double dt)
void RigidRepresentation::afterUpdate(double dt)
{
+ m_externalGeneralizedForce.publish();
+
RigidRepresentationBase::afterUpdate(dt);
SURGSIM_LOG_IF(!m_parametersValid,
@@ -322,13 +326,17 @@ void RigidRepresentation::afterUpdate(double dt)
return;
}
- m_externalGeneralizedForce.setZero();
- m_externalGeneralizedStiffness.setZero();
- m_externalGeneralizedDamping.setZero();
+ if (m_hasExternalGeneralizedForce)
+ {
+ m_hasExternalGeneralizedForce = false;
+ m_externalGeneralizedForce.unsafeGet().setZero();
+ m_externalGeneralizedStiffness.setZero();
+ m_externalGeneralizedDamping.setZero();
+ }
}
void RigidRepresentation::applyCorrection(double dt,
- const Eigen::VectorBlock<SurgSim::Math::Vector>& deltaVelocity)
+ const Eigen::VectorBlock<SurgSim::Math::Vector>& deltaVelocity)
{
using SurgSim::Math::Vector3d;
using SurgSim::Math::Matrix33d;
@@ -369,7 +377,7 @@ void RigidRepresentation::applyCorrection(double dt,
q.normalize();
}
R = q.matrix();
- m_currentState.setPose(SurgSim::Math::makeRigidTransform(R, G-R*getMassCenter()));
+ m_currentState.setPose(SurgSim::Math::makeRigidTransform(R, G - R * getMassCenter()));
// Compute the global inertia matrix with the current state
updateGlobalInertiaMatrices(m_currentState);
@@ -392,7 +400,7 @@ void RigidRepresentation::applyCorrection(double dt,
computeComplianceMatrix(dt);
}
-const Eigen::Matrix < double, 6, 6, Eigen::RowMajor > &
+const Eigen::Matrix <double, 6, 6, Eigen::RowMajor>&
SurgSim::Physics::RigidRepresentation::getComplianceMatrix() const
{
return m_C;
@@ -414,7 +422,10 @@ void RigidRepresentation::computeComplianceMatrix(double dt)
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;
+ if (m_hasExternalGeneralizedForce)
+ {
+ systemMatrix += m_externalGeneralizedDamping + m_externalGeneralizedStiffness * dt;
+ }
m_C.setZero();
@@ -424,7 +435,7 @@ void RigidRepresentation::computeComplianceMatrix(double dt)
m_C.block<3, 3>(3, 3) = systemMatrix.block<3, 3>(3, 3).inverse();
}
-void RigidRepresentation::updateGlobalInertiaMatrices(const RigidRepresentationState& state)
+void RigidRepresentation::updateGlobalInertiaMatrices(const RigidState& state)
{
if (!m_parametersValid)
{
diff --git a/SurgSim/Physics/RigidRepresentation.h b/SurgSim/Physics/RigidRepresentation.h
index dccc87b..6fd79d0 100644
--- a/SurgSim/Physics/RigidRepresentation.h
+++ b/SurgSim/Physics/RigidRepresentation.h
@@ -16,6 +16,7 @@
#ifndef SURGSIM_PHYSICS_RIGIDREPRESENTATION_H
#define SURGSIM_PHYSICS_RIGIDREPRESENTATION_H
+#include "SurgSim/DataStructures/BufferedValue.h"
#include "SurgSim/Framework/Macros.h"
#include "SurgSim/Framework/ObjectFactory.h"
#include "SurgSim/Math/Vector.h"
@@ -27,9 +28,11 @@ namespace SurgSim
namespace Physics
{
-class RigidRepresentationState;
+class RigidState;
class Localization;
+typedef RigidLocalization RigidLocalization;
+
SURGSIM_STATIC_REGISTRATION(RigidRepresentation);
/// \class RigidRepresentation
@@ -48,8 +51,6 @@ public:
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);
@@ -83,7 +84,7 @@ public:
const SurgSim::Math::Matrix66d& D = SurgSim::Math::Matrix66d::Zero());
/// \return the current external generalized 6D force
- const SurgSim::Math::Vector6d& getExternalGeneralizedForce() const;
+ SurgSim::DataStructures::BufferedValue<SurgSim::Math::Vector6d>& getExternalGeneralizedForce();
/// \return the current external generalized stiffness 6x6 matrix
const SurgSim::Math::Matrix66d& getExternalGeneralizedStiffness() const;
@@ -91,11 +92,11 @@ public:
/// \return the current external generalized damping 6x6 matrix
const SurgSim::Math::Matrix66d& getExternalGeneralizedDamping() const;
- virtual void beforeUpdate(double dt) override;
+ void beforeUpdate(double dt) override;
- virtual void update(double dt) override;
+ void update(double dt) override;
- virtual void afterUpdate(double dt) override;
+ void afterUpdate(double dt) override;
void applyCorrection(double dt, const Eigen::VectorBlock<SurgSim::Math::Vector>& deltaVelocity) override;
@@ -117,12 +118,16 @@ protected:
/// Compliance matrix (size of the number of Dof = 6)
SurgSim::Math::Matrix66d m_C;
- SurgSim::Math::Vector6d m_externalGeneralizedForce;
+ /// External generalized force, stiffness and damping applied on the rigid representation
+ /// @{
+ bool m_hasExternalGeneralizedForce;
+ SurgSim::DataStructures::BufferedValue<SurgSim::Math::Vector6d> m_externalGeneralizedForce;
SurgSim::Math::Matrix66d m_externalGeneralizedStiffness;
SurgSim::Math::Matrix66d m_externalGeneralizedDamping;
+ /// @}
private:
- virtual bool doInitialize() override;
+ bool doInitialize() override;
/// Compute compliance matrix (internal data structure)
/// \param dt The time step in use
@@ -130,7 +135,7 @@ private:
/// 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;
+ void updateGlobalInertiaMatrices(const RigidState& state) override;
};
}; // Physics
diff --git a/SurgSim/Physics/RigidRepresentationBase-inl.h b/SurgSim/Physics/RigidRepresentationBase-inl.h
index 6089fb5..5221f43 100644
--- a/SurgSim/Physics/RigidRepresentationBase-inl.h
+++ b/SurgSim/Physics/RigidRepresentationBase-inl.h
@@ -23,11 +23,12 @@ namespace Physics
{
template <class T>
-std::shared_ptr<T> SurgSim::Physics::RigidRepresentationBase::createTypedLocalization(
+std::shared_ptr<T> 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>();
+ std::shared_ptr<T> result = std::make_shared<T>(
+ std::static_pointer_cast<Representation>(getSharedPtr()));
SURGSIM_ASSERT(location.rigidLocalPosition.hasValue()) <<
"Tried to create a rigid localization without valid position information";
diff --git a/SurgSim/Physics/RigidRepresentationBase.cpp b/SurgSim/Physics/RigidRepresentationBase.cpp
index c7f11f3..51c2481 100644
--- a/SurgSim/Physics/RigidRepresentationBase.cpp
+++ b/SurgSim/Physics/RigidRepresentationBase.cpp
@@ -21,10 +21,8 @@
#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
@@ -43,8 +41,8 @@ RigidRepresentationBase::RigidRepresentationBase(const std::string& name) :
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, RigidState,
+ RigidState, 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);
@@ -65,8 +63,9 @@ bool RigidRepresentationBase::doInitialize()
{
if (m_shape != nullptr)
{
- SURGSIM_ASSERT(m_shape->isValid()) <<
- "An invalid shape is used in this RigidRepresentationBase.";
+ SURGSIM_ASSERT(m_shape->isValid())
+ << "An invalid shape is used in this RigidRepresentationBase.";
+ updateProperties();
}
return true;
@@ -83,7 +82,7 @@ bool RigidRepresentationBase::doWakeUp()
return true;
}
-void RigidRepresentationBase::setInitialState(const RigidRepresentationState& state)
+void RigidRepresentationBase::setInitialState(const RigidState& state)
{
m_initialState = state;
m_currentState = state;
@@ -103,17 +102,17 @@ void RigidRepresentationBase::resetState()
updateGlobalInertiaMatrices(m_currentState);
}
-const RigidRepresentationState& RigidRepresentationBase::getInitialState() const
+const RigidState& RigidRepresentationBase::getInitialState() const
{
return m_initialState;
}
-const RigidRepresentationState& RigidRepresentationBase::getCurrentState() const
+const RigidState& RigidRepresentationBase::getCurrentState() const
{
return m_currentState;
}
-const RigidRepresentationState& RigidRepresentationBase::getPreviousState() const
+const RigidState& RigidRepresentationBase::getPreviousState() const
{
return m_previousState;
}
@@ -121,7 +120,7 @@ const RigidRepresentationState& RigidRepresentationBase::getPreviousState() cons
std::shared_ptr<Localization> RigidRepresentationBase::createLocalization(
const SurgSim::DataStructures::Location& location)
{
- return std::move(createTypedLocalization<RigidRepresentationLocalization>(location));
+ return std::move(createTypedLocalization<RigidLocalization>(location));
}
void RigidRepresentationBase::setDensity(double rho)
diff --git a/SurgSim/Physics/RigidRepresentationBase.h b/SurgSim/Physics/RigidRepresentationBase.h
index 9fdfcf3..8321b24 100644
--- a/SurgSim/Physics/RigidRepresentationBase.h
+++ b/SurgSim/Physics/RigidRepresentationBase.h
@@ -19,7 +19,8 @@
#include "SurgSim/DataStructures/Location.h"
#include "SurgSim/Math/Shape.h"
#include "SurgSim/Physics/Representation.h"
-#include "SurgSim/Physics/RigidRepresentationState.h"
+#include "SurgSim/Physics/RigidLocalization.h"
+#include "SurgSim/Physics/RigidState.h"
namespace SurgSim
{
@@ -31,7 +32,6 @@ 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,...)
@@ -48,21 +48,21 @@ public:
/// 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();
+ void setInitialState(const RigidState& state);
+
+ void resetState() override;
/// Get the initial state of the rigid representation
/// \return The initial state (pose + lin/ang velocities)
- const RigidRepresentationState& getInitialState() const;
+ const RigidState& getInitialState() const;
/// Get the current state of the rigid representation
/// \return The current state (pose + lin/ang velocities)
- const RigidRepresentationState& getCurrentState() const;
+ const RigidState& getCurrentState() const;
/// Get the previous state of the rigid representation
/// \return The previous state (pose + lin/ang velocities)
- const RigidRepresentationState& getPreviousState() const;
+ const RigidState& getPreviousState() const;
- std::shared_ptr<Localization> createLocalization(const SurgSim::DataStructures::Location& location);
+ std::shared_ptr<Localization> createLocalization(const SurgSim::DataStructures::Location& location) override;
/// Set the mass density of the rigid representation
/// \param rho The density (in Kg.m-3)
@@ -114,24 +114,23 @@ public:
/// 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;
+ void setCollisionRepresentation(std::shared_ptr<SurgSim::Collision::Representation> representation) override;
- virtual void beforeUpdate(double dt) override;
- virtual void afterUpdate(double dt) override;
+ void beforeUpdate(double dt) override;
+ void afterUpdate(double dt) override;
protected:
- virtual bool doInitialize() override;
- virtual bool doWakeUp() override;
+ bool doInitialize() override;
+ bool doWakeUp() override;
/// Initial rigid representation state (useful for reset)
- RigidRepresentationState m_initialState;
+ RigidState m_initialState;
/// Previous rigid representation state
- RigidRepresentationState m_previousState;
+ RigidState m_previousState;
/// Current rigid representation state
- RigidRepresentationState m_currentState;
+ RigidState m_currentState;
/// Last valid/final rigid representation state
- RigidRepresentationState m_finalState;
+ RigidState m_finalState;
/// Validity of the parameters
bool m_parametersValid;
@@ -168,7 +167,7 @@ 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;
+ virtual void updateGlobalInertiaMatrices(const RigidState& state) = 0;
};
}; // Physics
diff --git a/SurgSim/Physics/RigidRepresentationBilateral3D.cpp b/SurgSim/Physics/RigidRepresentationBilateral3D.cpp
deleted file mode 100644
index 6bd46d8..0000000
--- a/SurgSim/Physics/RigidRepresentationBilateral3D.cpp
+++ /dev/null
@@ -1,136 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index dfad5cf..0000000
--- a/SurgSim/Physics/RigidRepresentationBilateral3D.h
+++ /dev/null
@@ -1,74 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
deleted file mode 100644
index b06485d..0000000
--- a/SurgSim/Physics/RigidRepresentationContact.cpp
+++ /dev/null
@@ -1,115 +0,0 @@
-//// This file is a part of the OpenSurgSim project.
-//// Copyright 2013, SimQuest Solutions Inc.
-////
-//// Licensed under the Apache License, Version 2.0 (the "License");
-//// you may not use this file except in compliance with the License.
-//// You may obtain a copy of the License at
-////
-//// http://www.apache.org/licenses/LICENSE-2.0
-////
-//// Unless required by applicable law or agreed to in writing, software
-//// distributed under the License is distributed on an "AS IS" BASIS,
-//// WITHOUT WARRANTIES 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
deleted file mode 100644
index ef2cdd4..0000000
--- a/SurgSim/Physics/RigidRepresentationContact.h
+++ /dev/null
@@ -1,75 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
deleted file mode 100644
index cf97736..0000000
--- a/SurgSim/Physics/RigidRepresentationLocalization.cpp
+++ /dev/null
@@ -1,88 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 9a2f977..0000000
--- a/SurgSim/Physics/RigidRepresentationLocalization.h
+++ /dev/null
@@ -1,78 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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
deleted file mode 100644
index b4d2ced..0000000
--- a/SurgSim/Physics/RigidRepresentationState.cpp
+++ /dev/null
@@ -1,114 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 8cb5c84..0000000
--- a/SurgSim/Physics/RigidRepresentationState.h
+++ /dev/null
@@ -1,108 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR 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/RigidState.cpp b/SurgSim/Physics/RigidState.cpp
new file mode 100644
index 0000000..e32b410
--- /dev/null
+++ b/SurgSim/Physics/RigidState.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/RigidState.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+RigidState::RigidState() :
+ m_v(SurgSim::Math::Vector3d::Zero()),
+ m_w(SurgSim::Math::Vector3d::Zero()),
+ m_pose(SurgSim::Math::RigidTransform3d::Identity())
+{
+ addSerializableProperty();
+}
+
+RigidState::RigidState(const RigidState& rhs) :
+ m_v(rhs.m_v),
+ m_w(rhs.m_w),
+ m_pose(rhs.m_pose)
+{
+ addSerializableProperty();
+}
+
+void RigidState::addSerializableProperty()
+{
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RigidState, SurgSim::Math::Vector3d, LinearVelocity,
+ getLinearVelocity, setLinearVelocity);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RigidState, SurgSim::Math::Vector3d, AngularVelocity,
+ getAngularVelocity, setAngularVelocity);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(RigidState, SurgSim::Math::RigidTransform3d, Pose,
+ getPose, setPose);
+}
+
+
+RigidState& RigidState::operator=(const RigidState& rhs)
+{
+ m_v = rhs.m_v;
+ m_w = rhs.m_w;
+ m_pose = rhs.m_pose;
+
+ return *this;
+}
+
+
+RigidState::~RigidState()
+{
+}
+
+bool RigidState::operator==(const RigidState& rhs) const
+{
+ return (m_pose.isApprox(rhs.m_pose) && m_v == rhs.m_v && m_w == rhs.m_w);
+}
+
+bool RigidState::operator!=(const RigidState& rhs) const
+{
+ return !((*this) == rhs);
+}
+
+void RigidState::reset()
+{
+ m_v.setZero();
+ m_w.setZero();
+ m_pose.setIdentity();
+}
+
+const SurgSim::Math::Vector3d& RigidState::getLinearVelocity() const
+{
+ return m_v;
+}
+
+const SurgSim::Math::Vector3d& RigidState::getAngularVelocity() const
+{
+ return m_w;
+}
+
+void RigidState::setLinearVelocity(const SurgSim::Math::Vector3d &v)
+{
+ m_v = v;
+}
+
+void RigidState::setAngularVelocity(const SurgSim::Math::Vector3d &w)
+{
+ m_w = w;
+}
+
+void RigidState::setPose(const SurgSim::Math::RigidTransform3d& pose)
+{
+ m_pose = pose;
+}
+
+const SurgSim::Math::RigidTransform3d& RigidState::getPose() const
+{
+ return m_pose;
+}
+
+}; // Physics
+}; // SurgSim
\ No newline at end of file
diff --git a/SurgSim/Physics/RigidState.h b/SurgSim/Physics/RigidState.h
new file mode 100644
index 0000000..c5c54a8
--- /dev/null
+++ b/SurgSim/Physics/RigidState.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_RIGIDSTATE_H
+#define SURGSIM_PHYSICS_RIGIDSTATE_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 RigidState class describes a state (position and velocity information).
+class RigidState : public SurgSim::Framework::Accessible
+{
+public:
+ /// Default constructor
+ RigidState();
+
+ /// Default constructor
+ RigidState(const RigidState& rhs);
+
+ /// Copy assignment
+ /// \param rhs Right hand side RigidState from which data are copied.
+ /// \note 'm_functors' in base class Accessible is NOT copied.
+ RigidState& operator=(const RigidState& rhs);
+
+ /// Destructor
+ virtual ~RigidState();
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::RigidState);
+
+ /// Comparison operator
+ /// \param rhs A RigidState to compare it to
+ /// \return True if the 2 states are equals, False otherwise
+ bool operator==(const RigidState& rhs) const;
+
+ /// Comparison operator
+ /// \param rhs A RigidState to compare it to
+ /// \return False if the 2 states are equals, True otherwise
+ bool operator!=(const RigidState& 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 RigidState 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_RIGIDSTATE_H
\ No newline at end of file
diff --git a/SurgSim/Physics/RotationVectorConstraint.cpp b/SurgSim/Physics/RotationVectorConstraint.cpp
new file mode 100644
index 0000000..5f03478
--- /dev/null
+++ b/SurgSim/Physics/RotationVectorConstraint.cpp
@@ -0,0 +1,98 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/RotationVectorConstraint.h"
+
+#include "SurgSim/Framework/Assert.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+RotationVectorConstraint::RotationVectorConstraint(ConstraintType constraintType,
+ std::shared_ptr<ConstraintData> data,
+ std::shared_ptr<Representation> representation0,
+ const SurgSim::DataStructures::Location& location0,
+ std::shared_ptr<Representation> representation1,
+ const SurgSim::DataStructures::Location& location1)
+ : Constraint(constraintType, data, representation0, location0, representation1, location1)
+{
+ SURGSIM_ASSERT(constraintType == SurgSim::Physics::ConstraintType::FIXED_3DROTATION_VECTOR) <<
+ "Invalid constraint type for a rotation vector constraint";
+}
+
+RotationVectorConstraint::~RotationVectorConstraint()
+{
+}
+
+void RotationVectorConstraint::doBuild(double dt,
+ const ConstraintData& data,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation0,
+ size_t indexOfRepresentation1,
+ size_t indexOfConstraint)
+{
+ m_implementations.first->build(
+ dt,
+ *m_data.get(),
+ m_localizations.first,
+ mlcp,
+ indexOfRepresentation0,
+ indexOfConstraint,
+ CONSTRAINT_POSITIVE_SIDE);
+
+ // This uses the vector mlcp->b as a temporary vector to retrieve the 1st rotation vector
+ // mlcp->b = rotationVector1
+ SurgSim::Math::Vector3d rotationVector1 = mlcp->b.segment<3>(indexOfConstraint);
+ SurgSim::Math::Quaterniond q1 = SurgSim::Math::Quaterniond::Identity();
+ if (rotationVector1.norm() > 1e-8)
+ {
+ q1 = SurgSim::Math::makeRotationQuaternion(rotationVector1.norm(), rotationVector1.normalized());
+ }
+
+ m_implementations.second->build(
+ dt,
+ *m_data.get(),
+ m_localizations.second,
+ mlcp,
+ indexOfRepresentation1,
+ indexOfConstraint,
+ CONSTRAINT_NEGATIVE_SIDE);
+
+ // This uses the vector mlcp->b as a temporary vector to retrieve the 2nd rotation vector
+ // mlcp->b = rotationVector1 - rotationVector2
+ SurgSim::Math::Vector3d rotationVector2 = rotationVector1 - mlcp->b.segment<3>(indexOfConstraint);
+ SurgSim::Math::Quaterniond q2 = SurgSim::Math::Quaterniond::Identity();
+ if (rotationVector2.norm() > 1e-8)
+ {
+ q2 = SurgSim::Math::makeRotationQuaternion(rotationVector2.norm(), rotationVector2.normalized());
+ }
+
+ // Transform the data to have a rotation vector violation as a rotation vector
+ double angle;
+ SurgSim::Math::Vector3d axis;
+ SurgSim::Math::computeAngleAndAxis((q1 * q2.inverse()).normalized(), &angle, &axis);
+ // Set mlcp->b with the properly calculated rotation vector, using a quaternion interpolation.
+ mlcp->b.segment<3>(indexOfConstraint) = angle * axis;
+
+ mlcp->constraintTypes.push_back(
+ (m_constraintType != INVALID_CONSTRAINT) ? m_mlcpMap[m_constraintType] : Math::MLCP_INVALID_CONSTRAINT);
+}
+
+}; // namespace Physics
+
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/RotationVectorConstraint.h b/SurgSim/Physics/RotationVectorConstraint.h
new file mode 100644
index 0000000..63935e2
--- /dev/null
+++ b/SurgSim/Physics/RotationVectorConstraint.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_PHYSICS_ROTATIONVECTORCONSTRAINT_H
+#define SURGSIM_PHYSICS_ROTATIONVECTORCONSTRAINT_H
+
+#include "SurgSim/Physics/Constraint.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Specific class for rotation vector constraints.
+/// It handles the specificity of the rotation vector in the constraint calculation.
+class RotationVectorConstraint: public Constraint
+{
+public:
+ RotationVectorConstraint(
+ ConstraintType constraintType,
+ std::shared_ptr<ConstraintData> data,
+ std::shared_ptr<Representation> representation0,
+ const SurgSim::DataStructures::Location& location0,
+ std::shared_ptr<Representation> representation1,
+ const SurgSim::DataStructures::Location& location1);
+
+ /// Destructor
+ virtual ~RotationVectorConstraint();
+
+protected:
+ /// \note The rotation vector violation being calculated based on a quaternion interpolation (slerp), and this
+ /// type of interpolation being highly non-linear, the classical way of using the implementation one after the
+ /// other one won't work.
+ /// Therefore, this method temporarily uses the vector mlcpPhysicsProblem->b to retrieve both representation's
+ /// rotation vectors, then calculate the proper slerp and set the violation back in mlcpPhysicsProblem->b
+ void doBuild(double dt,
+ const ConstraintData& data,
+ MlcpPhysicsProblem* mlcpPhysicsProblem,
+ size_t indexOfRepresentation0,
+ size_t indexOfRepresentation1,
+ size_t indexOfConstraint) override;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_ROTATIONVECTORCONSTRAINT_H
diff --git a/SurgSim/Physics/RotationVectorConstraintData.h b/SurgSim/Physics/RotationVectorConstraintData.h
new file mode 100644
index 0000000..0d3e558
--- /dev/null
+++ b/SurgSim/Physics/RotationVectorConstraintData.h
@@ -0,0 +1,121 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_ROTATIONVECTORCONSTRAINTDATA_H
+#define SURGSIM_PHYSICS_ROTATIONVECTORCONSTRAINTDATA_H
+
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+#include "SurgSim/Physics/RigidRepresentationBase.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Class for rotation vector constraint data between a rigid/fixed representation and Fem1d beam being controlled.
+///
+/// It considers the Fem1D rotational dof (beamRotationVector) to be the only variable to account for.
+/// The equation to verify is \f$ R_{rigid} * RAtGrasp_{rigid}^{-1} = R_{beam} * RAtGrasp_{beam}^{-1} \f$
+/// where \f$R\f$ denotes the current prefixed object 3x3 rotation matrix
+/// \f$RAtGrasp\f$ is the 3x3 rotation matrix of the prefixed object at the time of the constraint creation
+/// and \f$R_{beam}\f$ is decomposed into the initial rotation \f$R0_{beam}\f$ and the current rotation given by the
+/// rotational dof
+/// \f$R_{rigid} * RAtGrasp_{rigid}^{-1} = R(beamRotationVector) * R0_{beam} * RAtGrasp_{beam}^{-1}\f$
+/// \f$R_{rigid} * RAtGrasp_{rigid}^{-1} * RAtGrasp_{beam} * R0_{beam}^{-1} = R(beamRotationVector)\f$
+/// \f$rotationVector(R_{rigid} * RAtGrasp_{rigid}^{-1} * RAtGrasp_{beam} * R0_{beam}^{-1}) = beamRotationVector\f$
+class RotationVectorRigidFem1DConstraintData : public ConstraintData
+{
+public:
+ /// Default constructor
+ RotationVectorRigidFem1DConstraintData() :
+ ConstraintData()
+ {
+ }
+
+ /// Destructor
+ virtual ~RotationVectorRigidFem1DConstraintData()
+ {
+ }
+
+ /// Set the rigid/fixed object part that will control the fem1d
+ /// \param rigid The rigid base representation (either a RigidRepresentation or FixedRepresentation)
+ /// \param rigidRAtGrasp The rigid rotation at the time of the constraint creation
+ void setRigidOrFixedRotation(std::shared_ptr<SurgSim::Physics::RigidRepresentationBase> rigid,
+ const SurgSim::Math::Matrix33d& rigidRAtGrasp)
+ {
+ SURGSIM_ASSERT(nullptr != rigid) << "Need a valid rigid/fixed representation";
+
+ m_rigidRAtGrasp = rigidRAtGrasp;
+ m_rigid = rigid;
+ }
+
+ /// Set the fem1d object part
+ /// \param beams The Fem1DRepresentation to be controlled by the rigid/fixed representation orientation
+ /// \param beamId The beam id that is going to be controlled by the rigid/fixed representation orientation
+ void setFem1DRotation(std::shared_ptr<SurgSim::Physics::Fem1DRepresentation> beams, size_t beamId)
+ {
+ SURGSIM_ASSERT(nullptr != beams) << "Need a valid fem1D representation";
+ SURGSIM_ASSERT(beams->getNumFemElements() > beamId) << "The beam id " << beamId
+ << " does not exists, the fem1d has " << beams->getNumFemElements() << " beams";
+
+ auto beam = std::dynamic_pointer_cast<SurgSim::Physics::Fem1DElementBeam>(beams->getFemElement(beamId));
+
+ m_beams = beams;
+
+ m_beamR0 = beam->getInitialRotation();
+
+ const auto& rotVecBeamNode0 = beams->getCurrentState()->getPositions().segment<3>(6 * beam->getNodeId(0) + 3);
+ SurgSim::Math::Matrix33d R = SurgSim::Math::Matrix33d::Identity();
+ if (!rotVecBeamNode0.isApprox(SurgSim::Math::Vector3d::Zero()))
+ {
+ R = SurgSim::Math::makeRotationMatrix(rotVecBeamNode0.norm(), rotVecBeamNode0.normalized());
+ }
+ m_beamRAtGrasp = R * m_beamR0;
+ }
+
+ /// \return The current rotation vector that should correspond to the beam rotation vector
+ /// i.e. \f$ rotationVector(R_{rigid} * RAtGrasp_{rigid}^{-1} * RAtGrasp_{beam} * R0_{beam}^{-1}) \f$
+ SurgSim::Math::Vector3d getCurrentRotationVector() const
+ {
+ SURGSIM_ASSERT(nullptr != m_rigid) << "Did you call setRigidOrFixedRotation prior to using this class ?";
+ SURGSIM_ASSERT(nullptr != m_beams) << "Did you call setFem1DRotation prior to using this class ?";
+
+ const auto& rigidR = m_rigid->getCurrentState().getPose().linear();
+ Eigen::AngleAxisd angleAxis(rigidR * m_rigidRAtGrasp.inverse() * m_beamRAtGrasp * m_beamR0.inverse());
+ return angleAxis.angle() * angleAxis.axis();
+ }
+
+private:
+ /// Rigid/Fixed representation
+ std::shared_ptr<SurgSim::Physics::RigidRepresentationBase> m_rigid;
+ /// Rigid/Fixed rotation information at the time of the constraint creation
+ SurgSim::Math::Matrix33d m_rigidRAtGrasp;
+
+ /// Fem1D representation
+ std::shared_ptr<SurgSim::Physics::Fem1DRepresentation> m_beams;
+ /// The beam initial rotation and rotation at the time of the constraint creation
+ SurgSim::Math::Matrix33d m_beamR0, m_beamRAtGrasp;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_ROTATIONVECTORCONSTRAINTDATA_H
diff --git a/SurgSim/Physics/SlidingConstraint.cpp b/SurgSim/Physics/SlidingConstraint.cpp
new file mode 100644
index 0000000..4dcda87
--- /dev/null
+++ b/SurgSim/Physics/SlidingConstraint.cpp
@@ -0,0 +1,79 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <sstream>
+
+#include "SurgSim/DataStructures/Location.h"
+#include "SurgSim/Framework/Assert.h"
+#include "SurgSim/Math/Geometry.h"
+#include "SurgSim/Physics/Fem1DLocalization.h"
+#include "SurgSim/Physics/FemRepresentation.h"
+#include "SurgSim/Physics/SlidingConstraint.h"
+#include "SurgSim/Physics/SlidingConstraintData.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+SlidingConstraint::SlidingConstraint(ConstraintType constraintType,
+ std::shared_ptr<ConstraintData> data,
+ std::shared_ptr<Representation> representation0,
+ const DataStructures::Location& location0,
+ std::shared_ptr<Representation> representation1,
+ const DataStructures::Location& location1,
+ const Math::Vector3d& slidingDirection)
+ : Constraint(constraintType, data, representation0, location0, representation1, location1)
+{
+ m_slidingConstraintData = std::dynamic_pointer_cast<SlidingConstraintData>(data);
+ SURGSIM_ASSERT(m_slidingConstraintData != nullptr) <<
+ "The data sent in for the sliding constraint is not of type SlidingConstraintData.";
+
+ Math::Vector3d directionEnd = m_localizations.second->calculatePosition() + slidingDirection;
+ m_directionEnd = m_localizations.second->getElementPose().inverse() * directionEnd;
+}
+
+SlidingConstraint::~SlidingConstraint()
+{
+}
+
+void SlidingConstraint::doBuild(double dt,
+ const ConstraintData& data,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation0,
+ size_t indexOfRepresentation1,
+ size_t indexOfConstraint)
+{
+ // Update the SlidingConstraintData
+ Math::Vector3d pointOfConstraint = m_localizations.second->calculatePosition();
+ Math::Vector3d slidingDirection = m_localizations.second->getElementPose() * m_directionEnd - pointOfConstraint;
+ m_slidingConstraintData->setSlidingDirection(pointOfConstraint, slidingDirection);
+
+ // Update the representation0's localization (representation1's remains the same).
+ bool hasReachedEnd = false;
+ m_localizations.first->moveClosestTo(pointOfConstraint, &hasReachedEnd);
+
+ if (hasReachedEnd)
+ {
+ setActive(false);
+ }
+
+ Constraint::doBuild(dt, data, mlcp, indexOfRepresentation0, indexOfRepresentation1, indexOfConstraint);
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/SlidingConstraint.h b/SurgSim/Physics/SlidingConstraint.h
new file mode 100644
index 0000000..0bee993
--- /dev/null
+++ b/SurgSim/Physics/SlidingConstraint.h
@@ -0,0 +1,77 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_SLIDINGCONSTRAINT_H
+#define SURGSIM_PHYSICS_SLIDINGCONSTRAINT_H
+
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/SlidingConstraintData.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Sliding constraint between two physics representations.
+/// The sliding direction vector (direction + origin) needs to be updated each physics time step. This is done by
+/// transforming the direction and origin into the local coordinates of the physics representations and recalculating
+/// them every time step.
+/// representation0 is the one that can 'slide' against Representation1, for example Representation0 could be a suture
+/// Representation1 tissue.
+class SlidingConstraint : public Constraint
+{
+public:
+ /// Sets all the values for this constraints, does validation on the parameters and will throw if something
+ /// is wrong with the constraint.
+ /// \param constraintType The constraint type.
+ /// \param data The data for this constraint.
+ /// \param representation0 The moving representation e.g. suture.
+ /// \param representation1 The non-moving representation e.g. tissue.
+ /// \param location0, location1 Both locations of the representations involved in this constraint.
+ /// \param slidingDirection The direction of sliding.
+ SlidingConstraint(
+ ConstraintType constraintType,
+ std::shared_ptr<ConstraintData> data,
+ std::shared_ptr<Representation> representation0,
+ const SurgSim::DataStructures::Location& location0,
+ std::shared_ptr<Representation> representation1,
+ const SurgSim::DataStructures::Location& location1,
+ const Math::Vector3d& slidingDirection);
+
+ /// Destructor
+ virtual ~SlidingConstraint();
+
+private:
+ void doBuild(double dt,
+ const ConstraintData& data,
+ MlcpPhysicsProblem* mlcpPhysicsProblem,
+ size_t indexOfRepresentation0,
+ size_t indexOfRepresentation1,
+ size_t indexOfConstraint) override;
+
+ /// The sliding constraint data.
+ std::shared_ptr<SlidingConstraintData> m_slidingConstraintData;
+
+ /// The end of the sliding direction, local to the representation1.
+ Math::Vector3d m_directionEnd;
+};
+
+}; // namespace Physics
+
+}; // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_SLIDINGCONSTRAINT_H
diff --git a/SurgSim/Physics/SlidingConstraintData.cpp b/SurgSim/Physics/SlidingConstraintData.cpp
new file mode 100644
index 0000000..2d3819a
--- /dev/null
+++ b/SurgSim/Physics/SlidingConstraintData.cpp
@@ -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.
+
+#include "SurgSim/Physics/SlidingConstraintData.h"
+#include "SurgSim/Math/Matrix.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+SlidingConstraintData::SlidingConstraintData() :
+ ConstraintData(), m_mu(0.5)
+{
+ m_normals[0].setZero();
+ m_normals[1].setZero();
+ m_tangent.setZero();
+}
+
+SlidingConstraintData::SlidingConstraintData(const SurgSim::Math::Vector3d& point,
+ const SurgSim::Math::Vector3d& direction) :
+ ConstraintData(), m_mu(0.5)
+{
+ setSlidingDirection(point, direction);
+}
+
+SlidingConstraintData::~SlidingConstraintData()
+{
+}
+
+void SlidingConstraintData::setSlidingDirection(const SurgSim::Math::Vector3d& point,
+ const SurgSim::Math::Vector3d& direction)
+{
+ m_point = point;
+ m_slidingDirection = direction;
+ Math::buildOrthonormalBasis(&m_slidingDirection, &m_normals[0], &m_normals[1]);
+
+ m_tangent = direction;
+ m_distanceTangent = -point.dot(m_tangent);
+}
+
+void SlidingConstraintData::setFrictionCoefficient(double mu)
+{
+ m_mu = mu;
+}
+
+double SlidingConstraintData::getFrictionCoefficient() const
+{
+ return m_mu;
+}
+
+const Math::RigidTransform3d SlidingConstraintData::getPose()
+{
+ SurgSim::Math::Matrix33d rotation;
+ rotation << m_slidingDirection, m_normals[0], m_normals[1];
+ return SurgSim::Math::makeRigidTransform(rotation, m_point);
+}
+
+const std::array<Math::Vector3d, 2>& SlidingConstraintData::getNormals() const
+{
+ return m_normals;
+}
+
+const Math::Vector3d& SlidingConstraintData::getTangent() const
+{
+ return m_tangent;
+}
+
+const double SlidingConstraintData::getDistanceTangent() const
+{
+ return m_distanceTangent;
+}
+
+} // namespace Physics
+
+} // namespace SurgSim
diff --git a/SurgSim/Physics/SlidingConstraintData.h b/SurgSim/Physics/SlidingConstraintData.h
new file mode 100644
index 0000000..cd52dac
--- /dev/null
+++ b/SurgSim/Physics/SlidingConstraintData.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_PHYSICS_SLIDINGCONSTRAINTDATA_H
+#define SURGSIM_PHYSICS_SLIDINGCONSTRAINTDATA_H
+
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Math/RigidTransform.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+/// Class for Frictionless sliding constraint (stores two planes, the intersection of which, is the sliding direction)
+class SlidingConstraintData : public ConstraintData
+{
+public:
+ /// Default constructor
+ SlidingConstraintData();
+
+ /// Constructor
+ /// \param point The point through which the sliding direction vector passes.
+ /// \param direction The sliding direction vector.
+ SlidingConstraintData(const SurgSim::Math::Vector3d& point, const SurgSim::Math::Vector3d& direction);
+
+ /// Destructor
+ virtual ~SlidingConstraintData();
+
+ /// Calculate the two plane equations based on the given sliding direction.
+ /// \param point The point through which the sliding direction vector passes.
+ /// \param direction The sliding direction vector.
+ void setSlidingDirection(const SurgSim::Math::Vector3d& point, const SurgSim::Math::Vector3d& direction);
+
+ /// Set the friction coefficient for a frictional sliding constraint
+ /// \param mu The friction coefficient
+ void setFrictionCoefficient(double mu);
+
+ /// \return The friction coefficient (default is 0.5)
+ /// \note The friction coefficient is only used for frictional constraint, it is discarded othersise.
+ double getFrictionCoefficient() const;
+
+ /// \return The normals of the two planes.
+ const std::array<Math::Vector3d, 2>& getNormals() const;
+
+ /// /return The pose of constraint
+ const Math::RigidTransform3d getPose();
+
+ /// \return The tangent (direction defined by the two planes).
+ const Math::Vector3d& getTangent() const;
+
+ /// \return The distance from origin of the planes direction (tangent).
+ const double getDistanceTangent() const;
+
+private:
+ /// The normals of the two planes.
+ std::array<Math::Vector3d, 2> m_normals;
+
+ /// The point of constraint.
+ Math::Vector3d m_point;
+
+ /// The sliding direction (intersection of the two planes).
+ Math::Vector3d m_slidingDirection;
+
+ /// The tangent of the two planes.
+ Math::Vector3d m_tangent;
+
+ /// The distance from origin of the tangential plane.
+ double m_distanceTangent;
+
+ /// Friction coefficient for frictional constraint (unused for frictionless constraint).
+ double m_mu;
+};
+
+} // namespace Physics
+
+} // namespace SurgSim
+
+#endif // SURGSIM_PHYSICS_SLIDINGCONSTRAINTDATA_H
diff --git a/SurgSim/Physics/SolveMlcp.cpp b/SurgSim/Physics/SolveMlcp.cpp
index 5cba61f..bce05c3 100644
--- a/SurgSim/Physics/SolveMlcp.cpp
+++ b/SurgSim/Physics/SolveMlcp.cpp
@@ -13,16 +13,12 @@
// 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/SolveMlcp.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
#include "SurgSim/Physics/PhysicsManagerState.h"
-#include "SurgSim/Physics/SolveMlcp.h"
-#include "SurgSim/Math/MlcpGaussSeidelSolver.h"
namespace SurgSim
{
@@ -30,36 +26,81 @@ 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)
+ 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()));
+ // lambda
+ const Eigen::VectorXd& lambda = result->getMlcpSolution().x;
+ if (lambda.size() == 0)
+ {
+ return result;
+ }
+
+ // Copy constraintForces back to contact
+ auto& constraintsMapping = result->getConstraintsMapping();
+ const auto& activeConstraints = result->getActiveConstraints();
+
+ for (auto& constraint : activeConstraints)
+ {
+ if (constraint->getType() != ConstraintType::FRICTIONLESS_3DCONTACT)
+ {
+ continue;
+ }
+
+ ptrdiff_t indexConstraint = constraintsMapping.getValue(constraint.get());
+ SURGSIM_ASSERT(indexConstraint >= 0) << "Index for constraint is invalid: " << indexConstraint << std::endl;
+
+ auto contactConstraintData = std::dynamic_pointer_cast<ContactConstraintData>(constraint->getData());
+ SURGSIM_ASSERT(contactConstraintData != nullptr) << "";
+
+ auto contact = contactConstraintData->getContact();
+
+ contact->force = lambda[indexConstraint] * contact->normal;
+ }
+
return result;
}
-void SolveMlcp::setMaxIterations(int maxIterations)
+void SolveMlcp::setMaxIterations(size_t maxIterations)
{
m_gaussSeidelSolver.setMaxIterations(maxIterations);
}
-void SolveMlcp::setSolverPrecision(double epsilon)
+size_t SolveMlcp::getMaxIterations() const
+{
+ return m_gaussSeidelSolver.getMaxIterations();
+}
+
+void SolveMlcp::setPrecision(double epsilon)
{
m_gaussSeidelSolver.setEpsilonConvergence(epsilon);
}
+double SolveMlcp::getPrecision() const
+{
+ return m_gaussSeidelSolver.getEpsilonConvergence();
+}
+
void SolveMlcp::setContactTolerance(double epsilon)
{
m_gaussSeidelSolver.setContactTolerance(epsilon);
}
+double SolveMlcp::getContactTolerance() const
+{
+ return m_gaussSeidelSolver.getContactTolerance();
+}
+
}; // Physics
}; // SurgSim
diff --git a/SurgSim/Physics/SolveMlcp.h b/SurgSim/Physics/SolveMlcp.h
index edcde3d..8608078 100644
--- a/SurgSim/Physics/SolveMlcp.h
+++ b/SurgSim/Physics/SolveMlcp.h
@@ -18,8 +18,9 @@
#include <memory>
-#include "SurgSim/Physics/Computation.h"
+#include "SurgSim/Framework/Macros.h"
#include "SurgSim/Math/MlcpGaussSeidelSolver.h"
+#include "SurgSim/Physics/Computation.h"
namespace SurgSim
{
@@ -34,23 +35,43 @@ public:
/// \param doCopyState Specify if the ouput state is a copy or not of the input state in Computation::Update()
explicit SolveMlcp(bool doCopyState = false);
+ SURGSIM_CLASSNAME(SurgSim::Physics::SolveMlcp);
+
/// Destructor
virtual ~SolveMlcp();
- void setMaxIterations(int maxIterations);
+ /// Set the maximum number of iterations for the MLCP solver.
+ /// \param maxIterations The maximum number of iterations.
+ void setMaxIterations(size_t maxIterations);
- void setSolverPrecision(double epsilon);
+ /// Get the maximum number of iterations for the MLCP solver.
+ /// \return The maximum number of iterations.
+ size_t getMaxIterations() const;
+ /// Set the precision of the MLCP solver.
+ /// \param epsilon The precision.
+ void setPrecision(double epsilon);
+
+ /// Get the precision of the MLCP solver.
+ /// \return The precision.
+ double getPrecision() const;
+
+ /// Set the contact tolerance for the MLCP solver.
+ /// \param epsilon The contact tolerance.
void setContactTolerance(double epsilon);
+ /// Get the contact tolerance for the MLCP solver.
+ /// \return The contact tolerance.
+ double getContactTolerance() const;
+
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;
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+ override;
private:
diff --git a/SurgSim/Physics/Spring.cpp b/SurgSim/Physics/Spring.cpp
new file mode 100644
index 0000000..f338a3f
--- /dev/null
+++ b/SurgSim/Physics/Spring.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/Physics/Spring.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+Spring::~Spring()
+{}
+
+void Spring::initialize(const SurgSim::Math::OdeState& state)
+{
+}
+
+size_t Spring::getNumNodes() const
+{
+ return m_nodeIds.size();
+}
+
+size_t Spring::getNodeId(size_t springNodeId) const
+{
+ return m_nodeIds[springNodeId];
+}
+
+const std::vector<size_t>& Spring::getNodeIds() const
+{
+ return m_nodeIds;
+}
+
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/Spring.h b/SurgSim/Physics/Spring.h
index 345bd40..21df474 100644
--- a/SurgSim/Physics/Spring.h
+++ b/SurgSim/Physics/Spring.h
@@ -19,6 +19,7 @@
#include <vector>
#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/SparseMatrix.h"
#include "SurgSim/Math/Vector.h"
namespace SurgSim
@@ -42,29 +43,23 @@ class Spring
{
public:
/// Virtual destructor
- virtual ~Spring()
- {}
+ virtual ~Spring();
+
+ /// Initialize the Spring once everything has been set
+ /// \param state The state to initialize the Spring with
+ virtual void initialize(const SurgSim::Math::OdeState& state);
/// Gets the number of nodes the spring is connecting
/// \return The number of nodes
- size_t getNumNodes() const
- {
- return m_nodeIds.size();
- }
+ size_t getNumNodes() const;
/// Gets the springNodeId-th node id
/// \return The requested node id
- size_t getNodeId(size_t springNodeId) const
- {
- return m_nodeIds[springNodeId];
- }
+ size_t getNodeId(size_t springNodeId) const;
/// 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;
- }
+ const std::vector<size_t>& getNodeIds() 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
@@ -78,7 +73,7 @@ public:
/// \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,
+ virtual void addDamping(const SurgSim::Math::OdeState& state, SurgSim::Math::SparseMatrix* D,
double scale = 1.0) = 0;
/// Adds the spring stiffness matrix K (= -df/dx) (computed for a given state) to a complete system stiffness
@@ -86,7 +81,7 @@ public:
/// \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,
+ virtual void addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::SparseMatrix* K,
double scale = 1.0) = 0;
/// Adds the spring force vector, mass, stiffness and damping matrices (computed for a given state) into a
@@ -96,7 +91,7 @@ public:
/// \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;
+ SurgSim::Math::SparseMatrix* D, SurgSim::Math::SparseMatrix* 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)
diff --git a/SurgSim/Physics/UnitTests/BuildMlcpTests.cpp b/SurgSim/Physics/UnitTests/BuildMlcpTests.cpp
index 6201024..8777878 100644
--- a/SurgSim/Physics/UnitTests/BuildMlcpTests.cpp
+++ b/SurgSim/Physics/UnitTests/BuildMlcpTests.cpp
@@ -143,38 +143,21 @@ TEST_F(BuildMlcpTests, OneRepresentationOneConstraintTest)
m_usedRepresentations.push_back(m_fixedWorldRepresentation);
// Set the representation list in the Physics Manager State
m_physicsManagerState->setRepresentations(m_usedRepresentations);
+ // The type of constraint.
+ auto constraintType = SurgSim::Physics::FRICTIONLESS_3DCONTACT;
// 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);
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(constraintType,
+ data, m_usedRepresentations[0],
+ SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()),
+ m_fixedWorldRepresentation,
+ SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()));
// Register the constraint in the list of used constraints for this test
m_usedConstraints.push_back(constraint);
@@ -218,37 +201,26 @@ TEST_F(BuildMlcpTests, TwoRepresentationsOneConstraintSize3Test)
m_usedRepresentations.push_back(m_fixedWorldRepresentation);
// Set the representation list in the Physics Manager State
m_physicsManagerState->setRepresentations(m_usedRepresentations);
+ // The type of constraint.
+ auto constraintType = SurgSim::Physics::FIXED_3DPOINT;
+ // Add constraint to factory.
+ ConstraintImplementation::getFactory().addImplementation(
+ typeid(RigidRepresentation),
+ std::make_shared<MockRigidConstraintFixedPoint>());
+ ConstraintImplementation::getFactory().addImplementation(
+ typeid(FixedRepresentation),
+ std::make_shared<MockFixedConstraintFixedPoint>());
// 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>();
+ auto data = std::make_shared<ConstraintData>();
// Set up the constraint
- std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(
- data, rigidSideContact, rigidLocalization, fixedSideContact, fixedLocalization);
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(constraintType,
+ data, m_usedRepresentations[0],
+ SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()),
+ m_fixedWorldRepresentation,
+ SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()));
// Register the constraint in the list of used constraints for this test
m_usedConstraints.push_back(constraint);
@@ -271,7 +243,7 @@ TEST_F(BuildMlcpTests, TwoRepresentationsOneConstraintSize3Test)
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(3, mlcpProblem.mu.rows());
EXPECT_EQ(1u, mlcpProblem.constraintTypes.size());
EXPECT_EQ(SurgSim::Math::MLCP_BILATERAL_3D_CONSTRAINT, mlcpProblem.constraintTypes[0]);
EXPECT_TRUE(mlcpProblem.isConsistent());
@@ -292,72 +264,36 @@ TEST_F(BuildMlcpTests, OneRepresentationTwoConstraintsTest)
m_usedRepresentations.push_back(m_fixedWorldRepresentation);
// Set the representation list in the Physics Manager State
m_physicsManagerState->setRepresentations(m_usedRepresentations);
+ // The type of constraint.
+ auto constraintType = SurgSim::Physics::FRICTIONLESS_3DCONTACT;
// 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);
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(constraintType,
+ data, m_usedRepresentations[0],
+ SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()),
+ m_fixedWorldRepresentation,
+ SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()));
// 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);
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(constraintType,
+ data, m_usedRepresentations[0],
+ SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Ones()),
+ m_fixedWorldRepresentation,
+ SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Ones()));
// Register the constraint in the list of used constraints for this test
m_usedConstraints.push_back(constraint);
@@ -409,72 +345,36 @@ TEST_F(BuildMlcpTests, TwoRepresentationsTwoConstraintsTest)
m_usedRepresentations.push_back(m_allRepresentations[1]);
// Set the representation list in the Physics Manager State
m_physicsManagerState->setRepresentations(m_usedRepresentations);
+ // The type of constraint.
+ auto constraintType = SurgSim::Physics::FRICTIONLESS_3DCONTACT;
// 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);
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(constraintType,
+ data, m_usedRepresentations[0],
+ SurgSim::DataStructures::Location(pointOrigin),
+ m_usedRepresentations[1],
+ SurgSim::DataStructures::Location(pointOrigin));
// 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);
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(constraintType,
+ data, m_usedRepresentations[0],
+ SurgSim::DataStructures::Location(pointOrigin),
+ m_usedRepresentations[1],
+ SurgSim::DataStructures::Location(pointOne));
// Register the constraint in the list of used constraints for this test
m_usedConstraints.push_back(constraint);
diff --git a/SurgSim/Physics/UnitTests/CMakeLists.txt b/SurgSim/Physics/UnitTests/CMakeLists.txt
index 4c77286..ba86fc9 100644
--- a/SurgSim/Physics/UnitTests/CMakeLists.txt
+++ b/SurgSim/Physics/UnitTests/CMakeLists.txt
@@ -1,5 +1,5 @@
# This file is a part of the OpenSurgSim project.
-# Copyright 2012-2013, SimQuest Solutions Inc.
+# Copyright 2012-2016, SimQuest Solutions Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -15,70 +15,96 @@
include_directories(
${gtest_SOURCE_DIR}/include
+ ${gmock_SOURCE_DIR}/include
)
set(UNIT_TEST_SOURCES
BuildMlcpTests.cpp
+ CcdCollisionLoopTest.cpp
+ ComputationGroupTest.cpp
ComputationTests.cpp
ConstraintComponentTests.cpp
+ ConstraintImplementationFactoryTests.cpp
+ ConstraintImplementationTests.cpp
ConstraintTests.cpp
ContactConstraintDataTests.cpp
ContactConstraintGenerationTests.cpp
+ ContactFilteringTest.cpp
DcdCollisionTests.cpp
DeformableCollisionRepresentationTest.cpp
DeformableRepresentationTest.cpp
EigenGtestAsserts.cpp
+ Fem1DConstraintFixedPointTests.cpp
+ Fem1DConstraintFixedRotationVectorTests.cpp
+ Fem1DConstraintFrictionalSlidingTests.cpp
+ Fem1DConstraintFrictionlessContactTests.cpp
+ Fem1DConstraintFrictionlessSlidingTests.cpp
Fem1DElementBeamTests.cpp
+ Fem1DLocalizationTest.cpp
Fem1DMechanicalValidationTests.cpp
Fem1DPlyReaderDelegateTests.cpp
- Fem1DRepresentationLocalizationTest.cpp
Fem1DRepresentationTests.cpp
+ Fem2DConstraintFixedPointTests.cpp
+ Fem2DConstraintFrictionalSlidingTests.cpp
+ Fem2DConstraintFrictionlessContactTests.cpp
+ Fem2DConstraintFrictionlessSlidingTests.cpp
Fem2DElementTriangleTests.cpp
+ Fem2DLocalizationTest.cpp
Fem2DMechanicalValidationTests.cpp
Fem2DPlyReaderDelegateTests.cpp
- Fem2DRepresentationLocalizationTest.cpp
Fem2DRepresentationTests.cpp
+ Fem3DConstraintFixedPointTests.cpp
+ Fem3DConstraintFrictionalSlidingTests.cpp
+ Fem3DConstraintFrictionlessContactTests.cpp
+ Fem3DConstraintFrictionlessSlidingTests.cpp
Fem3DElementCorotationalTetrahedronTests.cpp
Fem3DElementCubeTests.cpp
Fem3DElementTetrahedronTests.cpp
+ Fem3DLocalizationTest.cpp
Fem3DPlyReaderDelegateTests.cpp
- Fem3DRepresentationBilateral3DTests.cpp
- Fem3DRepresentationContactTests.cpp
- Fem3DRepresentationLocalizationTest.cpp
Fem3DRepresentationTests.cpp
FemElementTests.cpp
- FemRepresentationParametersTest.cpp
+ FemLocalizationTest.cpp
FemRepresentationTests.cpp
- FixedRepresentationBilateral3DTests.cpp
- FixedRepresentationContactTests.cpp
- FixedRepresentationLocalizationTest.cpp
+ FixedConstraintFixedPointTests.cpp
+ FixedConstraintFixedRotationVectorTests.cpp
+ FixedConstraintFrictionlessContactTests.cpp
FixedRepresentationTest.cpp
FreeMotionTests.cpp
LinearSpringTest.cpp
+ MassSpringConstraintFixedPointTest.cpp
+ MassSpringConstraintFrictionlessContactTest.cpp
+ MassSpringLocalizationTest.cpp
MassSpringMechanicalValidationTests.cpp
- MassSpringRepresentationContactTest.cpp
- MassSpringRepresentationLocalizationTest.cpp
MassSpringRepresentationTests.cpp
MassTest.cpp
MockObjects.cpp
+ ParticleCollisionResponseTests.cpp
PhysicsManagerStateTests.cpp
PhysicsManagerTests.cpp
PostUpdateTests.cpp
+ PrepareCollisionPairsTests.cpp
PreUpdateTests.cpp
PushResultsTests.cpp
RepresentationTest.cpp
RigidCollisionRepresentationTest.cpp
- RigidRepresentationBilateral3DTests.cpp
- RigidRepresentationContactTests.cpp
- RigidRepresentationLocalizationTest.cpp
- RigidRepresentationStateTest.cpp
+ RigidConstraintFixedPointTests.cpp
+ RigidConstraintFixedRotationVectorTests.cpp
+ RigidConstraintFrictionlessContactTests.cpp
+ RigidLocalizationTest.cpp
RigidRepresentationTest.cpp
+ RigidStateTest.cpp
+ RotationVectorConstraintDataTests.cpp
+ SlidingConstraintDataTests.cpp
SolveMlcpTests.cpp
+ UpdateCollisionRepresentationsTest.cpp
VirtualToolCouplerTest.cpp
)
set(UNIT_TEST_HEADERS
CommonTests.h
+ DeformableTestsUtility.h
+ DeformableTestsUtility-inl.h
EigenGtestAsserts.h
MockObjects.h
)
@@ -86,16 +112,16 @@ set(UNIT_TEST_HEADERS
set(LIBS
SurgSimBlocks
SurgSimGraphics
- SurgSimPhysics
- SurgSimMath
SurgSimInput
+ SurgSimMath
+ SurgSimParticles
+ SurgSimPhysics
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(
diff --git a/SurgSim/Physics/UnitTests/CcdCollisionLoopTest.cpp b/SurgSim/Physics/UnitTests/CcdCollisionLoopTest.cpp
new file mode 100644
index 0000000..175a5a4
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/CcdCollisionLoopTest.cpp
@@ -0,0 +1,92 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/CcdCollisionLoop.h"
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+
+#include <gtest/gtest.h>
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/DataStructures/Location.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+TEST(CcdCollisionLoopTest, FilterContacts)
+{
+ double toi;
+ auto rep1 = std::make_shared<RigidCollisionRepresentation>("One");
+ rep1->setSelfCollisionDetectionType(Collision::COLLISION_DETECTION_TYPE_CONTINUOUS);
+ auto pair = std::make_shared<Collision::CollisionPair>(rep1, rep1);
+
+ auto computation = std::make_shared<CcdCollisionLoop>(false);
+ DataStructures::Location location;
+ std::vector<std::shared_ptr<Collision::CollisionPair>> pairs(1, pair);
+
+ EXPECT_FALSE(computation->findEarliestContact(pairs, &toi));
+
+ // Check that we find the toi correctly
+ pair->addCcdContact(0.0, 0.1, Math::Vector3d::Zero(), Math::Vector3d::Zero(),
+ std::make_pair(location, location));
+ EXPECT_TRUE(computation->findEarliestContact(pairs, &toi));
+ EXPECT_DOUBLE_EQ(0.1, toi);
+ computation->filterLaterContacts(&pairs, 0.0, toi);
+ EXPECT_EQ(1u, pair->getContacts().size());
+
+ pair->addCcdContact(0.0, 0.2, Math::Vector3d::Zero(), Math::Vector3d::Zero(),
+ std::make_pair(location, location));
+
+ pair->addCcdContact(0.0, 0.3, Math::Vector3d::Zero(), Math::Vector3d::Zero(),
+ std::make_pair(location, location));
+
+ // Check that we filter everything after the toi
+ toi = 0.0;
+ EXPECT_EQ(3u, pair->getContacts().size());
+ EXPECT_TRUE(computation->findEarliestContact(pairs, &toi));
+ computation->filterLaterContacts(&pairs, 0.0, toi);
+ EXPECT_DOUBLE_EQ(0.1, toi);
+ EXPECT_EQ(1u, pair->getContacts().size());
+}
+
+TEST(CcdCollisionLoopTest, FilterContactsWithEpsilon)
+{
+ double toi;
+ auto rep1 = std::make_shared<RigidCollisionRepresentation>("One");
+ rep1->setSelfCollisionDetectionType(Collision::COLLISION_DETECTION_TYPE_CONTINUOUS);
+ auto pair = std::make_shared<Collision::CollisionPair>(rep1, rep1);
+
+ auto computation = std::make_shared<CcdCollisionLoop>(false);
+ DataStructures::Location location;
+ std::vector<std::shared_ptr<Collision::CollisionPair>> pairs(1, pair);
+
+ // Check that we find the toi correctly
+ pair->addCcdContact(0.0, 0.1, Math::Vector3d::Zero(), Math::Vector3d::Zero(),
+ std::make_pair(location, location));
+ pair->addCcdContact(0.0, 0.2, Math::Vector3d::Zero(), Math::Vector3d::Zero(),
+ std::make_pair(location, location));
+ pair->addCcdContact(0.0, 0.3, Math::Vector3d::Zero(), Math::Vector3d::Zero(),
+ std::make_pair(location, location));
+ EXPECT_TRUE(computation->findEarliestContact(pairs, &toi));
+ computation->filterLaterContacts(&pairs, 0.11, toi);
+ // toi should be 0.1 + 0.11 i.e. toi + epsilon
+ EXPECT_DOUBLE_EQ(0.1, toi);
+ EXPECT_EQ(2u, pair->getContacts().size());
+}
+
+}
+}
diff --git a/SurgSim/Physics/UnitTests/CommonTests.h b/SurgSim/Physics/UnitTests/CommonTests.h
index 91102fe..c5e5522 100644
--- a/SurgSim/Physics/UnitTests/CommonTests.h
+++ b/SurgSim/Physics/UnitTests/CommonTests.h
@@ -27,15 +27,13 @@
#include "SurgSim/Physics/Constraint.h"
#include "SurgSim/Physics/ConstraintImplementation.h"
#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/FixedConstraintFrictionlessContact.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/RigidConstraintFrictionlessContact.h"
#include "SurgSim/Physics/RigidRepresentation.h"
-#include "SurgSim/Physics/RigidRepresentationContact.h"
-#include "SurgSim/Physics/RigidRepresentationLocalization.h"
using SurgSim::Math::BoxShape;
using SurgSim::Math::Shape;
@@ -113,22 +111,15 @@ public:
{
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().A.setZero(nbConstraint, nbConstraint);
+ m_physicsManagerState->getMlcpProblem().b.setZero(nbConstraint);
+ m_physicsManagerState->getMlcpProblem().CHt.setZero(nbDof, nbConstraint);
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().mu.setZero(nbConstraint);
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();
+ m_physicsManagerState->getMlcpSolution().x.setZero(nbConstraint);
+ m_physicsManagerState->getMlcpSolution().dofCorrection.setZero(nbDof);
}
}
diff --git a/SurgSim/Physics/UnitTests/ComputationGroupTest.cpp b/SurgSim/Physics/UnitTests/ComputationGroupTest.cpp
new file mode 100644
index 0000000..789d93a
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/ComputationGroupTest.cpp
@@ -0,0 +1,129 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/ComputationGroup.h"
+#include "SurgSim/Physics/Computation.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+
+#include <gtest/gtest.h>
+#include <gmock/gmock.h>
+
+namespace
+{
+
+typedef std::shared_ptr<SurgSim::Physics::PhysicsManagerState> State;
+
+class MockComputation : public SurgSim::Physics::Computation
+{
+public:
+ explicit MockComputation(bool val) : Computation(val)
+ {
+ testing::DefaultValue<State>::Set(std::make_shared<SurgSim::Physics::PhysicsManagerState>());
+ };
+
+ SURGSIM_CLASSNAME(MockComputation);
+
+ MOCK_METHOD2(doUpdate, State(const double&, const State&));
+
+};
+
+class MockGroup : public SurgSim::Physics::ComputationGroup
+{
+public:
+ explicit MockGroup(bool val) : ComputationGroup(val)
+ {
+ testing::DefaultValue<State>::Set(std::make_shared<SurgSim::Physics::PhysicsManagerState>());
+ ON_CALL(*this, endIteration()).WillByDefault(testing::Return(true));
+ }
+ MOCK_METHOD0(endIteration, bool(void));
+};
+
+
+}
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+TEST(ComputationGroupTest, Empty)
+{
+ auto state = std::make_shared<PhysicsManagerState>();
+ auto computation = std::make_shared<ComputationGroup>(false);
+
+ EXPECT_NO_THROW(computation->update(0.0, state));
+}
+
+TEST(ComputationGroupTest, RunOnce)
+{
+ using ::testing::_;
+ auto state = std::make_shared<PhysicsManagerState>();
+ auto computation = std::make_shared<MockGroup>(false);
+ auto mock = std::make_shared<MockComputation>(false);
+
+ computation->addComputation(mock);
+
+ EXPECT_CALL(*mock, doUpdate(0.0, _));
+ EXPECT_CALL(*computation, endIteration());
+
+ computation->update(0.0, state);
+}
+
+TEST(ComputationGroupTest, MultiplesRunMultiples)
+{
+ using ::testing::_;
+ using ::testing::Return;
+ auto state = std::make_shared<PhysicsManagerState>();
+ auto computation = std::make_shared<MockGroup>(false);
+ auto mock = std::make_shared<MockComputation>(false);
+
+ computation->addComputation(mock);
+ computation->addComputation(mock);
+
+ EXPECT_CALL(*mock, doUpdate(_, _)).Times(4);
+ EXPECT_CALL(*computation, endIteration()).Times(2).WillOnce(Return(false)).WillOnce(Return(true));
+
+ computation->update(0.0, state);
+}
+
+TEST(ComputationGroupTest, AbortInTheMiddle)
+{
+ using ::testing::_;
+ using ::testing::Return;
+ std::vector<std::shared_ptr<Computation>> computations;
+ auto state = std::make_shared<PhysicsManagerState>();
+ auto state2 = std::make_shared<PhysicsManagerState>();
+ state2->setAbortGroup(true);
+ auto computation = std::make_shared<MockGroup>(false);
+ auto mock = std::make_shared<MockComputation>(false);
+ computations.push_back(mock);
+ computations.push_back(mock);
+
+ computation->setComputations(computations);
+
+ EXPECT_CALL(*mock, doUpdate(_, _)).Times(3)
+ .WillOnce(Return(state))
+ .WillOnce(Return(state))
+ .WillOnce(Return(state2));
+
+ EXPECT_CALL(*computation, endIteration()).WillOnce(Return(false));
+
+ computation->update(0.0, state);
+
+ ASSERT_FALSE(state2->shouldAbortGroup());
+}
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Physics/UnitTests/ComputationTests.cpp b/SurgSim/Physics/UnitTests/ComputationTests.cpp
index 023b114..ef45732 100644
--- a/SurgSim/Physics/UnitTests/ComputationTests.cpp
+++ b/SurgSim/Physics/UnitTests/ComputationTests.cpp
@@ -16,37 +16,19 @@
#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/RigidConstraintFrictionlessContact.h"
#include "SurgSim/Physics/RigidRepresentation.h"
-#include "SurgSim/Physics/RigidRepresentationContact.h"
-#include "SurgSim/Physics/RigidRepresentationLocalization.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.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;});
@@ -80,6 +62,7 @@ TEST(ComputationTests, PreparePhysicsState)
// Setup the state.
std::vector<std::shared_ptr<Representation>> expectedRepresentations;
+ std::vector<std::shared_ptr<SurgSim::Collision::Representation>> expectedCollisionRepresentations;
// Add a representation.
auto rigid1 = std::make_shared<RigidRepresentation>("rigid1");
@@ -90,40 +73,36 @@ TEST(ComputationTests, PreparePhysicsState)
auto collisionRepresentation = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("rigid2 collision");
rigid2->setCollisionRepresentation(collisionRepresentation);
expectedRepresentations.push_back(rigid2);
+ expectedCollisionRepresentations.push_back(collisionRepresentation);
physicsState->setRepresentations(expectedRepresentations);
// Add a constraint.
std::vector<std::shared_ptr<Constraint>> expectedConstraints;
+ // Constraint type.
+ auto constraintType = SurgSim::Physics::FRICTIONLESS_3DCONTACT;
{
- // Create first side of a constraint.
+ // Create first representation.
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.
+ // Create second representation.
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);
+ expectedConstraints.push_back(std::make_shared<Constraint>(constraintType, data,
+ rigid1, SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()),
+ rigid2, SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero())));
physicsState->setConstraintGroup(SurgSim::Physics::CONSTRAINT_GROUP_TYPE_CONTACT, expectedConstraints);
}
+ // Add a collision representation.
+ auto collisionRepresentation2 = std::make_shared<SurgSim::Physics::RigidCollisionRepresentation>("collision2");
+ collisionRepresentation2->setLocalActive(false);
+ expectedCollisionRepresentations.push_back(collisionRepresentation2);
+ physicsState->setCollisionRepresentations(expectedCollisionRepresentations);
+
// Call update on Computation.
MockComputation c;
physicsState = c.update(0.0, physicsState);
@@ -140,6 +119,12 @@ TEST(ComputationTests, PreparePhysicsState)
actualConstraints = physicsState->getActiveConstraints();
ASSERT_EQ(1, actualConstraints.size());
EXPECT_EQ(expectedConstraints.front(), actualConstraints.front());
+
+ // Check the active collisions list.
+ std::vector<std::shared_ptr<SurgSim::Collision::Representation>> actualCollisionRepresentations;
+ actualCollisionRepresentations = physicsState->getActiveCollisionRepresentations();
+ ASSERT_EQ(1, actualCollisionRepresentations.size());
+ EXPECT_EQ(collisionRepresentation, actualCollisionRepresentations.front());
}
}; // namespace Physics
diff --git a/SurgSim/Physics/UnitTests/ConstraintImplementationFactoryTests.cpp b/SurgSim/Physics/UnitTests/ConstraintImplementationFactoryTests.cpp
new file mode 100644
index 0000000..0b8df06
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/ConstraintImplementationFactoryTests.cpp
@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Physics/ConstraintImplementationFactory.h"
+
+#include "SurgSim/Blocks/MassSpring1DRepresentation.h"
+#include "SurgSim/Blocks/MassSpring2DRepresentation.h"
+#include "SurgSim/Blocks/MassSpring3DRepresentation.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/FixedConstraintFrictionlessContact.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+TEST(ConstraintImplementationFactoryTest, GetImplementationTest)
+{
+ ConstraintImplementationFactory factory;
+ EXPECT_TRUE(factory.getImplementation(typeid(FixedRepresentation), FRICTIONLESS_3DCONTACT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(RigidRepresentation), FRICTIONLESS_3DCONTACT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(Fem3DRepresentation), FRICTIONLESS_3DCONTACT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(FixedRepresentation), FIXED_3DPOINT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(RigidRepresentation), FIXED_3DPOINT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(Fem1DRepresentation), FIXED_3DPOINT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(Fem2DRepresentation), FIXED_3DPOINT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(Fem3DRepresentation), FIXED_3DPOINT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(Fem1DRepresentation), FRICTIONLESS_SLIDING)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(Fem2DRepresentation), FRICTIONLESS_SLIDING)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(Fem3DRepresentation), FRICTIONLESS_SLIDING)
+ != nullptr);
+
+ EXPECT_TRUE(factory.getImplementation(typeid(MassSpringRepresentation), FRICTIONLESS_3DCONTACT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(SurgSim::Blocks::MassSpring1DRepresentation), FRICTIONLESS_3DCONTACT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(SurgSim::Blocks::MassSpring2DRepresentation), FRICTIONLESS_3DCONTACT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(SurgSim::Blocks::MassSpring3DRepresentation), FRICTIONLESS_3DCONTACT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(MassSpringRepresentation), FIXED_3DPOINT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(SurgSim::Blocks::MassSpring1DRepresentation), FIXED_3DPOINT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(SurgSim::Blocks::MassSpring2DRepresentation), FIXED_3DPOINT)
+ != nullptr);
+ EXPECT_TRUE(factory.getImplementation(typeid(SurgSim::Blocks::MassSpring3DRepresentation), FIXED_3DPOINT)
+ != nullptr);
+}
+
+TEST(ConstraintImplementationFactoryTest, AddImplementationTest)
+{
+
+ class TestRepresentation {};
+ ConstraintImplementationFactory factory;
+
+ EXPECT_TRUE(factory.getImplementation(typeid(TestRepresentation), FRICTIONLESS_3DCONTACT)
+ == nullptr);
+
+ // Add implementation for TestRepresentation.
+ EXPECT_NO_THROW(
+ factory.addImplementation(typeid(TestRepresentation), std::make_shared<FixedConstraintFrictionlessContact>()));
+
+ EXPECT_TRUE(factory.getImplementation(typeid(TestRepresentation), FRICTIONLESS_3DCONTACT)
+ != nullptr);
+
+ // Adding it again to make sure everything works, even if duplicate implementations are added.
+ EXPECT_NO_THROW(
+ factory.addImplementation(typeid(TestRepresentation), std::make_shared<FixedConstraintFrictionlessContact>()));
+
+ EXPECT_TRUE(factory.getImplementation(typeid(TestRepresentation), FRICTIONLESS_3DCONTACT)
+ != nullptr);
+}
+
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/ConstraintImplementationTests.cpp b/SurgSim/Physics/UnitTests/ConstraintImplementationTests.cpp
new file mode 100644
index 0000000..0953389
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/ConstraintImplementationTests.cpp
@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Physics/ConstraintImplementation.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+TEST(ConstraintImplementationTest, FactoryTest)
+{
+ ASSERT_NO_THROW(ConstraintImplementation::getFactory(););
+}
+
+} // namespace Physics
+} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/ConstraintTests.cpp b/SurgSim/Physics/UnitTests/ConstraintTests.cpp
index ce84c46..b7cba76 100644
--- a/SurgSim/Physics/UnitTests/ConstraintTests.cpp
+++ b/SurgSim/Physics/UnitTests/ConstraintTests.cpp
@@ -16,28 +16,28 @@
#include <memory>
#include <gtest/gtest.h>
+
+#include "SurgSim/DataStructures/Location.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/ConstraintData.h"
#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/FixedConstraintFrictionlessContact.h"
#include "SurgSim/Physics/FixedRepresentation.h"
-#include "SurgSim/Physics/FixedRepresentationContact.h"
-#include "SurgSim/Physics/FixedRepresentationLocalization.h"
#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/RigidConstraintFrictionlessContact.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::DataStructures::Location;
using SurgSim::Math::SphereShape;
using SurgSim::Math::Vector3d;
namespace
{
- const double epsilon = 1e-10;
+const double epsilon = 1e-10;
};
namespace SurgSim
@@ -82,14 +82,10 @@ protected:
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;
+ /// Location on the fixed plane
+ Location m_locFixedPlane;
+ /// Location on the rigid sphere
+ Location m_locRigidSphere;
/// Total number of degrees of freedom in the system (plane + sphere)
size_t m_numDof;
@@ -131,18 +127,8 @@ protected:
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_locFixedPlane.rigidLocalPosition = m_contactPositionPlane;
+ m_locRigidSphere.rigidLocalPosition = m_contactPositionSphere;
m_constraintData = std::make_shared<ContactConstraintData>();
@@ -155,154 +141,108 @@ protected:
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.A.setZero(numConstraint, numConstraint);
+ m_mlcpPhysicsProblem.b.setZero(numConstraint);
+ m_mlcpPhysicsProblem.mu.setZero(numConstraint);
+ m_mlcpPhysicsProblem.CHt.setZero(numDof, numConstraint);
m_mlcpPhysicsProblem.H.resize(numConstraint, numDof);
- m_mlcpPhysicsProblem.H.setZero();
// Empty all std::vector types
m_mlcpPhysicsProblem.constraintTypes.clear();
}
};
-TEST_F (ConstraintTests, TestConstructor)
+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>();
+ Location fixedLoc(m_contactPositionPlane);
+ Location rigidLoc(m_contactPositionSphere);
- fixedLoc->setRepresentation(fixedRep);
- rigidLoc->setRepresentation(rigidRep);
+ auto type = SurgSim::Physics::FRICTIONLESS_3DCONTACT;
- std::shared_ptr<ConstraintImplementation> fixedImp = std::make_shared<FixedRepresentationContact>();
- std::shared_ptr<ConstraintImplementation> rigidImp = std::make_shared<RigidRepresentationContact>();
+ ASSERT_NO_THROW({Constraint c(type, m_constraintData, fixedRep, fixedLoc, rigidRep, rigidLoc);});
- {
- 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);
- }
+ EXPECT_THROW(
+ { Constraint c(type, nullptr, nullptr, fixedLoc, nullptr, fixedLoc); },
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(
+ { Constraint c(type, m_constraintData, nullptr, fixedLoc, nullptr, fixedLoc); },
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(
+ { Constraint c(type, m_constraintData, fixedRep, fixedLoc, nullptr, fixedLoc); },
+ 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);
+ Constraint c(type, m_constraintData, fixedRep, fixedLoc, rigidRep, 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);
+ EXPECT_EQ(type, c.getImplementations().first->getConstraintType());
+ EXPECT_EQ(type, c.getImplementations().second->getConstraintType());
+ EXPECT_EQ(fixedRep, c.getLocalizations().first->getRepresentation());
+ EXPECT_EQ(rigidRep, c.getLocalizations().second->getRepresentation());
}
-TEST_F (ConstraintTests, TestGetNumDof)
+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);
+ Location fixedLoc(m_contactPositionPlane);
+ Location rigidLoc(m_contactPositionSphere);
- std::shared_ptr<ConstraintImplementation> fixedImp = std::make_shared<FixedRepresentationContact>();
- std::shared_ptr<ConstraintImplementation> rigidImp = std::make_shared<RigidRepresentationContact>();
+ auto type = SurgSim::Physics::FRICTIONLESS_3DCONTACT;
{
SCOPED_TRACE("1DOF for a frictionless contact");
- Constraint c(m_constraintData,
- m_implementationFixedPlane, m_locFixedPlane,
- m_implementationRigidSphere, m_locRigidSphere);
+ Constraint c(type, m_constraintData,
+ m_fixed, m_locFixedPlane,
+ m_rigid, 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);
+ Constraint c(type, m_constraintData, fixedRep, fixedLoc, fixedRep, 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);
+ Constraint c(type, m_constraintData, fixedRep, fixedLoc, rigidRep, rigidLoc);
EXPECT_EQ(1u, c.getNumDof());
}
}
+TEST_F(ConstraintTests, SetActive)
+{
+ auto type = SurgSim::Physics::FRICTIONLESS_3DCONTACT;
+ m_n.setZero();
+ m_n[1] = -1.0;
+ m_constraintData->setPlaneEquation(m_n, m_d);
+ m_constraint = std::make_shared<Constraint>(type, m_constraintData,
+ m_fixed, m_locFixedPlane,
+ m_rigid, m_locRigidSphere);
+
+ EXPECT_TRUE(m_constraint->isActive());
+ m_constraint->setActive(false);
+ EXPECT_FALSE(m_constraint->isActive());
+}
+
// 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)
+TEST_F(ConstraintTests, TestBuildMlcpSpherePlane)
{
+ auto type =SurgSim::Physics::FRICTIONLESS_3DCONTACT;
+ auto mlcptype = SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;
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);
+ m_constraint = std::make_shared<Constraint>(type, m_constraintData,
+ m_rigid, m_locRigidSphere,
+ m_fixed, m_locFixedPlane);
// Simulate 1 time step...to make sure all representation have a valid compliance matrix...
{
@@ -318,7 +258,7 @@ TEST_F (ConstraintTests, TestBuildMlcpSpherePlane)
// Fill up the Mlcp
m_constraint->build(m_dt, &m_mlcpPhysicsProblem, m_indexSphereRepresentation, m_indexPlaneRepresentation,
- m_indexConstraint);
+ 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
@@ -330,30 +270,33 @@ TEST_F (ConstraintTests, TestBuildMlcpSpherePlane)
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);
+ SurgSim::Math::Matrix H = m_mlcpPhysicsProblem.H;
+ EXPECT_NEAR(sign * m_dt * m_n[0] , H(0, 0), epsilon);
+ EXPECT_NEAR(sign * m_dt * m_n[1] , H(0, 1), epsilon);
+ EXPECT_NEAR(sign * m_dt * m_n[2] , H(0, 2), epsilon);
+ EXPECT_NEAR(sign * m_dt * n_GP[0], H(0, 3), epsilon);
+ EXPECT_NEAR(sign * m_dt * n_GP[1], H(0, 4), epsilon);
+ EXPECT_NEAR(sign * m_dt * n_GP[2], 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]);
+ EXPECT_EQ(mlcptype, 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)
+TEST_F(ConstraintTests, TestBuildMlcpPlaneSphere)
{
+ auto type = SurgSim::Physics::FRICTIONLESS_3DCONTACT;
+ auto mlcptype = SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;
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);
+ m_constraint = std::make_shared<Constraint>(type, m_constraintData,
+ m_fixed, m_locFixedPlane,
+ m_rigid, m_locRigidSphere);
// Simulate 1 time step...to make sure all representation have a valid compliance matrix...
{
@@ -369,7 +312,7 @@ TEST_F (ConstraintTests, TestBuildMlcpPlaneSphere)
// Fill up the Mlcp
m_constraint->build(m_dt, &m_mlcpPhysicsProblem, m_indexPlaneRepresentation, m_indexSphereRepresentation,
- m_indexConstraint);
+ 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
@@ -381,16 +324,17 @@ TEST_F (ConstraintTests, TestBuildMlcpPlaneSphere)
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);
+ SurgSim::Math::Matrix h = m_mlcpPhysicsProblem.H;
+ EXPECT_NEAR(sign * m_dt * m_n[0] , h(0, 0), epsilon);
+ EXPECT_NEAR(sign * m_dt * m_n[1] , h(0, 1), epsilon);
+ EXPECT_NEAR(sign * m_dt * m_n[2] , h(0, 2), epsilon);
+ EXPECT_NEAR(sign * m_dt * n_GP[0], h(0, 3), epsilon);
+ EXPECT_NEAR(sign * m_dt * n_GP[1], h(0, 4), epsilon);
+ EXPECT_NEAR(sign * m_dt * n_GP[2], 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]);
+ EXPECT_EQ(mlcptype, m_mlcpPhysicsProblem.constraintTypes[0]);
}
}; // namespace Physics
diff --git a/SurgSim/Physics/UnitTests/ContactConstraintDataTests.cpp b/SurgSim/Physics/UnitTests/ContactConstraintDataTests.cpp
index 467ec6a..0a86f25 100644
--- a/SurgSim/Physics/UnitTests/ContactConstraintDataTests.cpp
+++ b/SurgSim/Physics/UnitTests/ContactConstraintDataTests.cpp
@@ -33,6 +33,9 @@ namespace
TEST (ContactConstraintDataTests, TestSetGet)
{
+ using SurgSim::Collision::Contact;
+ using SurgSim::DataStructures::Location;
+
ContactConstraintData contactConstraintData;
Vector3d n(1.2, 4.5, 6.7);
double d = 5.566;
@@ -45,4 +48,10 @@ TEST (ContactConstraintDataTests, TestSetGet)
EXPECT_NEAR(d , contactConstraintData.getDistance(), epsilon);
EXPECT_TRUE(contactConstraintData.getNormal().isApprox(n, epsilon));
+
+ auto contact0 = std::make_shared<Contact>(SurgSim::Collision::COLLISION_DETECTION_TYPE_DISCRETE, 0.0, 0.0,
+ Vector3d::Zero(), Vector3d::UnitX(), std::pair<Location, Location>());
+ contactConstraintData.setContact(contact0);
+
+ EXPECT_TRUE(contactConstraintData.getContact() == contact0);
}
diff --git a/SurgSim/Physics/UnitTests/ContactConstraintGenerationTests.cpp b/SurgSim/Physics/UnitTests/ContactConstraintGenerationTests.cpp
index 40066a8..9e880dc 100644
--- a/SurgSim/Physics/UnitTests/ContactConstraintGenerationTests.cpp
+++ b/SurgSim/Physics/UnitTests/ContactConstraintGenerationTests.cpp
@@ -19,7 +19,7 @@
#include <utility>
#include "SurgSim/Collision/CollisionPair.h"
-#include "SurgSim/Collision/DcdCollision.h"
+#include "SurgSim/Collision/CcdDcdCollision.h"
#include "SurgSim/Collision/Representation.h"
#include "SurgSim/Math/DoubleSidedPlaneShape.h"
#include "SurgSim/Math/Quaternion.h"
@@ -63,6 +63,9 @@ struct ContactConstraintGenerationTests: public ::testing::Test
state = std::make_shared<PhysicsManagerState>();
state->setRepresentations(representations);
+
+ collision0->update(0.0);
+ collision1->update(0.0);
}
virtual void TearDown()
@@ -86,7 +89,7 @@ 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;
+ SurgSim::Collision::SphereDoubleSidedPlaneContact contactCalculation;
contactCalculation.calculateContact(pair);
ASSERT_TRUE(pair->hasContacts());
@@ -118,7 +121,7 @@ TEST_F(ContactConstraintGenerationTests, BasicTest)
TEST_F(ContactConstraintGenerationTests, CountTest)
{
std::shared_ptr<CollisionPair> pair;
- SurgSim::Collision::SphereDoubleSidedPlaneDcdContact contactCalculation;
+ SurgSim::Collision::SphereDoubleSidedPlaneContact contactCalculation;
pair = std::make_shared<CollisionPair>(collision0, collision1);
contactCalculation.calculateContact(pair);
@@ -146,7 +149,7 @@ TEST_F(ContactConstraintGenerationTests, CountTest)
TEST_F(ContactConstraintGenerationTests, InactivePhysics)
{
std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(collision0, collision1);
- SurgSim::Collision::SphereDoubleSidedPlaneDcdContact contactCalculation;
+ SurgSim::Collision::SphereDoubleSidedPlaneContact contactCalculation;
contactCalculation.calculateContact(pair);
pairs.push_back(pair);
state->setCollisionPairs(pairs);
@@ -179,7 +182,7 @@ TEST_F(ContactConstraintGenerationTests, LocalPoses)
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;
+ SurgSim::Collision::SphereDoubleSidedPlaneContact contactCalculation;
contactCalculation.calculateContact(pair);
ASSERT_EQ(1u, pair->getContacts().size());
@@ -198,7 +201,7 @@ TEST_F(ContactConstraintGenerationTests, LocalPoses)
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";
+ "The contact location is not in the same position as the localization produced by constraint generation";
}
diff --git a/SurgSim/Physics/UnitTests/ContactFilteringTest.cpp b/SurgSim/Physics/UnitTests/ContactFilteringTest.cpp
new file mode 100644
index 0000000..a9ff121
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/ContactFilteringTest.cpp
@@ -0,0 +1,145 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Collision/CollisionPair.h"
+#include "SurgSim/Collision/ContactFilter.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Physics/ContactFiltering.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+
+using ::testing::_;
+
+namespace SurgSim
+{
+
+class MockContactFilter : public Collision::ContactFilter
+{
+public:
+ explicit MockContactFilter(const std::string& name) : Collision::ContactFilter(name) {}
+ MOCK_METHOD0(doWakeUp, bool());
+ MOCK_METHOD0(doInitialize, bool());
+ MOCK_METHOD1(doUpdate, void(double));
+ MOCK_METHOD2(doFilterContacts, void(const std::shared_ptr<Physics::PhysicsManagerState>& state,
+ const std::shared_ptr<Collision::CollisionPair>& pair));
+
+
+};
+
+void removeOne(const std::shared_ptr<Physics::PhysicsManagerState>& state,
+ const std::shared_ptr<Collision::CollisionPair>& pair)
+{
+ pair->getContacts().pop_back();
+}
+
+namespace Physics
+{
+
+
+struct ContactFilteringTest : public ::testing::Test
+{
+ virtual void SetUp()
+ {
+ // Setup Framework
+ runtime = std::make_shared<Framework::Runtime>();
+ state = std::make_shared<PhysicsManagerState>();
+ contactFiltering = std::make_shared<ContactFiltering>(false);
+
+ std::vector<std::shared_ptr<Collision::ContactFilter>> filters;
+ filter = std::make_shared<MockContactFilter>("Filter");
+ filters.push_back(filter);
+ state->setContactFilters(filters);
+
+ collision0 = std::make_shared<Physics::RigidCollisionRepresentation>("Collision0");
+
+
+ pairWithContacts = std::make_shared<Collision::CollisionPair>(collision0, collision0);
+ auto contact = std::make_shared<Collision::Contact>(Collision::COLLISION_DETECTION_TYPE_NONE,
+ 0.0, 0.0, Math::Vector3d::Zero(), Math::Vector3d::Zero(),
+ std::make_pair(DataStructures::Location(), DataStructures::Location()));
+ pairWithContacts->addContact(contact);
+ pairWithContacts->addContact(contact);
+ pairWithoutContacts = std::make_shared<Collision::CollisionPair>(collision0, collision0);
+
+ }
+
+ std::shared_ptr<PhysicsManagerState> state;
+ std::shared_ptr<ContactFiltering> contactFiltering;
+ std::shared_ptr<Framework::Runtime> runtime;
+ std::shared_ptr<MockContactFilter> filter;
+
+ std::shared_ptr<Physics::RigidCollisionRepresentation> collision0;
+
+ std::shared_ptr<Collision::CollisionPair> pairWithContacts;
+ std::shared_ptr<Collision::CollisionPair> pairWithoutContacts;
+
+ std::vector<std::shared_ptr<Collision::CollisionPair>> pairs;
+
+};
+
+TEST_F(ContactFilteringTest, DontProcessWithoutPairs)
+{
+ EXPECT_CALL(*filter, doFilterContacts(_, _)).Times(0);
+ contactFiltering->update(1.0, state);
+}
+
+
+TEST_F(ContactFilteringTest, ProcessAllPairsWithContacts)
+{
+ pairs.push_back(pairWithContacts);
+ pairs.push_back(pairWithContacts);
+ pairs.push_back(pairWithoutContacts);
+ state->setCollisionPairs(pairs);
+
+ EXPECT_CALL(*filter, doFilterContacts(_, _)).Times(2);
+ contactFiltering->update(1.0, state);
+}
+
+
+TEST_F(ContactFilteringTest, ModifyContacts)
+{
+ pairs.push_back(pairWithContacts);
+
+ state->setCollisionPairs(pairs);
+
+ EXPECT_CALL(*filter, doFilterContacts(_, _)).WillOnce(testing::Invoke(removeOne));
+ contactFiltering->update(1.0, state);
+ EXPECT_EQ(1u, pairWithContacts->getContacts().size());
+}
+
+TEST_F(ContactFilteringTest, ProcessAllFilters)
+{
+ // Gmock is might not be threadsafe on windows, need separate instances
+ std::vector<std::shared_ptr<Collision::ContactFilter>> filters;
+ auto filter1 = std::make_shared<MockContactFilter>("Filter1");
+ filters.push_back(filter1);
+ auto filter2 = std::make_shared<MockContactFilter>("Filter2");
+ filters.push_back(filter2);
+ state->setContactFilters(filters);
+
+ pairs.push_back(pairWithContacts);
+ state->setCollisionPairs(pairs);
+
+ EXPECT_CALL(*filter1, doFilterContacts(_, _)).Times(1);
+ EXPECT_CALL(*filter2, doFilterContacts(_, _)).Times(1);
+ contactFiltering->update(1.0, state);
+}
+
+}
+
+}
\ No newline at end of file
diff --git a/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem1DMaterial.ply b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem1DMaterial.ply
new file mode 100644
index 0000000..31e717a
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem1DMaterial.ply
@@ -0,0 +1,32 @@
+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
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+element boundary_condition 3
+property uint vertex_index
+element radius 1
+property double value
+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 1.0 2.0 3.0
+2 2 6 4.0 5.0 6.0
+2 3 5 7.0 8.0 9.0
+2 4 3 10.0 11.0 12.0
+2
+4
+5
+0.11
diff --git a/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem1DNoMaterial.ply b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem1DNoMaterial.ply
new file mode 100644
index 0000000..c3c5168
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem1DNoMaterial.ply
@@ -0,0 +1,29 @@
+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
+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
diff --git a/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem2DMaterial.ply b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem2DMaterial.ply
new file mode 100644
index 0000000..f00be78
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem2DMaterial.ply
@@ -0,0 +1,29 @@
+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
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+element boundary_condition 2
+property uint vertex_index
+element thickness 1
+property double value
+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 1.0 2.0 3.0
+3 1 2 4 4.0 5.0 6.0
+3 3 4 5 7.0 8.0 9.0
+3
+2
+0.1
\ No newline at end of file
diff --git a/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem2DNoMaterial.ply b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem2DNoMaterial.ply
new file mode 100644
index 0000000..a4cce0b
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem2DNoMaterial.ply
@@ -0,0 +1,26 @@
+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
+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
\ No newline at end of file
diff --git a/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem3DCubeMaterial.ply b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem3DCubeMaterial.ply
new file mode 100644
index 0000000..fb1605f
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem3DCubeMaterial.ply
@@ -0,0 +1,30 @@
+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
+property double mass_density
+property double poisson_ratio
+property double young_modulus
+element boundary_condition 2
+property uint vertex_index
+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 1.0 2.0 3.0
+8 1 2 4 5 6 7 8 9 4.0 5.0 6.0
+8 3 4 5 0 2 6 8 7 7.0 8.0 9.0
+9
+5
\ No newline at end of file
diff --git a/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem3DCubeNoMaterial.ply b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem3DCubeNoMaterial.ply
new file mode 100644
index 0000000..5b01ef9
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Fem3DCubeNoMaterial.ply
@@ -0,0 +1,27 @@
+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
+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
\ No newline at end of file
diff --git a/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Wrong3DFileWithRotationData.ply b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Wrong3DFileWithRotationData.ply
new file mode 100644
index 0000000..a74fab8
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Data/PlyReaderTests/Wrong3DFileWithRotationData.ply
@@ -0,0 +1,26 @@
+ply
+format ascii 1.0
+comment Created by hand
+element vertex 4
+property double x
+property double y
+property double z
+property double thetax
+property double thetay
+property double thetaZ
+element 3d_element 1
+property list uint uint vertex_indices
+element boundary_condition 1
+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 0.0 0.0 0.0
+1.000000 -1.000000 -1.000000 0.0 0.0 0.0
+-1.000000 -1.000000 -1.000000 0.0 0.0 0.0
+1.000000 0.999999 1.000000 0.0 0.0 0.0
+4 0 1 2 3
+0
+0.1432 0.224 0.472
diff --git a/SurgSim/Physics/UnitTests/DcdCollisionTests.cpp b/SurgSim/Physics/UnitTests/DcdCollisionTests.cpp
index 93cd186..d103a3a 100644
--- a/SurgSim/Physics/UnitTests/DcdCollisionTests.cpp
+++ b/SurgSim/Physics/UnitTests/DcdCollisionTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,109 +16,107 @@
/// \file DcdCollisionTests.cpp
/// Tests for the DcdCollision Class
+#include <gtest/gtest.h>
#include <memory>
#include <vector>
-#include <gtest/gtest.h>
-
+#include "SurgSim/Blocks/SphereElement.h"
#include "SurgSim/Collision/CollisionPair.h"
-#include "SurgSim/Math/DoubleSidedPlaneShape.h"
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.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/PhysicsManager.h"
#include "SurgSim/Physics/PhysicsManagerState.h"
-#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/PrepareCollisionPairs.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));
+namespace SurgSim
+{
+namespace Physics
+{
- return representation;
+TEST(DcdCollisionTest, ConstructorTest)
+{
+ ASSERT_NO_THROW(std::make_shared<DcdCollision>(false));
+ ASSERT_NO_THROW(std::make_shared<DcdCollision>(true));
}
-
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());
+ auto sphere1 = std::make_shared<Blocks::SphereElement>("Sphere1");
+ auto sphere2 = std::make_shared<Blocks::SphereElement>("Sphere2");
+ sphere2->setPose(Math::makeRigidTransform(Math::Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.5)));
+
+ auto runtime = std::make_shared<Framework::Runtime>();
+ auto scene = runtime->getScene();
+ scene->addSceneElement(sphere1);
+ scene->addSceneElement(sphere2);
+
+ std::vector<std::shared_ptr<Physics::Representation>> physicsRepresentations;
+ physicsRepresentations.push_back(sphere1->getComponents<Physics::Representation>()[0]);
+ physicsRepresentations.push_back(sphere2->getComponents<Physics::Representation>()[0]);
+
+ auto sphere1Collision = sphere1->getComponents<Collision::Representation>()[0];
+ auto sphere2Collision = sphere2->getComponents<Collision::Representation>()[0];
+ sphere1Collision->update(0.0);
+ sphere2Collision->update(0.0);
+ std::vector<std::shared_ptr<Collision::Representation>> collisionRepresentations;
+ collisionRepresentations.push_back(sphere1Collision);
+ collisionRepresentations.push_back(sphere2Collision);
+
+ auto prepareState = std::make_shared<PrepareCollisionPairs>(false);
+ auto stateTmp = std::make_shared<PhysicsManagerState>();
+ stateTmp->setRepresentations(physicsRepresentations);
+ stateTmp->setCollisionRepresentations(collisionRepresentations);
+ auto state = prepareState->update(0.0, stateTmp); // This step prepares the collision pairs
+ ASSERT_EQ(1u, state->getCollisionPairs().size());
+
+ auto dcdCollision = std::make_shared<DcdCollision>(false);
+ std::shared_ptr<PhysicsManagerState> newState = dcdCollision->update(1.0, state);
EXPECT_TRUE(newState->getCollisionPairs().at(0)->hasContacts());
}
-TEST(DcdCollisionTest, FixedRigidCollisionTest)
+TEST(DcdCollisionTest, Deactivate)
{
- 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());
+ auto runtime = std::make_shared<Framework::Runtime>();
+ auto physicsManager = std::make_shared<PhysicsManager>();
+ runtime->addManager(physicsManager);
+
+ auto sphere1 = std::make_shared<Collision::ShapeCollisionRepresentation>("Sphere1");
+ sphere1->setShape(std::make_shared<Math::SphereShape>(1.0));
+
+ auto sphere2 = std::make_shared<Collision::ShapeCollisionRepresentation>("Sphere2");
+ sphere2->setShape(std::make_shared<Math::SphereShape>(1.0));
+ sphere2->setLocalPose(Math::makeRigidTransform(Math::Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.5)));
+
+ auto scene = runtime->getScene();
+ auto element = std::make_shared<Framework::BasicSceneElement>("Element");
+ element->addComponent(sphere2);
+ element->addComponent(sphere1);
+ scene->addSceneElement(element);
+
+ runtime->start(true);
+ runtime->step();
+ runtime->step();
+ EXPECT_TRUE(sphere1->collidedWith(sphere2));
+ EXPECT_TRUE(sphere2->collidedWith(sphere1));
+
+ sphere1->setLocalActive(false);
+ runtime->step();
+ runtime->step();
+ EXPECT_FALSE(sphere1->collidedWith(sphere2));
+ EXPECT_FALSE(sphere2->collidedWith(sphere1));
+
+ boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+ runtime->stop();
}
+
+};
+};
diff --git a/SurgSim/Physics/UnitTests/DeformableCollisionRepresentationTest.cpp b/SurgSim/Physics/UnitTests/DeformableCollisionRepresentationTest.cpp
index a13f7da..5a8fd1e 100644
--- a/SurgSim/Physics/UnitTests/DeformableCollisionRepresentationTest.cpp
+++ b/SurgSim/Physics/UnitTests/DeformableCollisionRepresentationTest.cpp
@@ -84,13 +84,13 @@ TEST_F(DeformableCollisionRepresentationTest, ShapeTest)
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());
+ m_deformableCollisionRepresentation->setShape(m_meshShape);
+ auto actualShape = std::dynamic_pointer_cast<SurgSim::Math::MeshShape>(
+ m_deformableCollisionRepresentation->getShape());
+ ASSERT_NE(nullptr, actualShape);
+ EXPECT_EQ(m_meshShape->getNumVertices(), actualShape->getNumVertices());
+ EXPECT_EQ(m_meshShape->getNumEdges(), actualShape->getNumEdges());
+ EXPECT_EQ(m_meshShape->getNumTriangles(), actualShape->getNumTriangles());
}
TEST_F(DeformableCollisionRepresentationTest, SerializationTest)
@@ -103,9 +103,9 @@ TEST_F(DeformableCollisionRepresentationTest, SerializationTest)
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>>())
- );
+ 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);
@@ -116,17 +116,15 @@ TEST_F(DeformableCollisionRepresentationTest, SerializationTest)
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());
+ EXPECT_EQ(m_meshShape->getNumVertices(), mesh->getNumVertices());
+ EXPECT_EQ(m_meshShape->getNumEdges(), mesh->getNumEdges());
+ EXPECT_EQ(m_meshShape->getNumTriangles(), mesh->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);
+ fem3DRepresentation->loadFem(m_filename);
// Member data 'odeState' will be created while loading.
ASSERT_TRUE(fem3DRepresentation->initialize(m_runtime));
@@ -137,8 +135,17 @@ TEST_F(DeformableCollisionRepresentationTest, UpdateAndInitializationTest)
// 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));
+ EXPECT_NO_THROW(m_deformableCollisionRepresentation->wakeUp());
+ EXPECT_NO_THROW(m_deformableCollisionRepresentation->updateShapeData());
+ EXPECT_TRUE(m_deformableCollisionRepresentation->isActive());
+
+ // The MeshShape fails to update due to the normal calculation, making the collision rep inactive
+ auto state = fem3DRepresentation->getCurrentState();
+ state->getPositions().setConstant(0.0);
+ fem3DRepresentation->setInitialState(state);
+ EXPECT_NO_THROW(m_deformableCollisionRepresentation->updateShapeData());
+ EXPECT_FALSE(m_deformableCollisionRepresentation->isActive());
}
} // namespace Physics
-} // namespace SurgSim
\ No newline at end of file
+} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/DeformableRepresentationTest.cpp b/SurgSim/Physics/UnitTests/DeformableRepresentationTest.cpp
index 2390ff0..b5ce7e4 100644
--- a/SurgSim/Physics/UnitTests/DeformableRepresentationTest.cpp
+++ b/SurgSim/Physics/UnitTests/DeformableRepresentationTest.cpp
@@ -21,6 +21,7 @@
#include "SurgSim/Framework/Runtime.h"
#include "SurgSim/Framework/FrameworkConvert.h"
#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/SparseMatrix.h"
#include "SurgSim/Math/Vector.h"
#include "SurgSim/Math/OdeSolver.h" // Need access to the enum IntegrationScheme
#include "SurgSim/Math/OdeSolverEulerExplicit.h"
@@ -36,6 +37,7 @@
#include "SurgSim/Physics/UnitTests/MockObjects.h"
using SurgSim::Math::Matrix;
+using SurgSim::Math::SparseMatrix;
using SurgSim::Math::Vector3d;
using SurgSim::Math::Vector;
using SurgSim::Physics::DeformableCollisionRepresentation;
@@ -65,7 +67,7 @@ public:
{
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->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);
@@ -74,7 +76,7 @@ public:
m_nonIdentityTransform = SurgSim::Math::makeRigidTransform(q, t);
m_identityTransform = SurgSim::Math::RigidTransform3d::Identity();
- m_localization0 = std::make_shared<SurgSim::Physics::MockDeformableRepresentationLocalization>();
+ m_localization0 = std::make_shared<SurgSim::Physics::MockDeformableLocalization>();
m_localization0->setLocalNode(0);
}
@@ -87,7 +89,7 @@ protected:
SurgSim::Math::RigidTransform3d m_nonIdentityTransform;
// Localization of node 0, to apply external force
- std::shared_ptr<SurgSim::Physics::MockDeformableRepresentationLocalization> m_localization0;
+ std::shared_ptr<SurgSim::Physics::MockDeformableLocalization> m_localization0;
};
TEST_F(DeformableRepresentationTest, ConstructorTest)
@@ -99,12 +101,14 @@ TEST_F(DeformableRepresentationTest, ConstructorTest)
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;});
+ 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>(); });
+ ASSERT_NO_THROW({std::shared_ptr<MockDeformableRepresentation> deformable = \
+ std::make_shared<MockDeformableRepresentation>();
+ });
}
TEST_F(DeformableRepresentationTest, SetGetTest)
@@ -129,16 +133,47 @@ TEST_F(DeformableRepresentationTest, SetGetTest)
EXPECT_EQ(0, getExternalGeneralizedDamping().rows());
EXPECT_EQ(0, getExternalGeneralizedDamping().cols());
setInitialState(m_localInitialState);
+ SparseMatrix zeroMatrix(static_cast<SparseMatrix::Index>(getNumDof()),
+ static_cast<SparseMatrix::Index>(getNumDof()));
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());
+ EXPECT_TRUE(getExternalGeneralizedStiffness().isApprox(zeroMatrix));
+ EXPECT_TRUE(getExternalGeneralizedDamping().isApprox(zeroMatrix));
- doWakeUp();
+ /// Set/Get the numerical integration scheme
+ for (int integerScheme = 0; integerScheme < SurgSim::Math::MAX_INTEGRATIONSCHEMES; integerScheme++)
+ {
+ auto integrationScheme = static_cast<SurgSim::Math::IntegrationScheme>(integerScheme);
+ EXPECT_NO_THROW(setIntegrationScheme(integrationScheme));
+ EXPECT_EQ(integrationScheme, getIntegrationScheme());
+ }
+ setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT);
+ EXPECT_EQ(nullptr, getOdeSolver());
+
+ /// Set/Get the linear solver
+ for (int integerLinearSolver = 0; integerLinearSolver < SurgSim::Math::MAX_LINEARSOLVER; integerLinearSolver++)
+ {
+ auto linearSolver = static_cast<SurgSim::Math::LinearSolver>(integerLinearSolver);
+ EXPECT_NO_THROW(setLinearSolver(linearSolver));
+ EXPECT_EQ(linearSolver, getLinearSolver());
+ }
+ setLinearSolver(SurgSim::Math::LINEARSOLVER_CONJUGATEGRADIENT);
+
+ initialize(std::make_shared<SurgSim::Framework::Runtime>());
+
+ EXPECT_NE(nullptr, getOdeSolver());
+ EXPECT_NE(nullptr, std::dynamic_pointer_cast<SurgSim::Math::OdeSolverEulerExplicit>(getOdeSolver()));
+ EXPECT_NE(nullptr,
+ std::dynamic_pointer_cast<SurgSim::Math::LinearSparseSolveAndInverseCG>(getOdeSolver()->getLinearSolver()));
+ EXPECT_THROW(setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT),
+ SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(setLinearSolver(SurgSim::Math::LINEARSOLVER_LU), SurgSim::Framework::AssertionFailure);
+
+ wakeUp();
EXPECT_TRUE(*m_initialState == *m_localInitialState);
EXPECT_TRUE(*m_currentState == *m_localInitialState);
@@ -154,29 +189,6 @@ TEST_F(DeformableRepresentationTest, SetGetTest)
// 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)
@@ -188,12 +200,13 @@ TEST_F(DeformableRepresentationTest, GetComplianceMatrix)
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)
+ // 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)));
+ EXPECT_NO_THROW(EXPECT_TRUE(applyCompliance(*m_localInitialState, Matrix::Identity(3,
+ 3)).isApprox(Matrix::Identity(3, 3) * dt)));
}
TEST_F(DeformableRepresentationTest, ResetStateTest)
@@ -312,6 +325,8 @@ TEST_F(DeformableRepresentationTest, AfterUpdateTest)
{
// setInitialState sets all 4 states (tested in method above !)
setInitialState(m_localInitialState);
+ SparseMatrix zeroMatrix(static_cast<SparseMatrix::Index>(getNumDof()),
+ static_cast<SparseMatrix::Index>(getNumDof()));
// Initialize and wake-up the deformable component
EXPECT_NO_THROW(EXPECT_TRUE(initialize(std::make_shared<SurgSim::Framework::Runtime>())));
@@ -319,15 +334,15 @@ TEST_F(DeformableRepresentationTest, AfterUpdateTest)
// Set external generalized force/stiffness/damping
EXPECT_TRUE(getExternalGeneralizedForce().isZero());
- EXPECT_TRUE(getExternalGeneralizedStiffness().isZero());
- EXPECT_TRUE(getExternalGeneralizedDamping().isZero());
+ EXPECT_TRUE(getExternalGeneralizedStiffness().isApprox(zeroMatrix));
+ EXPECT_TRUE(getExternalGeneralizedDamping().isApprox(zeroMatrix));
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_FALSE(getExternalGeneralizedStiffness().isApprox(zeroMatrix));
+ EXPECT_FALSE(getExternalGeneralizedDamping().isApprox(zeroMatrix));
EXPECT_TRUE(getExternalGeneralizedForce().isApprox(F));
EXPECT_TRUE(getExternalGeneralizedStiffness().isApprox(K));
EXPECT_TRUE(getExternalGeneralizedDamping().isApprox(D));
@@ -339,8 +354,8 @@ TEST_F(DeformableRepresentationTest, AfterUpdateTest)
// External generalized force/stiffness/damping should have been reset
EXPECT_TRUE(getExternalGeneralizedForce().isZero());
- EXPECT_TRUE(getExternalGeneralizedStiffness().isZero());
- EXPECT_TRUE(getExternalGeneralizedDamping().isZero());
+ EXPECT_TRUE(getExternalGeneralizedStiffness().isApprox(zeroMatrix));
+ EXPECT_TRUE(getExternalGeneralizedDamping().isApprox(zeroMatrix));
EXPECT_TRUE(*m_localInitialState == *m_initialState);
EXPECT_TRUE(*m_localInitialState == *m_previousState);
@@ -411,31 +426,28 @@ TEST_F(DeformableRepresentationTest, SetCollisionRepresentationTest)
EXPECT_EQ(nullptr, object->getCollisionRepresentation());
}
-TEST_F(DeformableRepresentationTest, DoWakeUpTest)
+TEST_F(DeformableRepresentationTest, DoInitializeFailTest)
{
- 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;
+ // The initial state needs to be set prior to calling initialize
+ EXPECT_THROW(initialize(std::make_shared<SurgSim::Framework::Runtime>()), SurgSim::Framework::AssertionFailure);
+}
+TEST_F(DeformableRepresentationTest, DoInitializeTest)
+{
// setInitialState sets all 4 states (tested in method above !)
- setLocalPose(m_nonIdentityTransform);
+ EXPECT_NO_THROW(setLocalPose(m_nonIdentityTransform));
setInitialState(m_localInitialState);
EXPECT_NO_THROW(EXPECT_TRUE(initialize(std::make_shared<SurgSim::Framework::Runtime>())));
- EXPECT_NO_THROW(EXPECT_TRUE(wakeUp()));
+
+ EXPECT_THROW(setLocalPose(m_nonIdentityTransform), SurgSim::Framework::AssertionFailure);
// 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::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));
@@ -443,13 +455,33 @@ TEST_F(DeformableRepresentationTest, DoWakeUpTest)
EXPECT_EQ(*getPreviousState(), *getInitialState());
EXPECT_EQ(*getCurrentState(), *getInitialState());
EXPECT_EQ(*getFinalState(), *getInitialState());
+}
+
+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::LinearSparseSolveAndInverseLU;
+
+ // 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 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());
+ std::shared_ptr<LinearSparseSolveAndInverseLU> expectedLinearSolverType;
+ expectedLinearSolverType = std::dynamic_pointer_cast<LinearSparseSolveAndInverseLU>
+ (m_odeSolver->getLinearSolver());
ASSERT_NE(nullptr, expectedLinearSolverType);
typedef OdeSolverEulerExplicit EESolver;
@@ -510,7 +542,7 @@ TEST_F(DeformableRepresentationTest, SerializationTest)
EXPECT_EQ(1u, node.size());
YAML::Node data = node["SurgSim::Physics::MockDeformableRepresentation"];
- EXPECT_EQ(9u, data.size());
+ EXPECT_EQ(10u, data.size());
std::shared_ptr<MockDeformableRepresentation> newRepresentation;
newRepresentation = std::dynamic_pointer_cast<MockDeformableRepresentation>
diff --git a/SurgSim/Physics/UnitTests/DeformableTestsUtility-inl.h b/SurgSim/Physics/UnitTests/DeformableTestsUtility-inl.h
new file mode 100644
index 0000000..44f92bb
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/DeformableTestsUtility-inl.h
@@ -0,0 +1,49 @@
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file DeformableTestsUtility.h
+/// Common functions for testing deformable compute/update functions.
+
+#ifndef SURGSIM_PHYSICS_UNITTESTS_DEFORMABLETESTSUTILITY_INL_H
+#define SURGSIM_PHYSICS_UNITTESTS_DEFORMABLETESTSUTILITY_INL_H
+
+template <class T>
+void SurgSim::Physics::testOdeEquationUpdate(std::shared_ptr<T> rep,
+ const SurgSim::Math::OdeState& state, const SurgSim::Math::Vector& expectedF,
+ const SurgSim::Math::Matrix& expectedM, const SurgSim::Math::Matrix& expectedD,
+ const SurgSim::Math::Matrix& expectedK)
+{
+ using SurgSim::Math::OdeEquationUpdate;
+
+ auto odeEquation = std::dynamic_pointer_cast<SurgSim::Math::OdeEquation>(rep);
+
+ odeEquation->updateFMDK(state, OdeEquationUpdate::ODEEQUATIONUPDATE_F);
+ EXPECT_NO_THROW(EXPECT_TRUE(odeEquation->getF().isApprox(expectedF)));
+ odeEquation->updateFMDK(state, OdeEquationUpdate::ODEEQUATIONUPDATE_M);
+ EXPECT_NO_THROW(EXPECT_TRUE(odeEquation->getM().isApprox(expectedM)));
+ odeEquation->updateFMDK(state, OdeEquationUpdate::ODEEQUATIONUPDATE_D);
+ EXPECT_NO_THROW(EXPECT_TRUE(odeEquation->getD().isApprox(expectedD)));
+ odeEquation->updateFMDK(state, OdeEquationUpdate::ODEEQUATIONUPDATE_K);
+ EXPECT_NO_THROW(EXPECT_TRUE(odeEquation->getK().isApprox(expectedK)));
+
+ // Test combo method computeFMDK
+ rep->clearFMDK();
+ odeEquation->updateFMDK(state, OdeEquationUpdate::ODEEQUATIONUPDATE_FMDK);
+ EXPECT_NO_THROW(EXPECT_TRUE(odeEquation->getF().isApprox(expectedF)));
+ EXPECT_NO_THROW(EXPECT_TRUE(odeEquation->getM().isApprox(expectedM)));
+ EXPECT_NO_THROW(EXPECT_TRUE(odeEquation->getD().isApprox(expectedD)));
+ EXPECT_NO_THROW(EXPECT_TRUE(odeEquation->getK().isApprox(expectedK)));
+}
+
+#endif // SURGSIM_PHYSICS_UNITTESTS_DEFORMABLETESTSUTILITY_INL_H
diff --git a/SurgSim/Physics/UnitTests/DeformableTestsUtility.h b/SurgSim/Physics/UnitTests/DeformableTestsUtility.h
new file mode 100644
index 0000000..2bcbdfe
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/DeformableTestsUtility.h
@@ -0,0 +1,44 @@
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file DeformableTestsUtility.h
+/// Common functions for testing deformable compute/update functions.
+
+#ifndef SURGSIM_PHYSICS_UNITTESTS_DEFORMABLETESTSUTILITY_H
+#define SURGSIM_PHYSICS_UNITTESTS_DEFORMABLETESTSUTILITY_H
+
+#include <memory>
+
+#include "SurgSim/Math/OdeEquation.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/Vector.h"
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+template <class T>
+void testOdeEquationUpdate(std::shared_ptr<T> rep,
+ const SurgSim::Math::OdeState& state, const SurgSim::Math::Vector& expectedF,
+ const SurgSim::Math::Matrix& expectedM, const SurgSim::Math::Matrix& expectedD,
+ const SurgSim::Math::Matrix& expectedK);
+
+}
+}
+
+#include "SurgSim/Physics/UnitTests/DeformableTestsUtility-inl.h"
+
+#endif // SURGSIM_PHYSICS_UNITTESTS_DEFORMABLETESTSUTILITY_H
diff --git a/SurgSim/Physics/UnitTests/Fem1DConstraintFixedPointTests.cpp b/SurgSim/Physics/UnitTests/Fem1DConstraintFixedPointTests.cpp
new file mode 100644
index 0000000..433fb23
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem1DConstraintFixedPointTests.cpp
@@ -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.
+
+#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/Fem1DElementBeam.h"
+#include "SurgSim/Physics/Fem1DLocalization.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/FemConstraintFixedPoint.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+
+using SurgSim::Math::Vector2d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+using SurgSim::Math::Vector6d;
+
+namespace
+{
+const double epsilon = 1e-10;
+const double dt = 1e-3;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+static std::shared_ptr<Fem1DElementBeam> getBeam(size_t node0, size_t node1,
+ double radius,
+ double massDensity,
+ double poissonRatio,
+ double youngModulus)
+{
+ std::array<size_t, 2> nodeIds = {node0, node1};
+ auto element = std::make_shared<Fem1DElementBeam>(nodeIds);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ element->setRadius(radius);
+ return element;
+}
+
+static std::shared_ptr<Fem1DRepresentation> getFem1d(const std::string &name,
+ double radius = 0.01,
+ double massDensity = 1.0,
+ double poissonRatio = 0.1,
+ double youngModulus = 1.0)
+{
+ auto fem = std::make_shared<Fem1DRepresentation>(name);
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(6, 4);
+
+ std::array<double, 6> p0 = {{0.11, 0.22, 0.33, 0.44, 0.55, 0.66}};
+ std::array<double, 6> p1 = {{0.12, -0.23, 0.34, -0.45, 0.56, -0.67}};
+ std::array<double, 6> p2 = {{-0.10, 0.21, -0.32, 0.43, -0.54, 0.65}};
+ std::array<double, 6> p3 = {{0.2, 0.2, 0.2, 0.2, 0.2, 0.2}};
+
+ state->getPositions().segment<6>(0 * 6) = Vector6d(p0.data());
+ state->getPositions().segment<6>(1 * 6) = Vector6d(p1.data());
+ state->getPositions().segment<6>(2 * 6) = Vector6d(p2.data());
+ state->getPositions().segment<6>(3 * 6) = Vector6d(p3.data());
+
+ fem->addFemElement(getBeam(0, 1, radius, massDensity, poissonRatio, youngModulus));
+ fem->addFemElement(getBeam(1, 2, radius, massDensity, poissonRatio, youngModulus));
+ fem->addFemElement(getBeam(2, 3, radius, 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(Fem1DConstraintFixedPointTests, Constructor)
+{
+ ASSERT_NO_THROW({ FemConstraintFixedPoint constraint; });
+}
+
+TEST(Fem1DConstraintFixedPointTests, Constants)
+{
+ FemConstraintFixedPoint constraint;
+
+ EXPECT_EQ(SurgSim::Physics::FIXED_3DPOINT, constraint.getConstraintType());
+ EXPECT_EQ(3u, constraint.getNumDof());
+}
+
+TEST(Fem1DConstraintFixedPointTests, 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 necessarily construct the physical parameters
+ // necessary to supply a realistic C. It only checks H and b.
+ FemConstraintFixedPoint constraint;
+
+ Vector3d actual;
+
+ // Setup parameters for FemConstraintFixedPoint::build
+ auto localization = std::make_shared<Fem1DLocalization>(getFem1d("representation"),
+ SurgSim::DataStructures::IndexedLocalCoordinate(2u, Vector2d(1.0, 0.0)));
+
+ actual = localization->calculatePosition();
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(24, 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, 24> H = Eigen::Matrix<double, 3, 24>::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); // This weight is on node 2 (beam 2, nodeId 0)
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST(Fem1DConstraintFixedPointTests, 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 necessarily construct the physical parameters
+ // necessary to supply a realistic C. It only checks H and b.
+ FemConstraintFixedPoint constraint;
+
+ Vector3d actual;
+
+ // Setup parameters for FemConstraintFixedPoint::build
+ auto localization = std::make_shared<Fem1DLocalization>(getFem1d("representation"),
+ SurgSim::DataStructures::IndexedLocalCoordinate(2u, Vector2d(0.3, 0.7)));
+
+ actual = localization->calculatePosition();
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(24, 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, 24> H = Eigen::Matrix<double, 3, 24>::Zero();
+ Eigen::Matrix<double, 3, 3> identity = Eigen::Matrix<double, 3, 3>::Identity();
+ SurgSim::Math::setSubMatrix(0.3 * dt * identity, 0, 4, 3, 3, &H); // This weight is on node 2 (beam 2, nodeId 0)
+ SurgSim::Math::setSubMatrix(0.7 * dt * identity, 0, 6, 3, 3, &H); // This weight is on node 3 (beam 2, nodeId 1)
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST(Fem1DConstraintFixedPointTests, 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 necessarily construct the physical parameters
+ // necessary to supply a realistic C. It only checks H and b.
+ FemConstraintFixedPoint constraint;
+
+ Vector3d actual;
+ Vector3d desired;
+
+ // Setup parameters for FemConstraintFixedPoint::build
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(24, 3, 1);
+
+ ConstraintData emptyConstraint;
+
+ auto localization = std::make_shared<Fem1DLocalization>(getFem1d("representation"),
+ SurgSim::DataStructures::IndexedLocalCoordinate(2u, Vector2d(0.11, 0.89)));
+ 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, Vector2d(0.32, 0.68)));
+ 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, 24> H = Eigen::Matrix<double, 3, 24>::Zero();
+ Eigen::Matrix<double, 3, 3> identity = Eigen::Matrix<double, 3, 3>::Identity();
+ SurgSim::Math::addSubMatrix( 0.11 * dt * identity, 0, 4, 3, 3, &H); // This weight is on node 2 (beam 2, nodeId 0)
+ SurgSim::Math::addSubMatrix( 0.89 * dt * identity, 0, 6, 3, 3, &H); // This weight is on node 3 (beam 2, nodeId 1)
+ SurgSim::Math::addSubMatrix(-0.32 * dt * identity, 0, 2, 3, 3, &H); // This weight is on node 1 (beam 1, nodeId 0)
+ SurgSim::Math::addSubMatrix(-0.68 * dt * identity, 0, 4, 3, 3, &H); // This weight is on node 2 (beam 1, nodeId 0)
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/Fem1DConstraintFixedRotationVectorTests.cpp b/SurgSim/Physics/UnitTests/Fem1DConstraintFixedRotationVectorTests.cpp
new file mode 100644
index 0000000..ef3a93b
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem1DConstraintFixedRotationVectorTests.cpp
@@ -0,0 +1,247 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Fem1DElementBeam.h"
+#include "SurgSim/Physics/Fem1DLocalization.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/FemConstraintFixedRotationVector.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/RotationVectorConstraintData.h"
+
+using SurgSim::Math::Vector2d;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+using SurgSim::Math::Vector6d;
+
+namespace
+{
+const double epsilon = 1e-10;
+const double dt = 1e-3;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+static std::shared_ptr<Fem1DElementBeam> getBeam(size_t node0, size_t node1,
+ double radius,
+ double massDensity,
+ double poissonRatio,
+ double youngModulus)
+{
+ std::array<size_t, 2> nodeIds = {node0, node1};
+ auto element = std::make_shared<Fem1DElementBeam>(nodeIds);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ element->setRadius(radius);
+ return element;
+}
+
+static std::shared_ptr<Fem1DRepresentation> getFem1d(const std::string &name,
+ double radius = 0.01,
+ double massDensity = 1.0,
+ double poissonRatio = 0.1,
+ double youngModulus = 1.0)
+{
+ auto fem = std::make_shared<Fem1DRepresentation>(name);
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(6, 4);
+
+ std::array<double, 6> p0 = {{0.11, 0.22, 0.33, 0.44, 0.55, 0.66}};
+ std::array<double, 6> p1 = {{0.12, -0.23, 0.34, -0.45, 0.56, -0.67}};
+ std::array<double, 6> p2 = {{-0.10, 0.21, -0.32, 0.43, -0.54, 0.65}};
+ std::array<double, 6> p3 = {{0.2, 0.2, 0.2, 0.2, 0.2, 0.2}};
+
+ state->getPositions().segment<6>(0 * 6) = Vector6d(p0.data());
+ state->getPositions().segment<6>(1 * 6) = Vector6d(p1.data());
+ state->getPositions().segment<6>(2 * 6) = Vector6d(p2.data());
+ state->getPositions().segment<6>(3 * 6) = Vector6d(p3.data());
+
+ fem->addFemElement(getBeam(0, 1, radius, massDensity, poissonRatio, youngModulus));
+ fem->addFemElement(getBeam(1, 2, radius, massDensity, poissonRatio, youngModulus));
+ fem->addFemElement(getBeam(2, 3, radius, 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(Fem1DConstraintFixedRotationVectorTests, Constructor)
+{
+ ASSERT_NO_THROW({ FemConstraintFixedRotationVector constraint; });
+}
+
+TEST(Fem1DConstraintFixedRotationVectorTests, Constants)
+{
+ FemConstraintFixedRotationVector constraint;
+
+ EXPECT_EQ(SurgSim::Physics::FIXED_3DROTATION_VECTOR, constraint.getConstraintType());
+ EXPECT_EQ(3u, constraint.getNumDof());
+}
+
+TEST(Fem1DConstraintFixedRotationVectorTests, BuildMlcpBasic)
+{
+ FemConstraintFixedRotationVector constraint;
+
+ // Prepare the fem1d representation for this constraint type
+ auto fem1d = getFem1d("representation");
+ auto localization = std::make_shared<Fem1DLocalization>(fem1d,
+ SurgSim::DataStructures::IndexedLocalCoordinate(2u, Vector2d(1.0, 0.0)));
+
+ // Prepare the rigid representation for this constraint type
+ auto rigid = std::make_shared<SurgSim::Physics::RigidRepresentation>("rigid");
+
+ // Prepare the specific constraint data
+ RotationVectorRigidFem1DConstraintData emptyConstraint;
+ emptyConstraint.setFem1DRotation(fem1d, localization->getLocalPosition().index);
+ emptyConstraint.setRigidOrFixedRotation(rigid, rigid->getPose().linear());
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(24, 3, 1);
+
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE));
+
+ // Compare results
+ Vector3d actual = Vector3d::Zero();
+ auto nodeIds = fem1d->getFemElement(2u)->getNodeIds();
+ Vector3d rotationVector0 = fem1d->getInitialState()->getPositions().segment<3>(6 * nodeIds[0] + 3);
+ Vector3d rotationVector1 = fem1d->getInitialState()->getPositions().segment<3>(6 * nodeIds[1] + 3);
+ actual = SurgSim::Math::interpolate(rotationVector0, rotationVector1, 0.0);
+ Eigen::Matrix<double, 3, 1> violation = actual;
+ EXPECT_NEAR_EIGEN(violation, mlcpPhysicsProblem.b, epsilon);
+
+ Eigen::Matrix<double, 3, 24> H = Eigen::Matrix<double, 3, 24>::Zero();
+ Eigen::Matrix<double, 3, 3> identity = Eigen::Matrix<double, 3, 3>::Identity();
+ SurgSim::Math::setSubMatrix(1.0 * dt * identity, 0, 5, 3, 3, &H); // This weight is on node 1 (beam 1, nodeId 0)
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST(Fem1DConstraintFixedRotationVectorTests, BuildMlcp)
+{
+ FemConstraintFixedRotationVector constraint;
+
+ // Prepare the fem1d representation for this constraint type
+ auto fem1d = getFem1d("representation");
+ auto localization = std::make_shared<Fem1DLocalization>(fem1d,
+ SurgSim::DataStructures::IndexedLocalCoordinate(2u, Vector2d(0.3, 0.7)));
+
+ // Prepare the rigid representation for this constraint type
+ auto rigid = std::make_shared<SurgSim::Physics::RigidRepresentation>("rigid");
+
+ // Prepare the specific constraint data
+ RotationVectorRigidFem1DConstraintData emptyConstraint;
+ emptyConstraint.setFem1DRotation(fem1d, localization->getLocalPosition().index);
+ emptyConstraint.setRigidOrFixedRotation(rigid, rigid->getPose().linear());
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(24, 3, 1);
+
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE));
+
+ // Compare results
+ Vector3d actual = Vector3d::Zero();
+ auto nodeIds = fem1d->getFemElement(2u)->getNodeIds();
+ Vector3d rotationVector0 = fem1d->getInitialState()->getPositions().segment<3>(6 * nodeIds[0] + 3);
+ Vector3d rotationVector1 = fem1d->getInitialState()->getPositions().segment<3>(6 * nodeIds[1] + 3);
+ actual = SurgSim::Math::interpolate(rotationVector0, rotationVector1, 0.7);
+ Eigen::Matrix<double, 3, 1> violation = actual;
+ EXPECT_NEAR_EIGEN(violation, mlcpPhysicsProblem.b, epsilon);
+
+ Eigen::Matrix<double, 3, 24> H = Eigen::Matrix<double, 3, 24>::Zero();
+ Eigen::Matrix<double, 3, 3> identity = Eigen::Matrix<double, 3, 3>::Identity();
+ SurgSim::Math::setSubMatrix(0.3 * dt * identity, 0, 5, 3, 3, &H); // This weight is on node 2 (beam 2, nodeId 0)
+ SurgSim::Math::setSubMatrix(0.7 * dt * identity, 0, 7, 3, 3, &H); // This weight is on node 3 (beam 2, nodeId 1)
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST(Fem1DConstraintFixedRotationVectorTests, BuildMlcpTwoStep)
+{
+ FemConstraintFixedRotationVector constraint;
+
+ // Prepare the fem1d representation for this constraint type
+ auto fem1d = getFem1d("representation");
+ auto localization = std::make_shared<Fem1DLocalization>(fem1d,
+ SurgSim::DataStructures::IndexedLocalCoordinate(2u, Vector2d(0.11, 0.89)));
+
+ // Prepare the rigid representation for this constraint type
+ auto rigid = std::make_shared<SurgSim::Physics::RigidRepresentation>("rigid");
+
+ // Prepare the specific constraint data
+ RotationVectorRigidFem1DConstraintData emptyConstraint;
+ emptyConstraint.setFem1DRotation(fem1d, localization->getLocalPosition().index);
+ emptyConstraint.setRigidOrFixedRotation(rigid, rigid->getPose().linear());
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(24, 3, 1);
+
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE));
+
+ localization->setLocalPosition(
+ SurgSim::DataStructures::IndexedLocalCoordinate(1u, Vector2d(0.32, 0.68)));
+ Vector3d desired = Vector3d::Zero();
+ {
+ auto nodeIds = fem1d->getFemElement(1u)->getNodeIds();
+ Vector3d rotationVector0 = fem1d->getInitialState()->getPositions().segment<3>(6 * nodeIds[0] + 3);
+ Vector3d rotationVector1 = fem1d->getInitialState()->getPositions().segment<3>(6 * nodeIds[1] + 3);
+ desired = SurgSim::Math::interpolate(rotationVector0, rotationVector1, 0.68);
+ }
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_NEGATIVE_SIDE));
+
+ // Compare results
+ Vector3d actual = Vector3d::Zero();
+ auto nodeIds = fem1d->getFemElement(2u)->getNodeIds();
+ Vector3d rotationVector0 = fem1d->getInitialState()->getPositions().segment<3>(6 * nodeIds[0] + 3);
+ Vector3d rotationVector1 = fem1d->getInitialState()->getPositions().segment<3>(6 * nodeIds[1] + 3);
+ actual = SurgSim::Math::interpolate(rotationVector0, rotationVector1, 0.89);
+ Eigen::Matrix<double, 3, 1> violation = actual - desired;
+ EXPECT_NEAR_EIGEN(violation, mlcpPhysicsProblem.b, epsilon);
+
+ Eigen::Matrix<double, 3, 24> H = Eigen::Matrix<double, 3, 24>::Zero();
+ Eigen::Matrix<double, 3, 3> identity = Eigen::Matrix<double, 3, 3>::Identity();
+ SurgSim::Math::addSubMatrix( 0.11 * dt * identity, 0, 5, 3, 3, &H); // This weight is on node 2 (beam 2, nodeId 0)
+ SurgSim::Math::addSubMatrix( 0.89 * dt * identity, 0, 7, 3, 3, &H); // This weight is on node 3 (beam 2, nodeId 1)
+ SurgSim::Math::addSubMatrix(-0.32 * dt * identity, 0, 3, 3, 3, &H); // This weight is on node 1 (beam 1, nodeId 0)
+ SurgSim::Math::addSubMatrix(-0.68 * dt * identity, 0, 5, 3, 3, &H); // This weight is on node 2 (beam 1, nodeId 0)
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/Fem1DConstraintFrictionalSlidingTests.cpp b/SurgSim/Physics/UnitTests/Fem1DConstraintFrictionalSlidingTests.cpp
new file mode 100644
index 0000000..4c9fa7c
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem1DConstraintFrictionalSlidingTests.cpp
@@ -0,0 +1,283 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/LinearSparseSolveAndInverse.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+#include "SurgSim/Physics/Fem1DLocalization.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/FemConstraintFrictionalSliding.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/SlidingConstraintData.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+using SurgSim::Framework::Runtime;
+using SurgSim::Physics::Fem1DRepresentation;
+using SurgSim::Physics::FemConstraintFrictionalSliding;
+using SurgSim::Physics::Fem1DLocalization;
+using SurgSim::Physics::Fem1DElementBeam;
+using SurgSim::Physics::MlcpPhysicsProblem;
+using SurgSim::Physics::MockFem1DRepresentation;
+using SurgSim::Physics::SlidingConstraintData;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector2d;
+
+namespace
+{
+ const double epsilon = 1e-10;
+ const double dt = 1e-3;
+};
+
+static void addBeam(Fem1DRepresentation* fem,
+ size_t node0, size_t node1,
+ double radius = 0.01,
+ double massDensity = 1.0,
+ double poissonRatio = 0.1,
+ double youngModulus = 1.0)
+{
+ std::array<size_t, 2> nodes = {node0, node1};
+ auto element = std::make_shared<Fem1DElementBeam>(nodes);
+ element->setRadius(radius);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ fem->addFemElement(element);
+}
+
+class Fem1DConstraintFrictionalSlidingTests : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ // Define plane with normal 'n' pointing against gravity.
+ m_slidingDirection = Vector3d(0.8539, 0.6289, -0.9978);
+ m_slidingDirection.normalize();
+
+ // Create mock FEM
+ m_fem = std::make_shared<MockFem1DRepresentation>("Fem1dRepresentation");
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(6, 5);
+
+ state->getPositions().segment<3>(0 * 6) = Vector3d(0.30, -0.57, 0.40);
+ state->getPositions().segment<3>(1 * 6) = Vector3d(0.06, 0.63, -0.32);
+ state->getPositions().segment<3>(2 * 6) = Vector3d(-0.91, 0.72, 0.72);
+ state->getPositions().segment<3>(3 * 6) = Vector3d(0.35, 0.52, 0.50);
+ state->getPositions().segment<3>(4 * 6) = Vector3d(1.14, 0.66, 0.71);
+
+ addBeam(m_fem.get(), 0, 1);
+ addBeam(m_fem.get(), 1, 2);
+ addBeam(m_fem.get(), 2, 3);
+ addBeam(m_fem.get(), 3, 4);
+
+ m_fem->setInitialState(state);
+ m_fem->setIntegrationScheme(SurgSim::Math::IntegrationScheme::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED);
+ 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 setSlidingConstraintAt(const IndexedLocalCoordinate& coord)
+ {
+ m_localization = std::make_shared<Fem1DLocalization>(m_fem, coord);
+ m_constraintData.setSlidingDirection(m_localization->calculatePosition(0.0), m_slidingDirection);
+ }
+
+ std::shared_ptr<MockFem1DRepresentation> m_fem;
+ std::shared_ptr<Fem1DLocalization> m_localization;
+
+ Vector3d m_slidingDirection;
+ SlidingConstraintData m_constraintData;
+};
+
+TEST_F(Fem1DConstraintFrictionalSlidingTests, ConstructorTest)
+{
+ ASSERT_NO_THROW(
+ {
+ FemConstraintFrictionalSliding femContact;
+ });
+}
+
+TEST_F(Fem1DConstraintFrictionalSlidingTests, ConstraintConstantsTest)
+{
+ auto implementation = std::make_shared<FemConstraintFrictionalSliding>();
+
+ EXPECT_EQ(SurgSim::Physics::FRICTIONAL_SLIDING, implementation->getConstraintType());
+ EXPECT_EQ(3u, implementation->getNumDof());
+}
+
+TEST_F(Fem1DConstraintFrictionalSlidingTests, BuildMlcpTest)
+{
+ // This test verifies the build mlcp function works for a simple case.
+ auto implementation = std::make_shared<FemConstraintFrictionalSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 3, 1);
+
+ // Apply constraint purely to the first node of the 0th beam.
+ IndexedLocalCoordinate coord(0, Vector2d(1.0, 0.0));
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ Eigen::Matrix<double, 3, 30> H = Eigen::Matrix<double, 3, 30>::Zero();
+ H.block<1, 3>(0, 0) = (dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 0) = (dt * m_slidingDirection).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 30, 30> C = dt * m_fem->getM().toDense().inverse();
+
+ EXPECT_NEAR_EIGEN(C * H.transpose(), mlcpPhysicsProblem.CHt, epsilon);
+
+ EXPECT_NEAR_EIGEN(H * C * H.transpose(), mlcpPhysicsProblem.A, epsilon);
+
+ EXPECT_DOUBLE_EQ(0.5, mlcpPhysicsProblem.mu[0]);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST_F(Fem1DConstraintFrictionalSlidingTests, BuildMlcpCoordinateTest)
+{
+ // This test verifies the build mlcp function works for a more complicated case with different nodes.
+ auto implementation = std::make_shared<FemConstraintFrictionalSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 3, 1);
+
+ // Apply constraint between the first two nodes of the 0th beam.
+ const Vector2d barycentric = Vector2d(0.24, 0.76);
+ IndexedLocalCoordinate coord(0, barycentric);
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ Eigen::Matrix<double, 3, 30> H = Eigen::Matrix<double, 3, 30>::Zero();
+ H.block<1, 3>(0, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 0) = (barycentric[0] * dt * m_slidingDirection).eval();
+ H.block<1, 3>(0, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 6) = (barycentric[1] * dt * m_slidingDirection).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 30, 30> C = dt * m_fem->getM().toDense().inverse();
+
+ EXPECT_NEAR_EIGEN(C * H.transpose(), mlcpPhysicsProblem.CHt, epsilon);
+
+ EXPECT_NEAR_EIGEN(H * C * H.transpose(), mlcpPhysicsProblem.A, epsilon);
+
+ EXPECT_DOUBLE_EQ(0.5, mlcpPhysicsProblem.mu[0]);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST_F(Fem1DConstraintFrictionalSlidingTests, BuildMlcpIndiciesTest)
+{
+ // This test verifies the build mlcp function works given a hypothetical, preexisting mlcp problem.
+
+ auto implementation = std::make_shared<FemConstraintFrictionalSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof() + 5, 4, 2);
+
+ // Suppose 5 dof and 1 constraint are defined elsewhere. Then H, CHt, HCHt, and b are prebuilt.
+ Eigen::SparseVector<double, Eigen::RowMajor, ptrdiff_t> localH;
+ localH.resize(5);
+ localH.reserve(5);
+ localH.insert(0) = 0.9478;
+ localH.insert(1) = -0.3807;
+ localH.insert(2) = 0.5536;
+ localH.insert(3) = -0.6944;
+ localH.insert(4) = 0.1815;
+
+ mlcpPhysicsProblem.H.coeffRef(0, 0) += localH.coeff(0);
+ mlcpPhysicsProblem.H.coeffRef(0, 1) += localH.coeff(1);
+ mlcpPhysicsProblem.H.coeffRef(0, 2) += localH.coeff(2);
+ mlcpPhysicsProblem.H.coeffRef(0, 3) += localH.coeff(3);
+ mlcpPhysicsProblem.H.coeffRef(0, 4) += localH.coeff(4);
+
+ 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 the Fem at 5th dof (1 or multiple representations exist before, covering a total of 5 dof)
+ // and the constraint at the index 1 (1 atomic constraint exists before this one)
+ size_t indexOfRepresentation = 5;
+ size_t indexOfConstraint = 1;
+
+ // Apply constraint between the first two nodes of the fem.
+ const Vector2d barycentric = Vector2d(0.24, 0.76);
+ IndexedLocalCoordinate coord(0, barycentric);
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, indexOfRepresentation, indexOfConstraint,
+ SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ Eigen::Matrix<double, 3, 30> H = Eigen::Matrix<double, 3, 30>::Zero();
+ H.block<1, 3>(0, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 0) = (barycentric[0] * dt * m_slidingDirection).eval();
+ H.block<1, 3>(0, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 6) = (barycentric[1] * dt * m_slidingDirection).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H.block(indexOfConstraint, indexOfRepresentation, 3, 30), epsilon);
+
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 30, 30> C = dt * m_fem->getM().toDense().inverse();
+
+ EXPECT_NEAR_EIGEN(
+ C * H.transpose(), mlcpPhysicsProblem.CHt.block(indexOfRepresentation, indexOfConstraint, 30, 3), epsilon);
+
+ EXPECT_NEAR_EIGEN(
+ H * C * H.transpose(), mlcpPhysicsProblem.A.block(indexOfConstraint, indexOfConstraint, 3, 3), epsilon);
+
+ EXPECT_DOUBLE_EQ(0.5, mlcpPhysicsProblem.mu[1]);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
diff --git a/SurgSim/Physics/UnitTests/Fem1DConstraintFrictionlessContactTests.cpp b/SurgSim/Physics/UnitTests/Fem1DConstraintFrictionlessContactTests.cpp
new file mode 100644
index 0000000..7973648
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem1DConstraintFrictionlessContactTests.cpp
@@ -0,0 +1,319 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/LinearSparseSolveAndInverse.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+#include "SurgSim/Physics/Fem1DLocalization.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/FemConstraintFrictionlessContact.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+using SurgSim::Framework::Runtime;
+using SurgSim::Physics::ContactConstraintData;
+using SurgSim::Physics::Fem1DRepresentation;
+using SurgSim::Physics::FemConstraintFrictionlessContact;
+using SurgSim::Physics::Fem1DLocalization;
+using SurgSim::Physics::Fem1DElementBeam;
+using SurgSim::Physics::MlcpPhysicsProblem;
+using SurgSim::Physics::MockFem1DRepresentation;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector2d;
+
+namespace
+{
+const double epsilon = 1e-10;
+const double dt = 1e-3;
+const double mlcpPrecision = 1e-04;
+};
+
+static void addBeam(Fem1DRepresentation* fem,
+ size_t node0, size_t node1,
+ double radius = 0.01,
+ double massDensity = 1.0,
+ double poissonRatio = 0.1,
+ double youngModulus = 1.0)
+{
+ std::array<size_t, 2> nodes = {node0, node1};
+ auto element = std::make_shared<Fem1DElementBeam>(nodes);
+ element->setRadius(radius);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ fem->addFemElement(element);
+}
+
+class Fem1DConstraintFrictionlessContactTests : 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<MockFem1DRepresentation>("Fem1dRepresentation");
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(6, 5);
+
+ state->getPositions().segment<3>(0 * 6) = Vector3d(0.30, -0.57, 0.40);
+ state->getPositions().segment<3>(1 * 6) = Vector3d(0.06, 0.63, -0.32);
+ state->getPositions().segment<3>(2 * 6) = Vector3d(-0.91, 0.72, 0.72);
+ state->getPositions().segment<3>(3 * 6) = Vector3d(0.35, 0.52, 0.50);
+ state->getPositions().segment<3>(4 * 6) = Vector3d(1.14, 0.66, 0.71);
+
+ addBeam(m_fem.get(), 0, 1);
+ addBeam(m_fem.get(), 1, 2);
+ addBeam(m_fem.get(), 2, 3);
+ addBeam(m_fem.get(), 3, 4);
+
+ m_fem->setInitialState(state);
+ m_fem->setIntegrationScheme(SurgSim::Math::IntegrationScheme::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED);
+ 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_localization = std::make_shared<Fem1DLocalization>(m_fem, 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);
+ }
+
+ Vector3d computeNewPosition(const IndexedLocalCoordinate& coord) const
+ {
+ // Solve the system M.a = f
+ // The gravity force should be M.g, but we actually use a mass per node diagonal matrix Md
+ // So the assumption that the violation should be oriented toward the gravity vector is false:
+ // Ma = f = Mg => a = M^-1.f = M^-1.M.g = g
+ // Instead, we have
+ // Ma = f = Md.g => a = M^-1.f = M^-1.Md.g
+ SurgSim::Math::Vector a = m_fem->getM().toDense().inverse() * m_fem->getF();
+ SurgSim::Math::Vector v = m_fem->getInitialState()->getVelocities() + a * dt;
+ SurgSim::Math::Vector p = m_fem->getInitialState()->getPositions() + v * dt;
+ Vector3d newPosition = Vector3d::Zero();
+ const auto& nodeIds = m_fem->getFemElement(coord.index)->getNodeIds();
+
+ for (size_t node = 0; node < 2; node++)
+ {
+ newPosition += p.segment<3>(6 * nodeIds[node]) * coord.coordinate[node];
+ }
+
+ return newPosition;
+ }
+
+ std::shared_ptr<MockFem1DRepresentation> m_fem;
+ std::shared_ptr<Fem1DLocalization> m_localization;
+
+ Vector3d m_n;
+ ContactConstraintData m_constraintData;
+};
+
+TEST_F(Fem1DConstraintFrictionlessContactTests, ConstructorTest)
+{
+ ASSERT_NO_THROW(
+ {
+ FemConstraintFrictionlessContact femContact;
+ });
+}
+
+TEST_F(Fem1DConstraintFrictionlessContactTests, ConstraintConstantsTest)
+{
+ auto implementation = std::make_shared<FemConstraintFrictionlessContact>();
+
+ EXPECT_EQ(SurgSim::Physics::FRICTIONLESS_3DCONTACT, implementation->getConstraintType());
+ EXPECT_EQ(1u, implementation->getNumDof());
+}
+
+TEST_F(Fem1DConstraintFrictionlessContactTests, BuildMlcpTest)
+{
+ // This test verifies the build mlcp function works for a simple case.
+
+ auto implementation = std::make_shared<FemConstraintFrictionlessContact>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 1, 1);
+
+ // Apply constraint purely to the first node of the 0th beam.
+ IndexedLocalCoordinate coord(0, Vector2d(1.0, 0.0));
+ setContactAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ const Vector3d newPosition = computeNewPosition(coord);
+ EXPECT_NEAR(newPosition.dot(m_n), mlcpPhysicsProblem.b[0] + mlcpPrecision, epsilon);
+
+ Eigen::Matrix<double, 1, 30> H = Eigen::Matrix<double, 1, 30>::Zero();
+ H.segment<3>(6 * 0) = dt * m_n;
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 30, 30> C = dt * m_fem->getM().toDense().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(Fem1DConstraintFrictionlessContactTests, BuildMlcpCoordinateTest)
+{
+ // This test verifies the build mlcp function works for a more complicated case with different nodes.
+
+ auto implementation = std::make_shared<FemConstraintFrictionlessContact>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 1, 1);
+
+ // Apply constraint to all nodes of an fem.
+ const Vector2d barycentric = Vector2d(0.24, 0.76);
+ IndexedLocalCoordinate coord(1, barycentric);
+ setContactAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ const Vector3d newPosition = computeNewPosition(coord);
+ EXPECT_NEAR(newPosition.dot(m_n), mlcpPhysicsProblem.b[0] + mlcpPrecision, epsilon);
+
+ Eigen::Matrix<double, 1, 30> H = Eigen::Matrix<double, 1, 30>::Zero();
+ H.segment<3>(6 * 1) = 0.24 * dt * m_n;
+ H.segment<3>(6 * 2) = 0.76 * dt * m_n;
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon)
+ << "H = " << H << std::endl << "mlcpH = " << mlcpPhysicsProblem.H << std::endl;
+
+ // C = dt * m^{-1}
+ SurgSim::Math::Matrix C;
+ SurgSim::Math::SparseMatrix M(30, 30);
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ M = m_fem->getM();
+ SurgSim::Math::LinearSparseSolveAndInverseLU solver;
+ SurgSim::Math::Vector b = SurgSim::Math::Vector::Zero(30);
+ solver.setMatrix(M);
+ C = solver.getInverse();
+ C *= dt;
+
+ 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(Fem1DConstraintFrictionlessContactTests, BuildMlcpIndiciesTest)
+{
+ // This test verifies the build mlcp function works given a hypothetical, preexisting mlcp problem.
+
+ auto implementation = std::make_shared<FemConstraintFrictionlessContact>();
+
+ // 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::SparseVector<double, Eigen::RowMajor, ptrdiff_t> localH;
+ localH.resize(5);
+ localH.reserve(5);
+ localH.insert(0) = 0.9478;
+ localH.insert(1) = -0.3807;
+ localH.insert(2) = 0.5536;
+ localH.insert(3) = -0.6944;
+ localH.insert(4) = 0.1815;
+
+ mlcpPhysicsProblem.H.coeffRef(0, 0) += localH.coeff(0);
+ mlcpPhysicsProblem.H.coeffRef(0, 1) += localH.coeff(1);
+ mlcpPhysicsProblem.H.coeffRef(0, 2) += localH.coeff(2);
+ mlcpPhysicsProblem.H.coeffRef(0, 3) += localH.coeff(3);
+ mlcpPhysicsProblem.H.coeffRef(0, 4) += localH.coeff(4);
+
+ 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 the Fem at 5th dof (1 or multiple representations exist before, covering a total of 5 dof)
+ // and the constraint at the index 1 (1 atomic constraint exists before this one)
+ size_t indexOfRepresentation = 5;
+ size_t indexOfConstraint = 1;
+
+ // Apply constraint to all nodes of an fem.
+ const Vector2d barycentric = Vector2d(0.24, 0.76);
+ IndexedLocalCoordinate coord(1, barycentric);
+ setContactAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, indexOfRepresentation, indexOfConstraint,
+ SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ const Vector3d newPosition = computeNewPosition(coord);
+ EXPECT_NEAR(newPosition.dot(m_n), mlcpPhysicsProblem.b[indexOfConstraint] + mlcpPrecision, epsilon);
+
+ Eigen::Matrix<double, 1, 30> H = Eigen::Matrix<double, 1, 30>::Zero();
+ H.segment<3>(6 * 1) = 0.24 * dt * m_n;
+ H.segment<3>(6 * 2) = 0.76 * dt * m_n;
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H.block(indexOfConstraint, indexOfRepresentation, 1, 30), epsilon);
+
+ SurgSim::Math::Matrix C;
+ SurgSim::Math::SparseMatrix M(30, 30);
+ m_fem->updateFMDK(*m_fem->getPreviousState(), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ M = m_fem->getM();
+ SurgSim::Math::LinearSparseSolveAndInverseLU solver;
+ SurgSim::Math::Vector b = SurgSim::Math::Vector::Zero(30);
+ solver.setMatrix(M);
+ C = solver.getInverse();
+ C *= dt;
+
+ EXPECT_NEAR_EIGEN(
+ C * H.transpose(), mlcpPhysicsProblem.CHt.block(indexOfRepresentation, indexOfConstraint, 30, 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/Fem1DConstraintFrictionlessSlidingTests.cpp b/SurgSim/Physics/UnitTests/Fem1DConstraintFrictionlessSlidingTests.cpp
new file mode 100644
index 0000000..b5fb5e5
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem1DConstraintFrictionlessSlidingTests.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.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/LinearSparseSolveAndInverse.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+#include "SurgSim/Physics/Fem1DLocalization.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/FemConstraintFrictionlessSliding.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/SlidingConstraintData.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+using SurgSim::Framework::Runtime;
+using SurgSim::Physics::Fem1DRepresentation;
+using SurgSim::Physics::FemConstraintFrictionlessSliding;
+using SurgSim::Physics::Fem1DLocalization;
+using SurgSim::Physics::Fem1DElementBeam;
+using SurgSim::Physics::MlcpPhysicsProblem;
+using SurgSim::Physics::MockFem1DRepresentation;
+using SurgSim::Physics::SlidingConstraintData;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector2d;
+
+namespace
+{
+ const double epsilon = 1e-10;
+ const double dt = 1e-3;
+};
+
+static void addBeam(Fem1DRepresentation* fem,
+ size_t node0, size_t node1,
+ double radius = 0.01,
+ double massDensity = 1.0,
+ double poissonRatio = 0.1,
+ double youngModulus = 1.0)
+{
+ std::array<size_t, 2> nodes = {node0, node1};
+ auto element = std::make_shared<Fem1DElementBeam>(nodes);
+ element->setRadius(radius);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ fem->addFemElement(element);
+}
+
+class Fem1DConstraintFrictionlessSlidingTests : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ // Define plane with normal 'n' pointing against gravity.
+ m_slidingDirection = Vector3d(0.8539, 0.6289, -0.9978);
+ m_slidingDirection.normalize();
+
+ // Create mock FEM
+ m_fem = std::make_shared<MockFem1DRepresentation>("Fem1dRepresentation");
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(6, 5);
+
+ state->getPositions().segment<3>(0 * 6) = Vector3d(0.30, -0.57, 0.40);
+ state->getPositions().segment<3>(1 * 6) = Vector3d(0.06, 0.63, -0.32);
+ state->getPositions().segment<3>(2 * 6) = Vector3d(-0.91, 0.72, 0.72);
+ state->getPositions().segment<3>(3 * 6) = Vector3d(0.35, 0.52, 0.50);
+ state->getPositions().segment<3>(4 * 6) = Vector3d(1.14, 0.66, 0.71);
+
+ addBeam(m_fem.get(), 0, 1);
+ addBeam(m_fem.get(), 1, 2);
+ addBeam(m_fem.get(), 2, 3);
+ addBeam(m_fem.get(), 3, 4);
+
+ m_fem->setInitialState(state);
+ m_fem->setIntegrationScheme(SurgSim::Math::IntegrationScheme::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED);
+ 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 setSlidingConstraintAt(const IndexedLocalCoordinate& coord)
+ {
+ m_localization = std::make_shared<Fem1DLocalization>(m_fem, coord);
+ m_constraintData.setSlidingDirection(m_localization->calculatePosition(0.0), m_slidingDirection);
+ }
+
+ std::shared_ptr<MockFem1DRepresentation> m_fem;
+ std::shared_ptr<Fem1DLocalization> m_localization;
+
+ Vector3d m_slidingDirection;
+ SlidingConstraintData m_constraintData;
+};
+
+TEST_F(Fem1DConstraintFrictionlessSlidingTests, ConstructorTest)
+{
+ ASSERT_NO_THROW(
+ {
+ FemConstraintFrictionlessSliding femContact;
+ });
+}
+
+TEST_F(Fem1DConstraintFrictionlessSlidingTests, ConstraintConstantsTest)
+{
+ auto implementation = std::make_shared<FemConstraintFrictionlessSliding>();
+
+ EXPECT_EQ(SurgSim::Physics::FRICTIONLESS_SLIDING, implementation->getConstraintType());
+ EXPECT_EQ(2u, implementation->getNumDof());
+}
+
+TEST_F(Fem1DConstraintFrictionlessSlidingTests, BuildMlcpTest)
+{
+ // This test verifies the build mlcp function works for a simple case.
+ auto implementation = std::make_shared<FemConstraintFrictionlessSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 2, 1);
+
+ // Apply constraint purely to the first node of the 0th beam.
+ IndexedLocalCoordinate coord(0, Vector2d(1.0, 0.0));
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ auto pose = m_constraintData.getPose().inverse().rotation();
+ EXPECT_TRUE((pose * m_slidingDirection).isApprox(Vector3d(1.0, 0.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[0]).isApprox(Vector3d(0.0, 1.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[1]).isApprox(Vector3d(0.0, 0.0, 1.0)));
+
+ Eigen::Matrix<double, 2, 30> H = Eigen::Matrix<double, 2, 30>::Zero();
+ H.block<1, 3>(0, 0) = (dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (dt * m_constraintData.getNormals()[1]).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 30, 30> C = dt * m_fem->getM().toDense().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(Fem1DConstraintFrictionlessSlidingTests, BuildMlcpCoordinateTest)
+{
+ // This test verifies the build mlcp function works for a more complicated case with different nodes.
+ auto implementation = std::make_shared<FemConstraintFrictionlessSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 2, 1);
+
+ // Apply constraint between the first two nodes of the 0th beam.
+ const Vector2d barycentric = Vector2d(0.24, 0.76);
+ IndexedLocalCoordinate coord(0, barycentric);
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ auto pose = m_constraintData.getPose().inverse().rotation();
+ EXPECT_TRUE((pose * m_slidingDirection).isApprox(Vector3d(1.0, 0.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[0]).isApprox(Vector3d(0.0, 1.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[1]).isApprox(Vector3d(0.0, 0.0, 1.0)));
+
+ Eigen::Matrix<double, 2, 30> H = Eigen::Matrix<double, 2, 30>::Zero();
+ H.block<1, 3>(0, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(0, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[1]).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 30, 30> C = dt * m_fem->getM().toDense().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(Fem1DConstraintFrictionlessSlidingTests, BuildMlcpIndiciesTest)
+{
+ // This test verifies the build mlcp function works given a hypothetical, preexisting mlcp problem.
+
+ auto implementation = std::make_shared<FemConstraintFrictionlessSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof() + 5, 3, 2);
+
+ // Suppose 5 dof and 1 constraint are defined elsewhere. Then H, CHt, HCHt, and b are prebuilt.
+ Eigen::SparseVector<double, Eigen::RowMajor, ptrdiff_t> localH;
+ localH.resize(5);
+ localH.reserve(5);
+ localH.insert(0) = 0.9478;
+ localH.insert(1) = -0.3807;
+ localH.insert(2) = 0.5536;
+ localH.insert(3) = -0.6944;
+ localH.insert(4) = 0.1815;
+
+ mlcpPhysicsProblem.H.coeffRef(0, 0) += localH.coeff(0);
+ mlcpPhysicsProblem.H.coeffRef(0, 1) += localH.coeff(1);
+ mlcpPhysicsProblem.H.coeffRef(0, 2) += localH.coeff(2);
+ mlcpPhysicsProblem.H.coeffRef(0, 3) += localH.coeff(3);
+ mlcpPhysicsProblem.H.coeffRef(0, 4) += localH.coeff(4);
+
+ 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 the Fem at 5th dof (1 or multiple representations exist before, covering a total of 5 dof)
+ // and the constraint at the index 1 (1 atomic constraint exists before this one)
+ size_t indexOfRepresentation = 5;
+ size_t indexOfConstraint = 1;
+
+ // Apply constraint between the first two nodes of the fem.
+ const Vector2d barycentric = Vector2d(0.24, 0.76);
+ IndexedLocalCoordinate coord(0, barycentric);
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, indexOfRepresentation, indexOfConstraint,
+ SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ auto pose = m_constraintData.getPose().inverse().rotation();
+ EXPECT_TRUE((pose * m_slidingDirection).isApprox(Vector3d(1.0, 0.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[0]).isApprox(Vector3d(0.0, 1.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[1]).isApprox(Vector3d(0.0, 0.0, 1.0)));
+
+ Eigen::Matrix<double, 2, 30> H = Eigen::Matrix<double, 2, 30>::Zero();
+ H.block<1, 3>(0, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(0, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[1]).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H.block(indexOfConstraint, indexOfRepresentation, 2, 30), epsilon);
+
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 30, 30> C = dt * m_fem->getM().toDense().inverse();
+
+ EXPECT_NEAR_EIGEN(
+ C * H.transpose(), mlcpPhysicsProblem.CHt.block(indexOfRepresentation, indexOfConstraint, 30, 2), epsilon);
+
+ EXPECT_NEAR_EIGEN(
+ H * C * H.transpose(), mlcpPhysicsProblem.A.block(indexOfConstraint, indexOfConstraint, 2, 2), epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
diff --git a/SurgSim/Physics/UnitTests/Fem1DElementBeamTests.cpp b/SurgSim/Physics/UnitTests/Fem1DElementBeamTests.cpp
index 5eb46df..b3b6906 100644
--- a/SurgSim/Physics/UnitTests/Fem1DElementBeamTests.cpp
+++ b/SurgSim/Physics/UnitTests/Fem1DElementBeamTests.cpp
@@ -21,6 +21,7 @@
#include "SurgSim/Framework/Assert.h"
#include "SurgSim/Math/GaussLegendreQuadrature.h"
#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeEquation.h"
#include "SurgSim/Math/OdeState.h"
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/Vector.h"
@@ -28,6 +29,7 @@
using SurgSim::Math::Matrix;
using SurgSim::Math::Quaterniond;
+using SurgSim::Math::SparseMatrix;
using SurgSim::Math::Vector;
using SurgSim::Math::Vector3d;
using SurgSim::Physics::Fem1DElementBeam;
@@ -40,12 +42,12 @@ const double epsilon = 1e-9;
class MockFem1DElement : public Fem1DElementBeam
{
public:
- MockFem1DElement(std::array<size_t, 2> nodeIds)
+ explicit MockFem1DElement(std::array<size_t, 2> nodeIds)
: Fem1DElementBeam(nodeIds)
{
}
- const Eigen::Matrix<double, 12, 12>& getInitialRotation() const
+ const Eigen::Matrix<double, 12, 12>& getInitialRotation12x12() const
{
return m_R0;
}
@@ -120,8 +122,12 @@ public:
double m_rho, m_E, m_nu, m_L;
double m_radius;
Quaterniond m_orientation;
+ Vector forceVector;
+ SparseMatrix massMatrix;
+ SparseMatrix dampingMatrix;
+ SparseMatrix stiffnessMatrix;
- virtual void SetUp() override
+ void SetUp() override
{
using SurgSim::Math::getSubVector;
@@ -138,7 +144,7 @@ public:
m_restState.setNumDof(6, m_numberNodes);
- m_orientation.coeffs().setRandom();
+ m_orientation.coeffs().setLinSpaced(2.0, 3.0);
m_orientation.normalize();
Vector3d firstExtremity(0.1, 1.2, 2.3);
@@ -147,6 +153,25 @@ public:
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;
+
+ // Initialize the global matrices for calculations.
+ std::vector<size_t> nodeIdsVectorForm(m_nodeIds.begin(), m_nodeIds.end());
+ forceVector = Vector::Zero(6 * m_numberNodes);
+ Fem1DElementBeam beam(m_nodeIds);
+ massMatrix.resize(6 * m_numberNodes, 6 * m_numberNodes);
+ beam.assembleMatrixBlocks(SurgSim::Math::Matrix::Zero(12, 12),
+ nodeIdsVectorForm, 6, &massMatrix, true);
+ massMatrix.makeCompressed();
+
+ dampingMatrix.resize(6 * m_numberNodes, 6 * m_numberNodes);
+ beam.assembleMatrixBlocks(SurgSim::Math::Matrix::Zero(12, 12),
+ nodeIdsVectorForm, 6, &dampingMatrix, true);
+ dampingMatrix.makeCompressed();
+
+ stiffnessMatrix.resize(6 * m_numberNodes, 6 * m_numberNodes);
+ beam.assembleMatrixBlocks(SurgSim::Math::Matrix::Zero(12, 12),
+ nodeIdsVectorForm, 6, &stiffnessMatrix, true);
+ stiffnessMatrix.makeCompressed();
}
void getExpectedMassMatrix(Eigen::Ref<SurgSim::Math::Matrix> mass)
@@ -162,52 +187,68 @@ public:
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;
+ 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);
+ 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;
+ 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;
+ 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;
+ 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);
+ 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;
+ 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);
+ 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;
+ 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;
+ 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;
+ 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);
+ 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;
@@ -314,52 +355,68 @@ public:
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;
+ 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);
+ 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;
+ 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;
+ 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;
+ 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);
+ 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;
+ -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);
+ 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;
+ 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;
+ 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;
+ 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);
+ 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);
@@ -371,7 +428,7 @@ public:
// Transform into correct coordinates and correct place in matrix
std::shared_ptr<MockFem1DElement> beam = getBeam();
- const Eigen::Matrix<double, 12, 12>& r = beam->getInitialRotation();
+ const Eigen::Matrix<double, 12, 12>& r = beam->getInitialRotation12x12();
SurgSim::Math::addSubMatrix(r * in * r.transpose(), nodeIdsVectorForm, 6, &out);
}
@@ -391,7 +448,7 @@ public:
TEST_F(Fem1DElementBeamTests, ConstructorTest)
{
ASSERT_NO_THROW(
- { MockFem1DElement beam(m_nodeIds); });
+ { MockFem1DElement beam(m_nodeIds); });
}
TEST_F(Fem1DElementBeamTests, NodeIdsTest)
@@ -480,19 +537,21 @@ TEST_F(Fem1DElementBeamTests, InitialRotationTest)
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());
+ EXPECT_TRUE(beam->getInitialRotation12x12().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);
+ Vector3d i_0 = beam->getInitialRotation12x12().block<3, 3>(0, 0).col(0);
+ Vector3d i_1 = beam->getInitialRotation12x12().block<3, 3>(3, 3).col(0);
+ Vector3d i_2 = beam->getInitialRotation12x12().block<3, 3>(6, 6).col(0);
+ Vector3d i_3 = beam->getInitialRotation12x12().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));
+
+ EXPECT_TRUE(beam->getInitialRotation().col(0).isApprox(expected_i));
}
TEST_F(Fem1DElementBeamTests, CoordinateTests)
@@ -526,13 +585,13 @@ TEST_F(Fem1DElementBeamTests, CoordinateTests)
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);
+ SurgSim::Framework::AssertionFailure);
EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSize1), \
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSize3), \
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSumNot1), \
- SurgSim::Framework::AssertionFailure);
+ 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));
@@ -554,10 +613,10 @@ TEST_F(Fem1DElementBeamTests, ForceAndMatricesTest)
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);
+ forceVector.setZero();
+ SurgSim::Math::clearMatrix(&massMatrix);
+ SurgSim::Math::clearMatrix(&dampingMatrix);
+ SurgSim::Math::clearMatrix(&stiffnessMatrix);
Matrix expectedMass(6 * m_numberNodes, 6 * m_numberNodes);
Matrix expectedMass2(6 * m_numberNodes, 6 * m_numberNodes);
@@ -568,26 +627,31 @@ TEST_F(Fem1DElementBeamTests, ForceAndMatricesTest)
getExpectedMassMatrix2(expectedMass2);
getExpectedStiffnessMatrix(expectedStiffness);
+ // Update the internal f, M, D, K variables.
+ beam->updateFMDK(m_restState, SurgSim::Math::ODEEQUATIONUPDATE_FMDK);
+
// No force should be produced when in rest state (x = x0) => F = K.(x-x0) = 0
- beam->addForce(m_restState, &forceVector);
+ beam->addForce(&forceVector);
EXPECT_TRUE(forceVector.isZero());
- beam->addMass(m_restState, &massMatrix);
- EXPECT_TRUE(massMatrix.isApprox(expectedMass));
- EXPECT_TRUE(massMatrix.isApprox(expectedMass2, 1e-6));
+ beam->addMass(&massMatrix);
+ EXPECT_TRUE(massMatrix.isApprox(expectedMass)) << "Expected Mass:" << std::endl << expectedMass << std::endl <<
+ "Mass Matrix:" << std::endl << massMatrix << std::endl;
+ EXPECT_TRUE(massMatrix.isApprox(expectedMass2, 1e-6)) << "Expected Mass 2:" << std::endl << expectedMass <<
+ std::endl << "Mass Matrix:" << std::endl << massMatrix << std::endl;
- beam->addDamping(m_restState, &dampingMatrix);
+ beam->addDamping(&dampingMatrix);
EXPECT_TRUE(dampingMatrix.isApprox(expectedDamping));
- beam->addStiffness(m_restState, &stiffnessMatrix);
+ beam->addStiffness(&stiffnessMatrix);
EXPECT_TRUE(stiffnessMatrix.isApprox(expectedStiffness));
forceVector.setZero();
- massMatrix.setZero();
- dampingMatrix.setZero();
- stiffnessMatrix.setZero();
+ SurgSim::Math::clearMatrix(&massMatrix);
+ SurgSim::Math::clearMatrix(&dampingMatrix);
+ SurgSim::Math::clearMatrix(&stiffnessMatrix);
- beam->addFMDK(m_restState, &forceVector, &massMatrix, &dampingMatrix, &stiffnessMatrix);
+ beam->addFMDK(&forceVector, &massMatrix, &dampingMatrix, &stiffnessMatrix);
EXPECT_TRUE(forceVector.isZero());
EXPECT_TRUE(massMatrix.isApprox(expectedMass));
EXPECT_TRUE(massMatrix.isApprox(expectedMass2, 1e-6));
@@ -596,7 +660,7 @@ TEST_F(Fem1DElementBeamTests, ForceAndMatricesTest)
// Test addMatVec API with Mass component only
forceVector.setZero();
- beam->addMatVec(m_restState, 1.0, 0.0, 0.0, vectorOnes, &forceVector);
+ beam->addMatVec(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);
@@ -604,21 +668,21 @@ TEST_F(Fem1DElementBeamTests, ForceAndMatricesTest)
}
// Test addMatVec API with Damping component only
forceVector.setZero();
- beam->addMatVec(m_restState, 0.0, 1.0, 0.0, vectorOnes, &forceVector);
+ beam->addMatVec(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);
+ beam->addMatVec(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);
+ beam->addMatVec(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()
diff --git a/SurgSim/Physics/UnitTests/Fem1DLocalizationTest.cpp b/SurgSim/Physics/UnitTests/Fem1DLocalizationTest.cpp
new file mode 100644
index 0000000..b9bee87
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem1DLocalizationTest.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 <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+#include "SurgSim/Physics/Fem1DLocalization.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentation.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 Fem1DLocalizationTest : 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_invalidIndexLocalPosition.coordinate = SurgSim::Math::Vector::Zero(2);
+ m_invalidIndexLocalPosition.coordinate[0] = 0.4;
+ m_invalidIndexLocalPosition.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(Fem1DLocalizationTest, ConstructorTest)
+{
+ // IndexedLocalCoordinate pointing to a node (node index + empty coordinate) are invalid. It will failed,
+ // either because the index is out of bound or because the coordinates are the wrong size (empty)
+ // This is tested by m_invalidIndexLocalPosition and m_invalidCoordinateLocalPosition
+ ASSERT_THROW(
+ std::make_shared<Fem1DLocalization>(m_fem, m_invalidIndexLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+
+ ASSERT_THROW(
+ std::make_shared<Fem1DLocalization>(m_fem, m_invalidCoordinateLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+
+ ASSERT_NO_THROW(std::make_shared<Fem1DLocalization>(m_fem, m_validLocalPosition););
+}
+
+TEST_F(Fem1DLocalizationTest, IsValidRepresentation)
+{
+ Fem1DLocalization localization(m_fem, m_validLocalPosition);
+
+ ASSERT_TRUE(localization.isValidRepresentation(m_fem));
+
+ // nullptr is valid
+ ASSERT_TRUE(localization.isValidRepresentation(nullptr));
+
+ ASSERT_FALSE(localization.isValidRepresentation(std::make_shared<Fem2DRepresentation>("fem2d")));
+ ASSERT_FALSE(localization.isValidRepresentation(std::make_shared<Fem3DRepresentation>("fem3d")));
+}
+
+TEST_F(Fem1DLocalizationTest, MoveClosestTo)
+{
+ Fem1DLocalization localization(m_fem, m_validLocalPosition);
+
+ {
+ SurgSim::DataStructures::IndexedLocalCoordinate testPosition1;
+ testPosition1.index = 1;
+ testPosition1.coordinate = SurgSim::Math::Vector::Zero(2);
+ testPosition1.coordinate[0] = 0.5;
+ testPosition1.coordinate[1] = 0.5;
+ Fem1DLocalization testLocalization1(m_fem, testPosition1);
+
+ bool hasReachedEnd = false;
+ EXPECT_TRUE(localization.moveClosestTo(testLocalization1.calculatePosition(), &hasReachedEnd));
+ EXPECT_FALSE(hasReachedEnd);
+ EXPECT_TRUE(localization.calculatePosition().isApprox(testLocalization1.calculatePosition()));
+ }
+
+ {
+ SurgSim::DataStructures::IndexedLocalCoordinate testPosition1;
+ testPosition1.index = 0;
+ testPosition1.coordinate = SurgSim::Math::Vector::Zero(2);
+ testPosition1.coordinate[0] = 0.1;
+ testPosition1.coordinate[1] = 0.9;
+ Fem1DLocalization testLocalization1(m_fem, testPosition1);
+
+ bool hasReachedEnd = false;
+ EXPECT_TRUE(localization.moveClosestTo(testLocalization1.calculatePosition(), &hasReachedEnd));
+ EXPECT_FALSE(hasReachedEnd);
+ EXPECT_TRUE(localization.calculatePosition().isApprox(testLocalization1.calculatePosition()));
+ }
+
+ {
+ SurgSim::DataStructures::IndexedLocalCoordinate testPosition1;
+ testPosition1.index = 0;
+ testPosition1.coordinate = SurgSim::Math::Vector::Zero(2);
+ testPosition1.coordinate[0] = 1.0;
+ testPosition1.coordinate[1] = 0.0;
+ Fem1DLocalization testLocalization1(m_fem, testPosition1);
+
+ bool hasReachedEnd = false;
+ EXPECT_TRUE(localization.moveClosestTo(testLocalization1.calculatePosition(), &hasReachedEnd));
+ EXPECT_TRUE(hasReachedEnd);
+ EXPECT_TRUE(localization.calculatePosition().isApprox(testLocalization1.calculatePosition()));
+ }
+}
+
+} // namespace SurgSim
+} // namespace Physics
diff --git a/SurgSim/Physics/UnitTests/Fem1DMechanicalValidationTests.cpp b/SurgSim/Physics/UnitTests/Fem1DMechanicalValidationTests.cpp
index 43dd819..8086bc2 100644
--- a/SurgSim/Physics/UnitTests/Fem1DMechanicalValidationTests.cpp
+++ b/SurgSim/Physics/UnitTests/Fem1DMechanicalValidationTests.cpp
@@ -26,6 +26,7 @@
#include "SurgSim/Math/Vector.h"
#include "SurgSim/Physics/Fem1DRepresentation.h"
#include "SurgSim/Physics/Fem1DElementBeam.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
using SurgSim::Math::Matrix;
using SurgSim::Math::Vector;
@@ -60,7 +61,7 @@ public:
size_t& nodes = nodesPerDimension[0];
SURGSIM_ASSERT(nodes > 0) << "Number of nodes incorrect: " << nodes;
- auto fem = std::make_shared<Fem1DRepresentation>(name);
+ auto fem = std::make_shared<MockFem1DRepresentation>(name);
auto state = std::make_shared<SurgSim::Math::OdeState>();
state->setNumDof(fem->getNumDofPerNode(), nodes);
@@ -98,6 +99,7 @@ public:
}
fem->setInitialState(state);
+ fem->doInitialize();
return fem;
}
@@ -145,7 +147,7 @@ public:
Fem1DBuilder m_fem1DBuilder;
protected:
- virtual void SetUp() override
+ void SetUp() override
{
using SurgSim::Math::getSubVector;
@@ -174,10 +176,10 @@ protected:
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;
+ 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;
+ 2.0, 0.0, 0.0, 0.0, 0.0, 0.0;
// Expected transformed values
m_expectedTransformedPositions.resize(m_initialState->getNumDof());
@@ -232,7 +234,8 @@ TEST_F(Fem1DMechanicalValidationTests, CantileverEndLoadedTest)
Vector applyForce = Vector::Zero(nodesPerDim * 6);
applyForce[applyIndex] = load;
- Matrix stiffness = fem->computeK(*fem->getInitialState());
+ fem->updateFMDK(*(m_fem->getInitialState()), Math::ODEEQUATIONUPDATE_K);
+ Matrix stiffness = fem->getK();
// Apply boundary conditions
fem->getInitialState()->applyBoundaryConditionsToVector(&applyForce);
@@ -265,7 +268,8 @@ TEST_F(Fem1DMechanicalValidationTests, CantileverPunctualLoadAnywhereTest)
Vector applyForce = Vector::Zero(nodesPerDim * 6);
applyForce[applyIndex] = load;
- Matrix stiffness = fem->computeK(*fem->getInitialState());
+ fem->updateFMDK(*(m_fem->getInitialState()), Math::ODEEQUATIONUPDATE_K);
+ Matrix stiffness = fem->getK();
// Apply boundary conditions
fem->getInitialState()->applyBoundaryConditionsToVector(&applyForce);
@@ -281,7 +285,7 @@ TEST_F(Fem1DMechanicalValidationTests, CantileverPunctualLoadAnywhereTest)
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);
+ : load * a * a * (3 * x - a) / (6 * m_E * m_Iz);
EXPECT_NEAR(deflection, calculatedDeflection[lookIndex], 1e-8);
}
@@ -303,7 +307,8 @@ TEST_F(Fem1DMechanicalValidationTests, CantileverEndBentTest)
Vector applyForce = Vector::Zero(nodesPerDim * 6);
applyForce[applyIndex] = moment;
- Matrix stiffness = fem->computeK(*fem->getInitialState());
+ fem->updateFMDK(*(m_fem->getInitialState()), Math::ODEEQUATIONUPDATE_K);
+ Matrix stiffness = fem->getK();
// Apply boundary conditions
fem->getInitialState()->applyBoundaryConditionsToVector(&applyForce);
@@ -339,7 +344,8 @@ TEST_F(Fem1DMechanicalValidationTests, EndSupportedBeamCenterLoadedTest)
Vector applyForce = Vector::Zero(nodesPerDim * 6);
applyForce[applyIndex] = load;
- Matrix stiffness = fem->computeK(*fem->getInitialState());
+ fem->updateFMDK(*(m_fem->getInitialState()), Math::ODEEQUATIONUPDATE_K);
+ Matrix stiffness = fem->getK();
// Apply boundary conditions
fem->getInitialState()->applyBoundaryConditionsToVector(&applyForce);
@@ -373,7 +379,8 @@ TEST_F(Fem1DMechanicalValidationTests, EndSupportedBeamIntermediatelyLoadedTest)
Vector applyForce = Vector::Zero(nodesPerDim * 6);
applyForce[applyIndex] = load;
- Matrix stiffness = fem->computeK(*fem->getInitialState());
+ fem->updateFMDK(*(m_fem->getInitialState()), Math::ODEEQUATIONUPDATE_K);
+ Matrix stiffness = fem->getK();
// Apply boundary conditions
fem->getInitialState()->applyBoundaryConditionsToVector(&applyForce);
diff --git a/SurgSim/Physics/UnitTests/Fem1DPlyReaderDelegateTests.cpp b/SurgSim/Physics/UnitTests/Fem1DPlyReaderDelegateTests.cpp
index f4c1128..a037717 100644
--- a/SurgSim/Physics/UnitTests/Fem1DPlyReaderDelegateTests.cpp
+++ b/SurgSim/Physics/UnitTests/Fem1DPlyReaderDelegateTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -18,9 +18,8 @@
#include "SurgSim/Framework/Runtime.h"
#include "SurgSim/Math/OdeState.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem.h"
#include "SurgSim/Physics/Fem1DPlyReaderDelegate.h"
-#include "SurgSim/Physics/Fem1DRepresentation.h"
-#include "SurgSim/Physics/Fem1DElementBeam.h"
namespace SurgSim
{
@@ -31,58 +30,76 @@ using SurgSim::DataStructures::PlyReader;
TEST(Fem1DRepresentationReaderTests, DelegateTest)
{
- auto femRepresentation = std::make_shared<Fem1DRepresentation>("Representation");
+ auto fem = std::make_shared<Fem1D>();
auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
- femRepresentation->setFilename("PlyReaderTests/Fem1D.ply");
- ASSERT_TRUE(femRepresentation->initialize(runtime));
+ fem->load("PlyReaderTests/Fem1D.ply");
// 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)));
+ EXPECT_TRUE(vertex0.isApprox(fem->getVertex(0).position));
+ EXPECT_TRUE(vertex6.isApprox(fem->getVertex(6).position));
// Number of beams
- ASSERT_EQ(4u, femRepresentation->getNumFemElements());
+ ASSERT_EQ(4u, fem->getNumElements());
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())));
+ std::begin(fem->getElement(0)->nodeIds)));
EXPECT_TRUE(std::equal(std::begin(beam2), std::end(beam2),
- std::begin(femRepresentation->getFemElement(2)->getNodeIds())));
+ std::begin(fem->getElement(2)->nodeIds)));
// Boundary conditions
- ASSERT_EQ(3u * 6u, femRepresentation->getInitialState()->getNumBoundaryConditions());
+ ASSERT_EQ(3u, fem->getBoundaryConditions().size());
+
+ EXPECT_EQ(2, fem->getBoundaryCondition(0));
+ EXPECT_EQ(4, fem->getBoundaryCondition(1));
+ EXPECT_EQ(5, fem->getBoundaryCondition(2));
+
+ // Material
+ for (size_t i = 0; i < fem->getNumElements(); ++i)
+ {
+ auto element = fem->getElement(i);
+ EXPECT_DOUBLE_EQ(0.21, element->massDensity);
+ EXPECT_DOUBLE_EQ(0.31, element->poissonRatio);
+ EXPECT_DOUBLE_EQ(0.41, element->youngModulus);
+ }
+}
- // Boundary condition 0 is on node 8
- size_t boundaryNode0 = 2;
- size_t boundaryNode2 = 5;
+TEST(Fem1DRepresentationReaderTests, PerElementMaterial)
+{
+ auto fem = std::make_shared<Fem1D>();
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
- 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));
+ fem->load("PlyReaderTests/Fem1DMaterial.ply");
// Material
- for (size_t i = 0; i < femRepresentation->getNumFemElements(); ++i)
+ double value = 1.0;
+ for (size_t i = 0; i < fem->getNumElements(); ++i)
+ {
+ auto element = fem->getElement(i);
+ EXPECT_DOUBLE_EQ(value++, element->massDensity);
+ EXPECT_DOUBLE_EQ(value++, element->poissonRatio);
+ EXPECT_DOUBLE_EQ(value++, element->youngModulus);
+ }
+}
+
+TEST(Fem1DRepresentationReaderTests, NoMaterials)
+{
+ auto fem = std::make_shared<Fem1D>();
+ auto runtime = std::make_shared<Framework::Runtime>("config.txt");
+
+ ASSERT_NO_THROW(fem->load("PlyReaderTests/Fem1DNoMaterial.ply"));
+
+ for (auto element : fem->getElements())
{
- 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());
+ EXPECT_DOUBLE_EQ(0.0, element->massDensity);
+ EXPECT_DOUBLE_EQ(0.0, element->poissonRatio);
+ EXPECT_DOUBLE_EQ(0.0, element->youngModulus);
}
}
diff --git a/SurgSim/Physics/UnitTests/Fem1DRepresentationLocalizationTest.cpp b/SurgSim/Physics/UnitTests/Fem1DRepresentationLocalizationTest.cpp
deleted file mode 100644
index 4a1379b..0000000
--- a/SurgSim/Physics/UnitTests/Fem1DRepresentationLocalizationTest.cpp
+++ /dev/null
@@ -1,228 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
index bb35dec..28ee58c 100644
--- a/SurgSim/Physics/UnitTests/Fem1DRepresentationTests.cpp
+++ b/SurgSim/Physics/UnitTests/Fem1DRepresentationTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -23,9 +23,10 @@
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
#include "SurgSim/Physics/Fem1DElementBeam.h"
+#include "SurgSim/Physics/Fem1DLocalization.h"
#include "SurgSim/Physics/Fem1DRepresentation.h"
-#include "SurgSim/Physics/Fem1DRepresentationLocalization.h"
#include "SurgSim/Physics/UnitTests/MockObjects.h"
namespace SurgSim
@@ -39,12 +40,6 @@ 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");
@@ -96,8 +91,8 @@ TEST(Fem1DRepresentationTests, TransformInitialStateTest)
TEST(Fem1DRepresentationTests, DoWakeUpTest)
{
- using SurgSim::Math::LinearSolveAndInverse;
- using SurgSim::Math::LinearSolveAndInverseTriDiagonalBlockMatrix;
+ using SurgSim::Math::LinearSparseSolveAndInverse;
+ using SurgSim::Math::LinearSparseSolveAndInverseLU;
std::shared_ptr<MockFem1DRepresentation> fem = std::make_shared<MockFem1DRepresentation>("Fem1D");
std::shared_ptr<SurgSim::Math::OdeState> initialState = std::make_shared<SurgSim::Math::OdeState>();
@@ -111,10 +106,10 @@ TEST(Fem1DRepresentationTests, DoWakeUpTest)
// Test that the OdeSolver has the proper linear solver type
EXPECT_NE(nullptr, fem->getOdeSolver());
- std::shared_ptr<LinearSolveAndInverse> linearSolver = fem->getOdeSolver()->getLinearSolver();
+ std::shared_ptr<LinearSparseSolveAndInverse> linearSolver = fem->getOdeSolver()->getLinearSolver();
EXPECT_NE(nullptr, linearSolver);
- std::shared_ptr<LinearSolveAndInverseTriDiagonalBlockMatrix<6>> expectedLinearSolverType;
- expectedLinearSolverType = std::dynamic_pointer_cast<LinearSolveAndInverseTriDiagonalBlockMatrix<6>>(linearSolver);
+ std::shared_ptr<LinearSparseSolveAndInverseLU> expectedLinearSolverType;
+ expectedLinearSolverType = std::dynamic_pointer_cast<LinearSparseSolveAndInverseLU>(linearSolver);
EXPECT_NE(nullptr, expectedLinearSolverType);
}
@@ -133,6 +128,10 @@ TEST(Fem1DRepresentationTests, ExternalForceAPITest)
fem->setInitialState(initialState);
+ Math::SparseMatrix zeroMat(static_cast<SparseMatrix::Index>(fem->getNumDof()),
+ static_cast<SparseMatrix::Index>(fem->getNumDof()));
+ zeroMat.setZero();
+
// Vector initialized (properly sized and zeroed)
EXPECT_NE(0, fem->getExternalGeneralizedForce().size());
EXPECT_NE(0, fem->getExternalGeneralizedStiffness().rows());
@@ -145,8 +144,8 @@ TEST(Fem1DRepresentationTests, ExternalForceAPITest)
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());
+ EXPECT_TRUE(fem->getExternalGeneralizedStiffness().isApprox(zeroMat));
+ EXPECT_TRUE(fem->getExternalGeneralizedDamping().isApprox(zeroMat));
std::array<size_t, 2> element1NodeIds = {{0, 1}};
auto element1 = std::make_shared<Fem1DElementBeam>(element1NodeIds);
@@ -159,49 +158,59 @@ TEST(Fem1DRepresentationTests, ExternalForceAPITest)
femRepCoordinate.index = 0;
femRepCoordinate.coordinate = SurgSim::Math::Vector::Zero(2);
femRepCoordinate.coordinate[0] = 1.0;
- auto localization = std::make_shared<Fem1DRepresentationLocalization>(fem, femRepCoordinate);
+ auto localization = std::make_shared<Fem1DLocalization>(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;
+ SparseMatrix K(static_cast<SparseMatrix::Index>(fem->getNumDof()),
+ static_cast<SparseMatrix::Index>(fem->getNumDof()));
+ K.setZero();
+ Math::addSubMatrix(Klocal, 0, 0, &K, true);
+ K.makeCompressed();
+ SparseMatrix D(static_cast<SparseMatrix::Index>(fem->getNumDof()),
+ static_cast<SparseMatrix::Index>(fem->getNumDof()));
+ D.setZero();
+ Math::addSubMatrix(Dlocal, 0, 0, &D, true);
+ D.makeCompressed();
// Test invalid localization nullptr
ASSERT_THROW(fem->addExternalGeneralizedForce(nullptr, Flocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
ASSERT_THROW(fem->addExternalGeneralizedForce(nullptr, Flocal, Klocal, Dlocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
// Test invalid localization type
ASSERT_THROW(fem->addExternalGeneralizedForce(wrongLocalizationType, Flocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
ASSERT_THROW(fem->addExternalGeneralizedForce(wrongLocalizationType, Flocal, Klocal, Dlocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
// Test invalid force size
ASSERT_THROW(fem->addExternalGeneralizedForce(localization, FLocalWrongSize),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
ASSERT_THROW(fem->addExternalGeneralizedForce(localization, FLocalWrongSize, Klocal, Dlocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
// Test invalid stiffness size
ASSERT_THROW(fem->addExternalGeneralizedForce(localization, Flocal, KLocalWrongSize, Dlocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
// Test invalid damping size
ASSERT_THROW(fem->addExternalGeneralizedForce(localization, Flocal, Klocal, DLocalWrongSize),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
// Test valid call to addExternalGeneralizedForce
fem->addExternalGeneralizedForce(localization, Flocal, Klocal, Dlocal);
+ SparseMatrix zeroMatrix(K.rows(), K.cols());
+ zeroMatrix.setZero();
EXPECT_FALSE(fem->getExternalGeneralizedForce().isZero());
- EXPECT_FALSE(fem->getExternalGeneralizedStiffness().isZero());
- EXPECT_FALSE(fem->getExternalGeneralizedDamping().isZero());
+ EXPECT_FALSE(fem->getExternalGeneralizedStiffness().isApprox(zeroMatrix));
+ EXPECT_FALSE(fem->getExternalGeneralizedDamping().isApprox(zeroMatrix));
EXPECT_TRUE(fem->getExternalGeneralizedForce().isApprox(F));
EXPECT_TRUE(fem->getExternalGeneralizedStiffness().isApprox(K));
EXPECT_TRUE(fem->getExternalGeneralizedDamping().isApprox(D));
@@ -213,25 +222,190 @@ TEST(Fem1DRepresentationTests, ExternalForceAPITest)
EXPECT_TRUE(fem->getExternalGeneralizedDamping().isApprox(2.0 * D));
}
+TEST(Fem1DRepresentationTests, LoadMeshTest)
+{
+ auto femRepresentation = std::make_shared<Fem1DRepresentation>("Representation");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+
+ femRepresentation->loadFem("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());
+ }
+}
+
+TEST(Fem1DRepresentationTests, CreateLocalizationTest)
+{
+ auto fem = std::make_shared<Fem1DRepresentation>("Representation");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ ASSERT_NO_THROW(fem->loadFem("PlyReaderTests/Fem1D.ply"));
+ ASSERT_NO_THROW(ASSERT_TRUE(fem->initialize(runtime)));
+
+ // Localization on an invalid node
+ {
+ SCOPED_TRACE("Invalid node");
+
+ SurgSim::DataStructures::Location location(1000);
+ std::shared_ptr<SurgSim::Physics::Fem1DLocalization> localization;
+ EXPECT_THROW(localization =
+ std::dynamic_pointer_cast<SurgSim::Physics::Fem1DLocalization>(
+ fem->createLocalization(location)), SurgSim::Framework::AssertionFailure);
+ }
+
+ // Localization on a valid node
+ {
+ SCOPED_TRACE("Valid node");
+
+ SurgSim::DataStructures::Location location(0);
+ std::shared_ptr<SurgSim::Physics::Fem1DLocalization> localization;
+ EXPECT_NO_THROW(localization =
+ std::dynamic_pointer_cast<SurgSim::Physics::Fem1DLocalization>(fem->createLocalization(location)););
+ EXPECT_TRUE(localization != nullptr);
+ EXPECT_TRUE(localization->getRepresentation() == fem);
+
+ SurgSim::Math::Vector3d globalPosition;
+ SurgSim::DataStructures::IndexedLocalCoordinate coordinate = localization->getLocalPosition();
+ EXPECT_NO_THROW(globalPosition = fem->getCurrentState()->getPosition(coordinate.index););
+ EXPECT_TRUE(globalPosition.isApprox(localization->calculatePosition()));
+ }
+
+ // In the 1d case, location of type triangle do not make sense and should raise an exception
+ // Localization on invalid triangle (no triangle exist, so any triangle id would be invalid)
+ {
+ SCOPED_TRACE("Triangle => all invalid");
+
+ Vector barycentricCoordinate = Vector::Zero(3);
+ barycentricCoordinate[0] = 1.0;
+ SurgSim::DataStructures::IndexedLocalCoordinate coord(0, barycentricCoordinate);
+ SurgSim::DataStructures::Location location(coord, SurgSim::DataStructures::Location::TRIANGLE);
+ std::shared_ptr<SurgSim::Physics::Localization> localization;
+ EXPECT_THROW(localization = fem->createLocalization(location), SurgSim::Framework::AssertionFailure);
+ }
+
+ // Localization on an invalid element
+ {
+ SCOPED_TRACE("Invalid element index");
+
+ Vector barycentricCoordinate = Vector::Zero(2);
+ barycentricCoordinate[0] = 1.0;
+ SurgSim::DataStructures::IndexedLocalCoordinate coord(10000, barycentricCoordinate);
+ SurgSim::DataStructures::Location location(coord, SurgSim::DataStructures::Location::ELEMENT);
+ std::shared_ptr<SurgSim::Physics::Localization> localization;
+ EXPECT_THROW(localization = fem->createLocalization(location), SurgSim::Framework::AssertionFailure);
+ }
+
+ // Localization on an valid element but invalid barycentric coordinate size
+ {
+ SCOPED_TRACE("Invalid element barycentric coordinate size");
+
+ Vector barycentricCoordinate = Vector::Zero(3);
+ barycentricCoordinate[0] = 1.0;
+ SurgSim::DataStructures::IndexedLocalCoordinate coord(0, barycentricCoordinate);
+ SurgSim::DataStructures::Location location(coord, SurgSim::DataStructures::Location::ELEMENT);
+ std::shared_ptr<SurgSim::Physics::Localization> localization;
+ EXPECT_THROW(localization = fem->createLocalization(location), SurgSim::Framework::AssertionFailure);
+ }
+
+ // Localization on an valid element but invalid barycentric coordinate
+ {
+ SCOPED_TRACE("Invalid element barycentric coordinate");
+
+ Vector barycentricCoordinate = Vector::Zero(2);
+ SurgSim::DataStructures::IndexedLocalCoordinate coord(0, barycentricCoordinate);
+ SurgSim::DataStructures::Location location(coord, SurgSim::DataStructures::Location::ELEMENT);
+ std::shared_ptr<SurgSim::Physics::Localization> localization;
+ EXPECT_THROW(localization = fem->createLocalization(location), SurgSim::Framework::AssertionFailure);
+ }
+
+ // Localization on a valid element
+ {
+ SCOPED_TRACE("Valid element");
+
+ Vector barycentricCoordinate = Vector::Zero(2);
+ barycentricCoordinate.setConstant(1.0 / 2.0);
+ SurgSim::DataStructures::IndexedLocalCoordinate coord(0, barycentricCoordinate);
+ SurgSim::DataStructures::Location location(coord, SurgSim::DataStructures::Location::ELEMENT);
+ std::shared_ptr<SurgSim::Physics::Fem1DLocalization> localization;
+ EXPECT_NO_THROW(localization =
+ std::dynamic_pointer_cast<SurgSim::Physics::Fem1DLocalization>(fem->createLocalization(location)));
+ EXPECT_TRUE(localization != nullptr);
+ EXPECT_TRUE(localization->getRepresentation() == fem);
+
+ SurgSim::DataStructures::IndexedLocalCoordinate coordinate = localization->getLocalPosition();
+ auto element = fem->getFemElement(coordinate.index);
+ SurgSim::Math::Vector3d globalPosition =
+ 1.0 / 2.0 * fem->getCurrentState()->getPosition(element->getNodeId(0)) +
+ 1.0 / 2.0 * fem->getCurrentState()->getPosition(element->getNodeId(1));
+ EXPECT_TRUE(globalPosition.isApprox(localization->calculatePosition()));
+ }
+}
+
TEST(Fem1DRepresentationTests, SerializationTest)
{
- auto fem1DRepresentation = std::make_shared<Fem1DRepresentation>("Test-Fem1D");
+ auto fem1DRepresentation = std::make_shared<SurgSim::Physics::Fem1DRepresentation>("Test-Fem1D");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ const std::string filename = "PlyReaderTests/Fem1D.ply";
+ fem1DRepresentation->loadFem(filename);
+ auto collisionRepresentation = std::make_shared<DeformableCollisionRepresentation>("Collision");
+ fem1DRepresentation->setCollisionRepresentation(collisionRepresentation);
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_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());
+ EXPECT_EQ(filename, newRepresentation->getFem()->getValue<std::string>("FileName"));
}
} // namespace Physics
diff --git a/SurgSim/Physics/UnitTests/Fem2DConstraintFixedPointTests.cpp b/SurgSim/Physics/UnitTests/Fem2DConstraintFixedPointTests.cpp
new file mode 100644
index 0000000..5dfb97d
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem2DConstraintFixedPointTests.cpp
@@ -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.
+
+#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/Fem2DElementTriangle.h"
+#include "SurgSim/Physics/Fem2DLocalization.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/FemConstraintFixedPoint.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+using SurgSim::Math::Vector6d;
+
+namespace
+{
+const double epsilon = 1e-10;
+const double dt = 1e-3;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+static std::shared_ptr<Fem2DElementTriangle> getTriangle(size_t nodeId0, size_t nodeId1, size_t nodeId2,
+ double thickness,
+ double massDensity,
+ double poissonRatio,
+ double youngModulus)
+{
+ std::array<size_t, 3> nodeIds = {{nodeId0, nodeId1, nodeId2}};
+ auto element = std::make_shared<Fem2DElementTriangle>(nodeIds);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ element->setThickness(thickness);
+ return element;
+}
+
+static std::shared_ptr<Fem2DRepresentation> getFem2d(const std::string &name,
+ double thickness = 0.01,
+ double massDensity = 1.0,
+ double poissonRatio = 0.1,
+ double youngModulus = 1.0)
+{
+ auto fem = std::make_shared<Fem2DRepresentation>(name);
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(6, 5);
+
+ std::array<double, 6> p0 = {{0.11, 0.22, 0.33, 0.44, 0.55, 0.66}};
+ std::array<double, 6> p1 = {{0.12, -0.23, 0.34, -0.45, 0.56, -0.67}};
+ std::array<double, 6> p2 = {{-0.10, 0.21, -0.32, 0.43, -0.54, 0.65}};
+ std::array<double, 6> p3 = {{0.2, 0.2, 0.2, 0.2, 0.2, 0.2}};
+ std::array<double, 6> p4 = {{-0.1, 0.1, -0.2, 0.2, -0.3, 0.3}};
+
+ state->getPositions().segment<6>(0 * 6) = Vector6d(p0.data());
+ state->getPositions().segment<6>(1 * 6) = Vector6d(p1.data());
+ state->getPositions().segment<6>(2 * 6) = Vector6d(p2.data());
+ state->getPositions().segment<6>(3 * 6) = Vector6d(p3.data());
+ state->getPositions().segment<6>(4 * 6) = Vector6d(p4.data());
+
+ fem->addFemElement(getTriangle(0, 1, 2, thickness, massDensity, poissonRatio, youngModulus));
+ fem->addFemElement(getTriangle(1, 2, 3, thickness, massDensity, poissonRatio, youngModulus));
+ fem->addFemElement(getTriangle(2, 3, 4, thickness, 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(Fem2DConstraintFixedPointTests, Constructor)
+{
+ ASSERT_NO_THROW({ FemConstraintFixedPoint constraint; });
+}
+
+TEST(Fem2DConstraintFixedPointTests, Constants)
+{
+ FemConstraintFixedPoint constraint;
+
+ EXPECT_EQ(SurgSim::Physics::FIXED_3DPOINT, constraint.getConstraintType());
+ EXPECT_EQ(3u, constraint.getNumDof());
+}
+
+TEST(Fem2DConstraintFixedPointTests, 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 necessarily construct the physical parameters
+ // necessary to supply a realistic C. It only checks H and b.
+ FemConstraintFixedPoint constraint;
+
+ Vector3d actual;
+
+ // Setup parameters for FemConstraintFixedPoint::build
+ auto localization = std::make_shared<Fem2DLocalization>(getFem2d("representation"),
+ SurgSim::DataStructures::IndexedLocalCoordinate(1u, Vector3d(0.0, 0.0, 1.0)));
+
+ actual = localization->calculatePosition();
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(30, 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, 30> H = Eigen::Matrix<double, 3, 30>::Zero();
+ Eigen::Matrix<double, 3, 3> identity = Eigen::Matrix<double, 3, 3>::Identity();
+ SurgSim::Math::setSubMatrix(1.0 * dt * identity, 0, 6, 3, 3, &H); // This weight is on node 3 (triId 1, nodeId 2)
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST(Fem2DConstraintFixedPointTests, 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 necessarily construct the physical parameters
+ // necessary to supply a realistic C. It only checks H and b.
+ FemConstraintFixedPoint constraint;
+
+ Vector3d actual;
+
+ // Setup parameters for FemConstraintFixedPoint::build
+ auto localization = std::make_shared<Fem2DLocalization>(getFem2d("representation"),
+ SurgSim::DataStructures::IndexedLocalCoordinate(1u, Vector3d(0.11, 0.02, 0.87)));
+
+ actual = localization->calculatePosition();
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(30, 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, 30> H = Eigen::Matrix<double, 3, 30>::Zero();
+ Eigen::Matrix<double, 3, 3> identity = Eigen::Matrix<double, 3, 3>::Identity();
+ SurgSim::Math::setSubMatrix(0.11 * dt * identity, 0, 2, 3, 3, &H); // This weight is on node 1 (triId 1, nodeId 0)
+ SurgSim::Math::setSubMatrix(0.02 * dt * identity, 0, 4, 3, 3, &H); // This weight is on node 2 (triId 1, nodeId 1)
+ SurgSim::Math::setSubMatrix(0.87 * dt * identity, 0, 6, 3, 3, &H); // This weight is on node 3 (triId 1, nodeId 2)
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST(Fem2DConstraintFixedPointTests, 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 necessarily construct the physical parameters
+ // necessary to supply a realistic C. It only checks H and b.
+ FemConstraintFixedPoint constraint;
+
+ Vector3d actual;
+ Vector3d desired;
+
+ // Setup parameters for FemConstraintFixedPoint::build
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(30, 3, 1);
+
+ ConstraintData emptyConstraint;
+
+ auto localization = std::make_shared<Fem2DLocalization>(getFem2d("representation"),
+ SurgSim::DataStructures::IndexedLocalCoordinate(1u, Vector3d(0.11, 0.02, 0.87)));
+ actual = localization->calculatePosition();
+ ASSERT_NO_THROW(constraint.build(
+ dt, emptyConstraint, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE));
+
+ localization->setLocalPosition(
+ SurgSim::DataStructures::IndexedLocalCoordinate(2u, Vector3d(0.32, 0.5, 0.18)));
+ 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, 30> H = Eigen::Matrix<double, 3, 30>::Zero();
+ Eigen::Matrix<double, 3, 3> identity = Eigen::Matrix<double, 3, 3>::Identity();
+ SurgSim::Math::addSubMatrix( 0.11 * dt * identity, 0, 2, 3, 3, &H); // This weight is on node 1 (triId 1, nodeId 0)
+ SurgSim::Math::addSubMatrix( 0.02 * dt * identity, 0, 4, 3, 3, &H); // This weight is on node 2 (triId 1, nodeId 1)
+ SurgSim::Math::addSubMatrix( 0.87 * dt * identity, 0, 6, 3, 3, &H); // This weight is on node 3 (triId 1, nodeId 2)
+ SurgSim::Math::addSubMatrix(-0.32 * dt * identity, 0, 4, 3, 3, &H); // This weight is on node 2 (triId 2, nodeId 0)
+ SurgSim::Math::addSubMatrix(-0.50 * dt * identity, 0, 6, 3, 3, &H); // This weight is on node 3 (triId 2, nodeId 1)
+ SurgSim::Math::addSubMatrix(-0.18 * dt * identity, 0, 8, 3, 3, &H); // This weight is on node 4 (triId 2, nodeId 2)
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/Fem2DConstraintFrictionalSlidingTests.cpp b/SurgSim/Physics/UnitTests/Fem2DConstraintFrictionalSlidingTests.cpp
new file mode 100644
index 0000000..52149fb
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem2DConstraintFrictionalSlidingTests.cpp
@@ -0,0 +1,288 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/LinearSparseSolveAndInverse.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem2DElementTriangle.h"
+#include "SurgSim/Physics/Fem2DLocalization.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/FemConstraintFrictionalSliding.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/SlidingConstraintData.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+using SurgSim::Framework::Runtime;
+using SurgSim::Physics::Fem2DRepresentation;
+using SurgSim::Physics::FemConstraintFrictionalSliding;
+using SurgSim::Physics::Fem2DLocalization;
+using SurgSim::Physics::Fem2DElementTriangle;
+using SurgSim::Physics::MlcpPhysicsProblem;
+using SurgSim::Physics::MockFem2DRepresentation;
+using SurgSim::Physics::SlidingConstraintData;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+ const double epsilon = 1e-10;
+ const double dt = 1e-3;
+};
+
+static void addTriangle(Fem2DRepresentation* fem,
+ size_t node0, size_t node1, size_t node2,
+ double thickness = 0.01,
+ double massDensity = 1.0,
+ double poissonRatio = 0.1,
+ double youngModulus = 1.0)
+{
+ std::array<size_t, 3> nodes = {node0, node1, node2};
+ auto element = std::make_shared<Fem2DElementTriangle>(nodes);
+ element->setThickness(thickness);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ fem->addFemElement(element);
+}
+
+class Fem2DConstraintFrictionalSlidingTests : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ // Define plane with normal 'n' pointing against gravity.
+ m_slidingDirection = Vector3d(0.8539, 0.6289, -0.9978);
+ m_slidingDirection.normalize();
+
+ // Create mock FEM
+ m_fem = std::make_shared<MockFem2DRepresentation>("Fem2dRepresentation");
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(6, 5);
+
+ state->getPositions().segment<3>(0 * 6) = Vector3d(0.30, -0.57, 0.40);
+ state->getPositions().segment<3>(1 * 6) = Vector3d(0.06, 0.63, -0.32);
+ state->getPositions().segment<3>(2 * 6) = Vector3d(-0.91, 0.72, 0.72);
+ state->getPositions().segment<3>(3 * 6) = Vector3d(0.35, 0.52, 0.50);
+ state->getPositions().segment<3>(4 * 6) = Vector3d(1.14, 0.66, 0.71);
+
+ addTriangle(m_fem.get(), 0, 1, 2);
+ addTriangle(m_fem.get(), 1, 2, 3);
+ addTriangle(m_fem.get(), 2, 3, 4);
+
+ m_fem->setInitialState(state);
+ m_fem->setIntegrationScheme(SurgSim::Math::IntegrationScheme::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED);
+ 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 setSlidingConstraintAt(const IndexedLocalCoordinate& coord)
+ {
+ m_localization = std::make_shared<Fem2DLocalization>(m_fem, coord);
+ m_constraintData.setSlidingDirection(m_localization->calculatePosition(0.0), m_slidingDirection);
+ }
+
+ std::shared_ptr<MockFem2DRepresentation> m_fem;
+ std::shared_ptr<Fem2DLocalization> m_localization;
+
+ Vector3d m_slidingDirection;
+ SlidingConstraintData m_constraintData;
+};
+
+TEST_F(Fem2DConstraintFrictionalSlidingTests, ConstructorTest)
+{
+ ASSERT_NO_THROW(
+ {
+ FemConstraintFrictionalSliding femContact;
+ });
+}
+
+TEST_F(Fem2DConstraintFrictionalSlidingTests, ConstraintConstantsTest)
+{
+ auto implementation = std::make_shared<FemConstraintFrictionalSliding>();
+
+ EXPECT_EQ(SurgSim::Physics::FRICTIONAL_SLIDING, implementation->getConstraintType());
+ EXPECT_EQ(3u, implementation->getNumDof());
+}
+
+TEST_F(Fem2DConstraintFrictionalSlidingTests, BuildMlcpTest)
+{
+ // This test verifies the build mlcp function works for a simple case.
+
+ auto implementation = std::make_shared<FemConstraintFrictionalSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 3, 1);
+
+ // Apply constraint purely to the first node of the 0th tetrahedron.
+ IndexedLocalCoordinate coord(0, Vector3d(1.0, 0.0, 0.0));
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ Eigen::Matrix<double, 3, 30> H = Eigen::Matrix<double, 3, 30>::Zero();
+ H.block<1, 3>(0, 0) = (dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 0) = (dt * m_slidingDirection).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 30, 30> C = dt * m_fem->getM().toDense().inverse();
+
+ EXPECT_NEAR_EIGEN(C * H.transpose(), mlcpPhysicsProblem.CHt, epsilon);
+
+ EXPECT_NEAR_EIGEN(H * C * H.transpose(), mlcpPhysicsProblem.A, epsilon);
+
+ EXPECT_DOUBLE_EQ(0.5, mlcpPhysicsProblem.mu[0]);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST_F(Fem2DConstraintFrictionalSlidingTests, BuildMlcpCoordinateTest)
+{
+ // This test verifies the build mlcp function works for a more complicated case with different nodes.
+ auto implementation = std::make_shared<FemConstraintFrictionalSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 3, 1);
+
+ // Apply constraint to all nodes of an fem.
+ const Vector3d barycentric = Vector3d(0.25, 0.33, 0.42);
+ IndexedLocalCoordinate coord(0, barycentric);
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ Eigen::Matrix<double, 3, 30> H = Eigen::Matrix<double, 3, 30>::Zero();
+ H.block<1, 3>(0, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 0) = (barycentric[0] * dt * m_slidingDirection).eval();
+ H.block<1, 3>(0, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 6) = (barycentric[1] * dt * m_slidingDirection).eval();
+ H.block<1, 3>(0, 12) = (barycentric[2] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 12) = (barycentric[2] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 12) = (barycentric[2] * dt * m_slidingDirection).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 30, 30> C = dt * m_fem->getM().toDense().inverse();
+
+ EXPECT_NEAR_EIGEN(C * H.transpose(), mlcpPhysicsProblem.CHt, epsilon);
+
+ EXPECT_NEAR_EIGEN(H * C * H.transpose(), mlcpPhysicsProblem.A, epsilon);
+
+ EXPECT_DOUBLE_EQ(0.5, mlcpPhysicsProblem.mu[0]);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST_F(Fem2DConstraintFrictionalSlidingTests, BuildMlcpIndiciesTest)
+{
+ // This test verifies the build mlcp function works given a hypothetical, preexisting mlcp problem.
+ auto implementation = std::make_shared<FemConstraintFrictionalSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof() + 5, 4, 2);
+
+ // Suppose 5 dof and 1 constraint are defined elsewhere. Then H, CHt, HCHt, and b are prebuilt.
+ Eigen::SparseVector<double, Eigen::RowMajor, ptrdiff_t> localH;
+ localH.resize(5);
+ localH.reserve(5);
+ localH.insert(0) = 0.9478;
+ localH.insert(1) = -0.3807;
+ localH.insert(2) = 0.5536;
+ localH.insert(3) = -0.6944;
+ localH.insert(4) = 0.1815;
+
+ mlcpPhysicsProblem.H.coeffRef(0, 0) += localH.coeff(0);
+ mlcpPhysicsProblem.H.coeffRef(0, 1) += localH.coeff(1);
+ mlcpPhysicsProblem.H.coeffRef(0, 2) += localH.coeff(2);
+ mlcpPhysicsProblem.H.coeffRef(0, 3) += localH.coeff(3);
+ mlcpPhysicsProblem.H.coeffRef(0, 4) += localH.coeff(4);
+
+ 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 the Fem at 5th dof (1 or multiple representations exist before, covering a total of 5 dof)
+ // and the constraint at the index 1 (1 atomic constraint exists before this one)
+ size_t indexOfRepresentation = 5;
+ size_t indexOfConstraint = 1;
+
+ // Apply constraint to all nodes of an fem.
+ Vector3d barycentric = Vector3d(0.25, 0.33, 0.42);
+ IndexedLocalCoordinate coord(0, barycentric);
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, indexOfRepresentation, indexOfConstraint,
+ SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ Eigen::Matrix<double, 3, 30> H = Eigen::Matrix<double, 3, 30>::Zero();
+ H.block<1, 3>(0, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 0) = (barycentric[0] * dt * m_slidingDirection).eval();
+ H.block<1, 3>(0, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 6) = (barycentric[1] * dt * m_slidingDirection).eval();
+ H.block<1, 3>(0, 12) = (barycentric[2] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 12) = (barycentric[2] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 12) = (barycentric[2] * dt * m_slidingDirection).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H.block(indexOfConstraint, indexOfRepresentation, 3, 30), epsilon);
+
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 30, 30> C = dt * m_fem->getM().toDense().inverse();
+
+ EXPECT_NEAR_EIGEN(
+ C * H.transpose(), mlcpPhysicsProblem.CHt.block(indexOfRepresentation, indexOfConstraint, 30, 3), epsilon);
+
+ EXPECT_NEAR_EIGEN(
+ H * C * H.transpose(), mlcpPhysicsProblem.A.block(indexOfConstraint, indexOfConstraint, 3, 3), epsilon);
+
+ EXPECT_DOUBLE_EQ(0.5, mlcpPhysicsProblem.mu[1]);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
diff --git a/SurgSim/Physics/UnitTests/Fem2DConstraintFrictionlessContactTests.cpp b/SurgSim/Physics/UnitTests/Fem2DConstraintFrictionlessContactTests.cpp
new file mode 100644
index 0000000..d274ece
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem2DConstraintFrictionlessContactTests.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 "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/LinearSparseSolveAndInverse.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/Fem2DElementTriangle.h"
+#include "SurgSim/Physics/Fem2DLocalization.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/FemConstraintFrictionlessContact.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+using SurgSim::Framework::Runtime;
+using SurgSim::Physics::ContactConstraintData;
+using SurgSim::Physics::Fem2DRepresentation;
+using SurgSim::Physics::FemConstraintFrictionlessContact;
+using SurgSim::Physics::Fem2DLocalization;
+using SurgSim::Physics::Fem2DElementTriangle;
+using SurgSim::Physics::MlcpPhysicsProblem;
+using SurgSim::Physics::MockFem2DRepresentation;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+const double epsilon = 1e-10;
+const double dt = 1e-3;
+const double mlcpPrecision = 1e-04;
+};
+
+static void addTriangle(Fem2DRepresentation* fem,
+ size_t node0, size_t node1, size_t node2,
+ double thickness = 0.01,
+ double massDensity = 1.0,
+ double poissonRatio = 0.1,
+ double youngModulus = 1.0)
+{
+ std::array<size_t, 3> nodes = {node0, node1, node2};
+ auto element = std::make_shared<Fem2DElementTriangle>(nodes);
+ element->setThickness(thickness);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ fem->addFemElement(element);
+}
+
+class Fem2DConstraintFrictionlessContactTests : 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<MockFem2DRepresentation>("Fem2dRepresentation");
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(6, 5);
+
+ state->getPositions().segment<3>(0 * 6) = Vector3d(0.30, -0.57, 0.40);
+ state->getPositions().segment<3>(1 * 6) = Vector3d(0.06, 0.63, -0.32);
+ state->getPositions().segment<3>(2 * 6) = Vector3d(-0.91, 0.72, 0.72);
+ state->getPositions().segment<3>(3 * 6) = Vector3d(0.35, 0.52, 0.50);
+ state->getPositions().segment<3>(4 * 6) = Vector3d(1.14, 0.66, 0.71);
+
+ addTriangle(m_fem.get(), 0, 1, 2);
+ addTriangle(m_fem.get(), 1, 2, 3);
+ addTriangle(m_fem.get(), 2, 3, 4);
+
+ m_fem->setInitialState(state);
+ m_fem->setIntegrationScheme(SurgSim::Math::IntegrationScheme::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED);
+ 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_localization = std::make_shared<Fem2DLocalization>(m_fem, 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);
+ }
+
+ Vector3d computeNewPosition(const IndexedLocalCoordinate& coord) const
+ {
+ // Solve the system M.a = f
+ // The gravity force should be M.g, but we actually use a mass per node diagonal matrix Md
+ // So the assumption that the violation should be oriented toward the gravity vector is false:
+ // Ma = f = Mg => a = M^-1.f = M^-1.M.g = g
+ // Instead, we have
+ // Ma = f = Md.g => a = M^-1.f = M^-1.Md.g
+ SurgSim::Math::Vector a = m_fem->getM().toDense().inverse() * m_fem->getF();
+ SurgSim::Math::Vector v = m_fem->getInitialState()->getVelocities() + a * dt;
+ SurgSim::Math::Vector p = m_fem->getInitialState()->getPositions() + v * dt;
+ Vector3d newPosition = Vector3d::Zero();
+ const auto& nodeIds = m_fem->getFemElement(coord.index)->getNodeIds();
+
+ for (size_t node = 0; node < 3; node++)
+ {
+ newPosition += p.segment<3>(6 * nodeIds[node]) * coord.coordinate[node];
+ }
+
+ return newPosition;
+ }
+
+ std::shared_ptr<MockFem2DRepresentation> m_fem;
+ std::shared_ptr<Fem2DLocalization> m_localization;
+
+ Vector3d m_n;
+ ContactConstraintData m_constraintData;
+};
+
+TEST_F(Fem2DConstraintFrictionlessContactTests, ConstructorTest)
+{
+ ASSERT_NO_THROW(
+ {
+ FemConstraintFrictionlessContact femContact;
+ });
+}
+
+TEST_F(Fem2DConstraintFrictionlessContactTests, ConstraintConstantsTest)
+{
+ auto implementation = std::make_shared<FemConstraintFrictionlessContact>();
+
+ EXPECT_EQ(SurgSim::Physics::FRICTIONLESS_3DCONTACT, implementation->getConstraintType());
+ EXPECT_EQ(1u, implementation->getNumDof());
+}
+
+TEST_F(Fem2DConstraintFrictionlessContactTests, BuildMlcpTest)
+{
+ // This test verifies the build mlcp function works for a simple case.
+
+ auto implementation = std::make_shared<FemConstraintFrictionlessContact>();
+
+ // 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, Vector3d(1.0, 0.0, 0.0));
+ setContactAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ const Vector3d newPosition = computeNewPosition(coord);
+ EXPECT_NEAR(newPosition.dot(m_n), mlcpPhysicsProblem.b[0] + mlcpPrecision, epsilon);
+
+ Eigen::Matrix<double, 1, 30> H = Eigen::Matrix<double, 1, 30>::Zero();
+ H.segment<3>(6 * 0) = dt * m_n;
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 30, 30> C = dt * m_fem->getM().toDense().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(Fem2DConstraintFrictionlessContactTests, BuildMlcpCoordinateTest)
+{
+ // This test verifies the build mlcp function works for a more complicated case with different nodes.
+
+ auto implementation = std::make_shared<FemConstraintFrictionlessContact>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 1, 1);
+
+ // Apply constraint to all nodes of an fem.
+ const Vector3d barycentric = Vector3d(0.25, 0.33, 0.42);
+ IndexedLocalCoordinate coord(1, barycentric);
+ setContactAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ const Vector3d newPosition = computeNewPosition(coord);
+ EXPECT_NEAR(newPosition.dot(m_n), mlcpPhysicsProblem.b[0] + mlcpPrecision, epsilon);
+
+ Eigen::Matrix<double, 1, 30> H = Eigen::Matrix<double, 1, 30>::Zero();
+ H.segment<3>(6 * 1) = 0.25 * dt * m_n;
+ H.segment<3>(6 * 2) = 0.33 * dt * m_n;
+ H.segment<3>(6 * 3) = 0.42 * dt * m_n;
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ SurgSim::Math::Matrix C;
+ SurgSim::Math::SparseMatrix M(30, 30);
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ M = m_fem->getM();
+ SurgSim::Math::LinearSparseSolveAndInverseLU solver;
+ SurgSim::Math::Vector b = SurgSim::Math::Vector::Zero(30);
+ solver.setMatrix(M);
+ C = solver.getInverse();
+ C *= dt;
+
+ 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(Fem2DConstraintFrictionlessContactTests, BuildMlcpIndiciesTest)
+{
+ // This test verifies the build mlcp function works given a hypothetical, preexisting mlcp problem.
+
+ auto implementation = std::make_shared<FemConstraintFrictionlessContact>();
+
+ // 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::SparseVector<double, Eigen::RowMajor, ptrdiff_t> localH;
+ localH.resize(5);
+ localH.reserve(5);
+ localH.insert(0) = 0.9478;
+ localH.insert(1) = -0.3807;
+ localH.insert(2) = 0.5536;
+ localH.insert(3) = -0.6944;
+ localH.insert(4) = 0.1815;
+
+ mlcpPhysicsProblem.H.coeffRef(0, 0) += localH.coeff(0);
+ mlcpPhysicsProblem.H.coeffRef(0, 1) += localH.coeff(1);
+ mlcpPhysicsProblem.H.coeffRef(0, 2) += localH.coeff(2);
+ mlcpPhysicsProblem.H.coeffRef(0, 3) += localH.coeff(3);
+ mlcpPhysicsProblem.H.coeffRef(0, 4) += localH.coeff(4);
+
+ 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 the Fem at 5th dof (1 or multiple representations exist before, covering a total of 5 dof)
+ // and the constraint at the index 1 (1 atomic constraint exists before this one)
+ size_t indexOfRepresentation = 5;
+ size_t indexOfConstraint = 1;
+
+ // Apply constraint to all nodes of an fem.
+ Vector3d barycentric = Vector3d(0.25, 0.33, 0.42);
+ IndexedLocalCoordinate coord(1, barycentric);
+ setContactAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, indexOfRepresentation, indexOfConstraint,
+ SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ const Vector3d newPosition = computeNewPosition(coord);
+ EXPECT_NEAR(newPosition.dot(m_n), mlcpPhysicsProblem.b[indexOfConstraint] + mlcpPrecision, epsilon);
+
+ Eigen::Matrix<double, 1, 30> H = Eigen::Matrix<double, 1, 30>::Zero();
+ H.segment<3>(6 * 1) = 0.25 * dt * m_n;
+ H.segment<3>(6 * 2) = 0.33 * dt * m_n;
+ H.segment<3>(6 * 3) = 0.42 * dt * m_n;
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H.block(indexOfConstraint, indexOfRepresentation, 1, 30), epsilon);
+
+ SurgSim::Math::Matrix C;
+ SurgSim::Math::SparseMatrix M(30, 30);
+ m_fem->updateFMDK(*m_fem->getPreviousState(), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ M = m_fem->getM();
+ SurgSim::Math::LinearSparseSolveAndInverseLU solver;
+ SurgSim::Math::Vector b = SurgSim::Math::Vector::Zero(30);
+ solver.setMatrix(M);
+ C = solver.getInverse();
+ C *= dt;
+
+ EXPECT_NEAR_EIGEN(
+ C * H.transpose(), mlcpPhysicsProblem.CHt.block(indexOfRepresentation, indexOfConstraint, 30, 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/Fem2DConstraintFrictionlessSlidingTests.cpp b/SurgSim/Physics/UnitTests/Fem2DConstraintFrictionlessSlidingTests.cpp
new file mode 100644
index 0000000..0179a16
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem2DConstraintFrictionlessSlidingTests.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.
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/LinearSparseSolveAndInverse.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem2DElementTriangle.h"
+#include "SurgSim/Physics/Fem2DLocalization.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/FemConstraintFrictionlessSliding.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/SlidingConstraintData.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+using SurgSim::Framework::Runtime;
+using SurgSim::Physics::Fem2DRepresentation;
+using SurgSim::Physics::FemConstraintFrictionlessSliding;
+using SurgSim::Physics::Fem2DLocalization;
+using SurgSim::Physics::Fem2DElementTriangle;
+using SurgSim::Physics::MlcpPhysicsProblem;
+using SurgSim::Physics::MockFem2DRepresentation;
+using SurgSim::Physics::SlidingConstraintData;
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+ const double epsilon = 1e-10;
+ const double dt = 1e-3;
+};
+
+static void addTriangle(Fem2DRepresentation* fem,
+ size_t node0, size_t node1, size_t node2,
+ double thickness = 0.01,
+ double massDensity = 1.0,
+ double poissonRatio = 0.1,
+ double youngModulus = 1.0)
+{
+ std::array<size_t, 3> nodes = {node0, node1, node2};
+ auto element = std::make_shared<Fem2DElementTriangle>(nodes);
+ element->setThickness(thickness);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ fem->addFemElement(element);
+}
+
+class Fem2DConstraintFrictionlessSlidingTests : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ // Define plane with normal 'n' pointing against gravity.
+ m_slidingDirection = Vector3d(0.8539, 0.6289, -0.9978);
+ m_slidingDirection.normalize();
+
+ // Create mock FEM
+ m_fem = std::make_shared<MockFem2DRepresentation>("Fem2dRepresentation");
+ auto state = std::make_shared<SurgSim::Math::OdeState>();
+ state->setNumDof(6, 5);
+
+ state->getPositions().segment<3>(0 * 6) = Vector3d(0.30, -0.57, 0.40);
+ state->getPositions().segment<3>(1 * 6) = Vector3d(0.06, 0.63, -0.32);
+ state->getPositions().segment<3>(2 * 6) = Vector3d(-0.91, 0.72, 0.72);
+ state->getPositions().segment<3>(3 * 6) = Vector3d(0.35, 0.52, 0.50);
+ state->getPositions().segment<3>(4 * 6) = Vector3d(1.14, 0.66, 0.71);
+
+ addTriangle(m_fem.get(), 0, 1, 2);
+ addTriangle(m_fem.get(), 1, 2, 3);
+ addTriangle(m_fem.get(), 2, 3, 4);
+
+ m_fem->setInitialState(state);
+ m_fem->setIntegrationScheme(SurgSim::Math::IntegrationScheme::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED);
+ 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 setSlidingConstraintAt(const IndexedLocalCoordinate& coord)
+ {
+ m_localization = std::make_shared<Fem2DLocalization>(m_fem, coord);
+ m_constraintData.setSlidingDirection(m_localization->calculatePosition(0.0), m_slidingDirection);
+ }
+
+ std::shared_ptr<MockFem2DRepresentation> m_fem;
+ std::shared_ptr<Fem2DLocalization> m_localization;
+
+ Vector3d m_slidingDirection;
+ SlidingConstraintData m_constraintData;
+};
+
+TEST_F(Fem2DConstraintFrictionlessSlidingTests, ConstructorTest)
+{
+ ASSERT_NO_THROW(
+ {
+ FemConstraintFrictionlessSliding femContact;
+ });
+}
+
+TEST_F(Fem2DConstraintFrictionlessSlidingTests, ConstraintConstantsTest)
+{
+ auto implementation = std::make_shared<FemConstraintFrictionlessSliding>();
+
+ EXPECT_EQ(SurgSim::Physics::FRICTIONLESS_SLIDING, implementation->getConstraintType());
+ EXPECT_EQ(2u, implementation->getNumDof());
+}
+
+TEST_F(Fem2DConstraintFrictionlessSlidingTests, BuildMlcpTest)
+{
+ // This test verifies the build mlcp function works for a simple case.
+
+ auto implementation = std::make_shared<FemConstraintFrictionlessSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 2, 1);
+
+ // Apply constraint purely to the first node of the 0th tetrahedron.
+ IndexedLocalCoordinate coord(0, Vector3d(1.0, 0.0, 0.0));
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ auto pose = m_constraintData.getPose().inverse().rotation();
+ EXPECT_TRUE((pose * m_slidingDirection).isApprox(Vector3d(1.0, 0.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[0]).isApprox(Vector3d(0.0, 1.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[1]).isApprox(Vector3d(0.0, 0.0, 1.0)));
+
+ Eigen::Matrix<double, 2, 30> H = Eigen::Matrix<double, 2, 30>::Zero();
+ H.block<1, 3>(0, 0) = (dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (dt * m_constraintData.getNormals()[1]).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 30, 30> C = dt * m_fem->getM().toDense().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(Fem2DConstraintFrictionlessSlidingTests, BuildMlcpCoordinateTest)
+{
+ // This test verifies the build mlcp function works for a more complicated case with different nodes.
+ auto implementation = std::make_shared<FemConstraintFrictionlessSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 2, 1);
+
+ // Apply constraint to all nodes of an fem.
+ const Vector3d barycentric = Vector3d(0.25, 0.33, 0.42);
+ IndexedLocalCoordinate coord(0, barycentric);
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ auto pose = m_constraintData.getPose().inverse().rotation();
+ EXPECT_TRUE((pose * m_slidingDirection).isApprox(Vector3d(1.0, 0.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[0]).isApprox(Vector3d(0.0, 1.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[1]).isApprox(Vector3d(0.0, 0.0, 1.0)));
+
+ Eigen::Matrix<double, 2, 30> H = Eigen::Matrix<double, 2, 30>::Zero();
+ H.block<1, 3>(0, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(0, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(0, 12) = (barycentric[2] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 12) = (barycentric[2] * dt * m_constraintData.getNormals()[1]).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 30, 30> C = dt * m_fem->getM().toDense().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(Fem2DConstraintFrictionlessSlidingTests, BuildMlcpIndiciesTest)
+{
+ // This test verifies the build mlcp function works given a hypothetical, preexisting mlcp problem.
+ auto implementation = std::make_shared<FemConstraintFrictionlessSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof() + 5, 3, 2);
+
+ // Suppose 5 dof and 1 constraint are defined elsewhere. Then H, CHt, HCHt, and b are prebuilt.
+ Eigen::SparseVector<double, Eigen::RowMajor, ptrdiff_t> localH;
+ localH.resize(5);
+ localH.reserve(5);
+ localH.insert(0) = 0.9478;
+ localH.insert(1) = -0.3807;
+ localH.insert(2) = 0.5536;
+ localH.insert(3) = -0.6944;
+ localH.insert(4) = 0.1815;
+
+ mlcpPhysicsProblem.H.coeffRef(0, 0) += localH.coeff(0);
+ mlcpPhysicsProblem.H.coeffRef(0, 1) += localH.coeff(1);
+ mlcpPhysicsProblem.H.coeffRef(0, 2) += localH.coeff(2);
+ mlcpPhysicsProblem.H.coeffRef(0, 3) += localH.coeff(3);
+ mlcpPhysicsProblem.H.coeffRef(0, 4) += localH.coeff(4);
+
+ 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 the Fem at 5th dof (1 or multiple representations exist before, covering a total of 5 dof)
+ // and the constraint at the index 1 (1 atomic constraint exists before this one)
+ size_t indexOfRepresentation = 5;
+ size_t indexOfConstraint = 1;
+
+ // Apply constraint to all nodes of an fem.
+ Vector3d barycentric = Vector3d(0.25, 0.33, 0.42);
+ IndexedLocalCoordinate coord(0, barycentric);
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, indexOfRepresentation, indexOfConstraint,
+ SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ auto pose = m_constraintData.getPose().inverse().rotation();
+ EXPECT_TRUE((pose * m_slidingDirection).isApprox(Vector3d(1.0, 0.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[0]).isApprox(Vector3d(0.0, 1.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[1]).isApprox(Vector3d(0.0, 0.0, 1.0)));
+
+ Eigen::Matrix<double, 2, 30> H = Eigen::Matrix<double, 2, 30>::Zero();
+ H.block<1, 3>(0, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(0, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 6) = (barycentric[1] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(0, 12) = (barycentric[2] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 12) = (barycentric[2] * dt * m_constraintData.getNormals()[1]).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H.block(indexOfConstraint, indexOfRepresentation, 2, 30), epsilon);
+
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 30, 30> C = dt * m_fem->getM().toDense().inverse();
+
+ EXPECT_NEAR_EIGEN(
+ C * H.transpose(), mlcpPhysicsProblem.CHt.block(indexOfRepresentation, indexOfConstraint, 30, 2), epsilon);
+
+ EXPECT_NEAR_EIGEN(
+ H * C * H.transpose(), mlcpPhysicsProblem.A.block(indexOfConstraint, indexOfConstraint, 2, 2), epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
diff --git a/SurgSim/Physics/UnitTests/Fem2DElementTriangleTests.cpp b/SurgSim/Physics/UnitTests/Fem2DElementTriangleTests.cpp
index 1581b8f..66274bd 100644
--- a/SurgSim/Physics/UnitTests/Fem2DElementTriangleTests.cpp
+++ b/SurgSim/Physics/UnitTests/Fem2DElementTriangleTests.cpp
@@ -21,14 +21,19 @@
#include "SurgSim/Framework/Assert.h"
#include "SurgSim/Math/GaussLegendreQuadrature.h"
#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeEquation.h"
#include "SurgSim/Math/OdeState.h"
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/Vector.h"
#include "SurgSim/Physics/Fem2DElementTriangle.h"
+using SurgSim::Math::gaussQuadrature2DTriangle6Points;
+using SurgSim::Math::gaussQuadrature2DTriangle12Points;
+using SurgSim::Math::clearMatrix;
using SurgSim::Math::Matrix;
using SurgSim::Math::Matrix33d;
using SurgSim::Math::Quaterniond;
+using SurgSim::Math::SparseMatrix;
using SurgSim::Math::Vector;
using SurgSim::Math::Vector3d;
using SurgSim::Physics::Fem2DElementTriangle;
@@ -41,7 +46,7 @@ const double epsilon = 1e-8;
class MockFem2DElement : public Fem2DElementTriangle
{
public:
- MockFem2DElement(std::array<size_t, 3> nodeIds)
+ explicit MockFem2DElement(std::array<size_t, 3> nodeIds)
: Fem2DElementTriangle(nodeIds)
{
}
@@ -67,9 +72,19 @@ public:
return m_initialRotationTimes6;
}
- const Eigen::Matrix<double, 3, 9> getBatozStrainDisplacement(double xi, double neta) const
+ const double get_xij(size_t i) const
{
- return batozStrainDisplacement(xi, neta);
+ return m_xij[i];
+ }
+
+ const double get_yij(size_t i) const
+ {
+ return m_yij[i];
+ }
+
+ const Eigen::Matrix<double, 3, 9> getBatozStrainDisplacement(double xi, double eta) const
+ {
+ return batozStrainDisplacement(xi, eta);
}
const Eigen::Matrix<double, 18, 18>& getLocalStiffnessMatrix() const
@@ -77,7 +92,7 @@ public:
return m_KLocal;
}
- const Eigen::Matrix<double, 18, 18>& getGlobalStiffnessMatrix() const
+ const SurgSim::Math::Matrix& getGlobalStiffnessMatrix() const
{
return m_K;
}
@@ -87,7 +102,7 @@ public:
return m_MLocal;
}
- const Eigen::Matrix<double, 18, 18>& getGlobalMassMatrix() const
+ const SurgSim::Math::Matrix& getGlobalMassMatrix() const
{
return m_M;
}
@@ -106,7 +121,7 @@ public:
/// \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
+ void getMembraneShapeFunction(int i, double* ai, double* bi, double* ci) const
{
*ai = m_membraneShapeFunctionsParameters(i, 0);
*bi = m_membraneShapeFunctionsParameters(i, 1);
@@ -114,330 +129,390 @@ public:
}
// 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)
+ // N1(xi, eta) = 2(1-xi-eta)(0.5-xi-eta)
+ double batozN1(double xi, double eta) const
+ {
+ return 2.0 * (1.0 - xi - eta) * (0.5 - xi - eta);
+ }
+ // N2(xi, eta) = xi(2 xi-1)
+ double batozN2(double xi, double eta) const
+ {
+ return xi * (2.0 * xi - 1.0);
+ }
+ // N3(xi, eta) = eta(2 eta-1)
+ double batozN3(double xi, double eta) const
+ {
+ return eta * (2.0 * eta - 1.0);
+ }
+ // N4(xi, eta) = 4 xi eta
+ double batozN4(double xi, double eta) const
+ {
+ return 4.0 * xi * eta;
+ }
+ // N5(xi, eta) = 4 eta(1-xi-eta)
+ double batozN5(double xi, double eta) const
+ {
+ return 4.0 * eta * (1.0 - xi - eta);
+ }
+ // N6(xi, eta) = 4 xi(1-xi-eta)
+ double batozN6(double xi, double eta) const
+ {
+ return 4.0 * xi * (1.0 - xi - eta);
+ }
+
+ // dN1/dxi(xi, eta) = 2[(-0.5+xi+eta) + (-1+xi+eta)] = 2(-3/2+2(xi+eta)) = 4(xi+eta) - 3
+ double batozDN1Dxi(double xi, double eta) const
+ {
+ return 4.0 * (xi + eta) - 3.0;
+ }
+ // dN2/dxi(xi, eta) = (2xi-1) + 2xi = 4xi-1
+ double batozDN2Dxi(double xi, double eta) const
+ {
+ return 4.0 * xi - 1.0;
+ }
+ // dN3/dxi(xi, eta) = 0
+ double batozDN3Dxi(double xi, double eta) const
+ {
+ return 0.0;
+ }
+ // dN4/dxi(xi, eta) = 4 eta
+ double batozDN4Dxi(double xi, double eta) const
+ {
+ return 4.0 * eta;
+ }
+ // dN5/dxi(xi,eta) = -4 eta
+ double batozDN5Dxi(double xi, double eta) const
+ {
+ return -4.0 * eta;
+ }
+ // dN6/dxi(xi,eta) = 4(1-xi-eta) -4xi = 4(1-2xi-eta)
+ double batozDN6Dxi(double xi, double eta) const
+ {
+ return 4.0 * (1.0 - 2.0 * xi - eta);
+ }
+
+ // dN1/deta(xi, eta) = 2[(-0.5+xi+eta) + (-1+xi+eta)] = 2(-3/2 + 2xi + 2eta) = 4(xi+eta) - 3
+ double batozDN1Deta(double xi, double eta) const
+ {
+ return 4.0 * (xi + eta) - 3.0;
+ }
+ // dN2/deta(xi, eta) = 0
+ double batozDN2Deta(double xi, double eta) const
+ {
+ return 0.0;
+ }
+ // dN3/deta(xi, eta) = 2eta-1 + 2eta = 4eta-1
+ double batozDN3Deta(double xi, double eta) const
+ {
+ return 4.0 * eta - 1.0;
+ }
+ // dN4/deta(xi, eta) = 4xi
+ double batozDN4Deta(double xi, double eta) const
+ {
+ return 4.0 * xi;
+ }
+ // dN5/deta(xi, eta) = 4[(1-xi-eta) - eta] = 4(1-xi-2eta)
+ double batozDN5Deta(double xi, double eta) const
+ {
+ return 4.0 * (1.0 - xi - 2.0 * eta);
+ }
+ // dN6/deta(xi, eta) = -4xi
+ double batozDN6Deta(double xi, double eta) const
+ {
+ return -4.0 * xi;
+ }
+
+ std::array<double, 9> batozHx(double xi, double eta) const
{
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));
+ res[0] = 1.5 * (m_ak[2] * batozN6(xi, eta) - m_ak[1] * batozN5(xi, eta));
// b5N5+b6N6
- res[1] = m_bk[1] * batozN5(xi, neta) + m_bk[2] * batozN6(xi, neta);
+ res[1] = m_bk[1] * batozN5(xi, eta) + m_bk[2] * batozN6(xi, eta);
// N1 - c5N5 - c6N6
- res[2] = batozN1(xi, neta) - m_ck[1] * batozN5(xi, neta) - m_ck[2] * batozN6(xi, neta);
+ res[2] = batozN1(xi, eta) - m_ck[1] * batozN5(xi, eta) - m_ck[2] * batozN6(xi, eta);
// 1.5 (a4N4-a6N6)
- res[3] = 1.5 * (m_ak[0] * batozN4(xi, neta) - m_ak[2] * batozN6(xi, neta));
+ res[3] = 1.5 * (m_ak[0] * batozN4(xi, eta) - m_ak[2] * batozN6(xi, eta));
// b6N6+b4N4
- res[4] = m_bk[2] * batozN6(xi, neta) + m_bk[0] * batozN4(xi, neta);
+ res[4] = m_bk[2] * batozN6(xi, eta) + m_bk[0] * batozN4(xi, eta);
// N2 - c6N6 - c4N4
- res[5] = batozN2(xi, neta) - m_ck[2] * batozN6(xi, neta) - m_ck[0] * batozN4(xi, neta);
+ res[5] = batozN2(xi, eta) - m_ck[2] * batozN6(xi, eta) - m_ck[0] * batozN4(xi, eta);
// 1.5 (a5N5-a4N4)
- res[6] = 1.5 * (m_ak[1] * batozN5(xi, neta) - m_ak[0] * batozN4(xi, neta));
+ res[6] = 1.5 * (m_ak[1] * batozN5(xi, eta) - m_ak[0] * batozN4(xi, eta));
// b4N4+b5N5
- res[7] = m_bk[0] * batozN4(xi, neta) + m_bk[1] * batozN5(xi, neta);
+ res[7] = m_bk[0] * batozN4(xi, eta) + m_bk[1] * batozN5(xi, eta);
// N3 - c4N4 - c5N5
- res[8] = batozN3(xi, neta) - m_ck[0] * batozN4(xi, neta) - m_ck[1] * batozN5(xi, neta);
+ res[8] = batozN3(xi, eta) - m_ck[0] * batozN4(xi, eta) - m_ck[1] * batozN5(xi, eta);
return res;
}
- std::array<double, 9> batozDhxDxiAlternative(double xi, double neta)
+ std::array<double, 9> batozDhxDxiAlternative(double xi, double eta) const
{
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));
+ res[0] = 1.5 * (m_ak[2] * batozDN6Dxi(xi, eta) - m_ak[1] * batozDN5Dxi(xi, eta));
// b5dN5+b6dN6
- res[1] = m_bk[1] * batozDN5Dxi(xi, neta) + m_bk[2] * batozDN6Dxi(xi, neta);
+ res[1] = m_bk[1] * batozDN5Dxi(xi, eta) + m_bk[2] * batozDN6Dxi(xi, eta);
// dN1 - c5dN5 - c6dN6
- res[2] = batozDN1Dxi(xi, neta) - m_ck[1] * batozDN5Dxi(xi, neta) - m_ck[2] * batozDN6Dxi(xi, neta);
+ res[2] = batozDN1Dxi(xi, eta) - m_ck[1] * batozDN5Dxi(xi, eta) - m_ck[2] * batozDN6Dxi(xi, eta);
// 1.5 (a4dN4-a6dN6)
- res[3] = 1.5 * (m_ak[0] * batozDN4Dxi(xi, neta) - m_ak[2] * batozDN6Dxi(xi, neta));
+ res[3] = 1.5 * (m_ak[0] * batozDN4Dxi(xi, eta) - m_ak[2] * batozDN6Dxi(xi, eta));
// b6dN6+b4dN4
- res[4] = m_bk[2] * batozDN6Dxi(xi, neta) + m_bk[0] * batozDN4Dxi(xi, neta);
+ res[4] = m_bk[2] * batozDN6Dxi(xi, eta) + m_bk[0] * batozDN4Dxi(xi, eta);
// dN2 - c6dN6 - c4dN4
- res[5] = batozDN2Dxi(xi, neta) - m_ck[2] * batozDN6Dxi(xi, neta) - m_ck[0] * batozDN4Dxi(xi, neta);
+ res[5] = batozDN2Dxi(xi, eta) - m_ck[2] * batozDN6Dxi(xi, eta) - m_ck[0] * batozDN4Dxi(xi, eta);
// 1.5 (a5dN5-a4dN4)
- res[6] = 1.5 * (m_ak[1] * batozDN5Dxi(xi, neta) - m_ak[0] * batozDN4Dxi(xi, neta));
+ res[6] = 1.5 * (m_ak[1] * batozDN5Dxi(xi, eta) - m_ak[0] * batozDN4Dxi(xi, eta));
// b4dN4+b5dN5
- res[7] = m_bk[0] * batozDN4Dxi(xi, neta) + m_bk[1] * batozDN5Dxi(xi, neta);
+ res[7] = m_bk[0] * batozDN4Dxi(xi, eta) + m_bk[1] * batozDN5Dxi(xi, eta);
// dN3 - c4dN4 - c5dN5
- res[8] = batozDN3Dxi(xi, neta) - m_ck[0] * batozDN4Dxi(xi, neta) - m_ck[1] * batozDN5Dxi(xi, neta);
+ res[8] = batozDN3Dxi(xi, eta) - m_ck[0] * batozDN4Dxi(xi, eta) - m_ck[1] * batozDN5Dxi(xi, eta);
return res;
}
- std::array<double, 9> batozDhxDnetaAlternative(double xi, double neta)
+ std::array<double, 9> batozDhxDetaAlternative(double xi, double eta) const
{
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));
+ res[0] = 1.5 * (m_ak[2] * batozDN6Deta(xi, eta) - m_ak[1] * batozDN5Deta(xi, eta));
// b5dN5+b6dN6
- res[1] = m_bk[1] * batozDN5Dneta(xi, neta) + m_bk[2] * batozDN6Dneta(xi, neta);
+ res[1] = m_bk[1] * batozDN5Deta(xi, eta) + m_bk[2] * batozDN6Deta(xi, eta);
// dN1 - c5dN5 - c6dN6
- res[2] = batozDN1Dneta(xi, neta) - m_ck[1] * batozDN5Dneta(xi, neta) - m_ck[2] * batozDN6Dneta(xi, neta);
+ res[2] = batozDN1Deta(xi, eta) - m_ck[1] * batozDN5Deta(xi, eta) - m_ck[2] * batozDN6Deta(xi, eta);
// 1.5 (a4dN4-a6dN6)
- res[3] = 1.5 * (m_ak[0] * batozDN4Dneta(xi, neta) - m_ak[2] * batozDN6Dneta(xi, neta));
+ res[3] = 1.5 * (m_ak[0] * batozDN4Deta(xi, eta) - m_ak[2] * batozDN6Deta(xi, eta));
// b6dN6+b4dN4
- res[4] = m_bk[2] * batozDN6Dneta(xi, neta) + m_bk[0] * batozDN4Dneta(xi, neta);
+ res[4] = m_bk[2] * batozDN6Deta(xi, eta) + m_bk[0] * batozDN4Deta(xi, eta);
// dN2 - c6dN6 - c4dN4
- res[5] = batozDN2Dneta(xi, neta) - m_ck[2] * batozDN6Dneta(xi, neta) - m_ck[0] * batozDN4Dneta(xi, neta);
+ res[5] = batozDN2Deta(xi, eta) - m_ck[2] * batozDN6Deta(xi, eta) - m_ck[0] * batozDN4Deta(xi, eta);
// 1.5 (a5dN5-a4dN4)
- res[6] = 1.5 * (m_ak[1] * batozDN5Dneta(xi, neta) - m_ak[0] * batozDN4Dneta(xi, neta));
+ res[6] = 1.5 * (m_ak[1] * batozDN5Deta(xi, eta) - m_ak[0] * batozDN4Deta(xi, eta));
// b4dN4+b5dN5
- res[7] = m_bk[0] * batozDN4Dneta(xi, neta) + m_bk[1] * batozDN5Dneta(xi, neta);
+ res[7] = m_bk[0] * batozDN4Deta(xi, eta) + m_bk[1] * batozDN5Deta(xi, eta);
// dN3 - c4dN4 - c5dN5
- res[8] = batozDN3Dneta(xi, neta) - m_ck[0] * batozDN4Dneta(xi, neta) - m_ck[1] * batozDN5Dneta(xi, neta);
+ res[8] = batozDN3Deta(xi, eta) - m_ck[0] * batozDN4Deta(xi, eta) - m_ck[1] * batozDN5Deta(xi, eta);
return res;
}
- std::array<double, 9> batozHy(double xi, double neta)
+ std::array<double, 9> batozHy(double xi, double eta) const
{
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));
+ res[0] = 1.5 * (m_dk[2] * batozN6(xi, eta) - m_dk[1] * batozN5(xi, eta));
// -N1 + e5N5 + e6N6
- res[1] = -batozN1(xi, neta) + m_ek[1] * batozN5(xi, neta) + m_ek[2] * batozN6(xi, neta);
+ res[1] = -batozN1(xi, eta) + m_ek[1] * batozN5(xi, eta) + m_ek[2] * batozN6(xi, eta);
// -b5N5-b6N6
- res[2] = -m_bk[1] * batozN5(xi, neta) - m_bk[2] * batozN6(xi, neta);
+ res[2] = -m_bk[1] * batozN5(xi, eta) - m_bk[2] * batozN6(xi, eta);
// 1.5 (d4N4-d6N6)
- res[3] = 1.5 * (m_dk[0] * batozN4(xi, neta) - m_dk[2] * batozN6(xi, neta));
+ res[3] = 1.5 * (m_dk[0] * batozN4(xi, eta) - m_dk[2] * batozN6(xi, eta));
// -N2 + e6N6 + e4N4
- res[4] = -batozN2(xi, neta) + m_ek[2] * batozN6(xi, neta) + m_ek[0] * batozN4(xi, neta);
+ res[4] = -batozN2(xi, eta) + m_ek[2] * batozN6(xi, eta) + m_ek[0] * batozN4(xi, eta);
// -b6N6-b4N4
- res[5] = -m_bk[2] * batozN6(xi, neta) - m_bk[0] * batozN4(xi, neta);
+ res[5] = -m_bk[2] * batozN6(xi, eta) - m_bk[0] * batozN4(xi, eta);
// 1.5 (d5N5-d4N4)
- res[6] = 1.5 * (m_dk[1] * batozN5(xi, neta) - m_dk[0] * batozN4(xi, neta));
+ res[6] = 1.5 * (m_dk[1] * batozN5(xi, eta) - m_dk[0] * batozN4(xi, eta));
// -N3 + e4N4 + e5N5
- res[7] = -batozN3(xi, neta) + m_ek[0] * batozN4(xi, neta) + m_ek[1] * batozN5(xi, neta);
+ res[7] = -batozN3(xi, eta) + m_ek[0] * batozN4(xi, eta) + m_ek[1] * batozN5(xi, eta);
// -b4N4-b5N5
- res[8] = -m_bk[0] * batozN4(xi, neta) - m_bk[1] * batozN5(xi, neta);
+ res[8] = -m_bk[0] * batozN4(xi, eta) - m_bk[1] * batozN5(xi, eta);
return res;
}
- std::array<double, 9> batozDhyDxiAlternative(double xi, double neta)
+ std::array<double, 9> batozDhyDxiAlternative(double xi, double eta) const
{
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));
+ res[0] = 1.5 * (m_dk[2] * batozDN6Dxi(xi, eta) - m_dk[1] * batozDN5Dxi(xi, eta));
// -dN1 + e5dN5 + e6dN6
- res[1] = -batozDN1Dxi(xi, neta) + m_ek[1] * batozDN5Dxi(xi, neta) + m_ek[2] * batozDN6Dxi(xi, neta);
+ res[1] = -batozDN1Dxi(xi, eta) + m_ek[1] * batozDN5Dxi(xi, eta) + m_ek[2] * batozDN6Dxi(xi, eta);
// -b5dN5-b6dN6
- res[2] = -m_bk[1] * batozDN5Dxi(xi, neta) - m_bk[2] * batozDN6Dxi(xi, neta);
+ res[2] = -m_bk[1] * batozDN5Dxi(xi, eta) - m_bk[2] * batozDN6Dxi(xi, eta);
// 1.5 (d4dN4-d6dN6)
- res[3] = 1.5 * (m_dk[0] * batozDN4Dxi(xi, neta) - m_dk[2] * batozDN6Dxi(xi, neta));
+ res[3] = 1.5 * (m_dk[0] * batozDN4Dxi(xi, eta) - m_dk[2] * batozDN6Dxi(xi, eta));
// -dN2 + e6dN6 + e4dN4
- res[4] = -batozDN2Dxi(xi, neta) + m_ek[2] * batozDN6Dxi(xi, neta) + m_ek[0] * batozDN4Dxi(xi, neta);
+ res[4] = -batozDN2Dxi(xi, eta) + m_ek[2] * batozDN6Dxi(xi, eta) + m_ek[0] * batozDN4Dxi(xi, eta);
// -b6dN6-b4dN4
- res[5] = -m_bk[2] * batozDN6Dxi(xi, neta) - m_bk[0] * batozDN4Dxi(xi, neta);
+ res[5] = -m_bk[2] * batozDN6Dxi(xi, eta) - m_bk[0] * batozDN4Dxi(xi, eta);
// 1.5 (d5dN5-d4dN4)
- res[6] = 1.5 * (m_dk[1] * batozDN5Dxi(xi, neta) - m_dk[0] * batozDN4Dxi(xi, neta));
+ res[6] = 1.5 * (m_dk[1] * batozDN5Dxi(xi, eta) - m_dk[0] * batozDN4Dxi(xi, eta));
// -dN3 + e4dN4 + e5dN5
- res[7] = -batozDN3Dxi(xi, neta) + m_ek[0] * batozDN4Dxi(xi, neta) + m_ek[1] * batozDN5Dxi(xi, neta);
+ res[7] = -batozDN3Dxi(xi, eta) + m_ek[0] * batozDN4Dxi(xi, eta) + m_ek[1] * batozDN5Dxi(xi, eta);
// -b4dN4-b5dN5
- res[8] = -m_bk[0] * batozDN4Dxi(xi, neta) - m_bk[1] * batozDN5Dxi(xi, neta);
+ res[8] = -m_bk[0] * batozDN4Dxi(xi, eta) - m_bk[1] * batozDN5Dxi(xi, eta);
return res;
}
- std::array<double, 9> batozDhyDnetaAlternative(double xi, double neta)
+ std::array<double, 9> batozDhyDetaAlternative(double xi, double eta) const
{
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));
+ res[0] = 1.5 * (m_dk[2] * batozDN6Deta(xi, eta) - m_dk[1] * batozDN5Deta(xi, eta));
// -dN1 + e5dN5 + e6dN6
- res[1] = -batozDN1Dneta(xi, neta) + m_ek[1] * batozDN5Dneta(xi, neta) + m_ek[2] * batozDN6Dneta(xi, neta);
+ res[1] = -batozDN1Deta(xi, eta) + m_ek[1] * batozDN5Deta(xi, eta) + m_ek[2] * batozDN6Deta(xi, eta);
// -b5dN5-b6dN6
- res[2] = -m_bk[1] * batozDN5Dneta(xi, neta) - m_bk[2] * batozDN6Dneta(xi, neta);
+ res[2] = -m_bk[1] * batozDN5Deta(xi, eta) - m_bk[2] * batozDN6Deta(xi, eta);
// 1.5 (d4dN4-d6dN6)
- res[3] = 1.5 * (m_dk[0] * batozDN4Dneta(xi, neta) - m_dk[2] * batozDN6Dneta(xi, neta));
+ res[3] = 1.5 * (m_dk[0] * batozDN4Deta(xi, eta) - m_dk[2] * batozDN6Deta(xi, eta));
// -dN2 + e6dN6 + e4dN4
- res[4] = -batozDN2Dneta(xi, neta) + m_ek[2] * batozDN6Dneta(xi, neta) + m_ek[0] * batozDN4Dneta(xi, neta);
+ res[4] = -batozDN2Deta(xi, eta) + m_ek[2] * batozDN6Deta(xi, eta) + m_ek[0] * batozDN4Deta(xi, eta);
// -b6dN6-b4dN4
- res[5] = -m_bk[2] * batozDN6Dneta(xi, neta) - m_bk[0] * batozDN4Dneta(xi, neta);
+ res[5] = -m_bk[2] * batozDN6Deta(xi, eta) - m_bk[0] * batozDN4Deta(xi, eta);
// -dN3 + e4dN4 + e5dN5
- res[6] = 1.5 * (m_dk[1] * batozDN5Dneta(xi, neta) - m_dk[0] * batozDN4Dneta(xi, neta));
+ res[6] = 1.5 * (m_dk[1] * batozDN5Deta(xi, eta) - m_dk[0] * batozDN4Deta(xi, eta));
// -dN3 + e4dN4 + e5dN5
- res[7] = -batozDN3Dneta(xi, neta) + m_ek[0] * batozDN4Dneta(xi, neta) + m_ek[1] * batozDN5Dneta(xi, neta);
+ res[7] = -batozDN3Deta(xi, eta) + m_ek[0] * batozDN4Deta(xi, eta) + m_ek[1] * batozDN5Deta(xi, eta);
// -b4dN4-b5dN5
- res[8] = -m_bk[0] * batozDN4Dneta(xi, neta) - m_bk[1] * batozDN5Dneta(xi, neta);
+ res[8] = -m_bk[0] * batozDN4Deta(xi, eta) - m_bk[1] * batozDN5Deta(xi, eta);
return res;
}
- Eigen::Matrix<double, 3, 9> batozStrainDisplacementAlternativeDerivative(double xi, double neta)
+ Eigen::Matrix<double, 3, 9> batozStrainDisplacementAlternativeDerivative(double xi, double eta) const
{
Eigen::Matrix<double, 3, 9> res;
- std::array<double, 9> dHx_dxi, dHx_dneta, dHy_dxi, dHy_dneta;
+ std::array<double, 9> dHx_dxi, dHx_deta, dHy_dxi, dHy_deta;
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);
+ dHx_dxi = batozDhxDxiAlternative(xi, eta);
+ dHx_deta = batozDhxDetaAlternative(xi, eta);
+ dHy_dxi = batozDhyDxiAlternative(xi, eta);
+ dHy_deta = batozDhyDetaAlternative(xi, eta);
- for(size_t i = 0; i < 9; ++i)
+ 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(0, i) = coefficient * (m_yij[1] * dHx_dxi[i] + m_yij[2] * dHx_deta[i]);
+ res(1, i) = coefficient * (-m_xij[1] * dHy_dxi[i] - m_xij[2] * dHy_deta[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]);
+ (-m_xij[1] * dHx_dxi[i] - m_xij[2] * dHx_deta[i] +
+ m_yij[1] * dHy_dxi[i] + m_yij[2] * dHy_deta[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> batozFx(double xi, double eta,
+ 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) 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));
+ res[0] = 1.5 * (m_ak[2] * (this->*f6)(xi, eta) - m_ak[1] * (this->*f5)(xi, eta));
// b5N5+b6N6
- res[1] = m_bk[1] * (this->*f5)(xi, neta) + m_bk[2] * (this->*f6)(xi, neta);
+ res[1] = m_bk[1] * (this->*f5)(xi, eta) + m_bk[2] * (this->*f6)(xi, eta);
// N1 - c5N5 - c6N6
- res[2] = (this->*f1)(xi, neta) - m_ck[1] * (this->*f5)(xi, neta) - m_ck[2] * (this->*f6)(xi, neta);
+ res[2] = (this->*f1)(xi, eta) - m_ck[1] * (this->*f5)(xi, eta) - m_ck[2] * (this->*f6)(xi, eta);
// 1.5 (a4N4-a6N6)
- res[3] = 1.5 * (m_ak[0] * (this->*f4)(xi, neta) - m_ak[2] * (this->*f6)(xi, neta));
+ res[3] = 1.5 * (m_ak[0] * (this->*f4)(xi, eta) - m_ak[2] * (this->*f6)(xi, eta));
// b6N6+b4N4
- res[4] = m_bk[2] * (this->*f6)(xi, neta) + m_bk[0] * (this->*f4)(xi, neta);
+ res[4] = m_bk[2] * (this->*f6)(xi, eta) + m_bk[0] * (this->*f4)(xi, eta);
// N2 - c6N6 - c4N4
- res[5] = (this->*f2)(xi, neta) - m_ck[2] * (this->*f6)(xi, neta) - m_ck[0] * (this->*f4)(xi, neta);
+ res[5] = (this->*f2)(xi, eta) - m_ck[2] * (this->*f6)(xi, eta) - m_ck[0] * (this->*f4)(xi, eta);
// 1.5 (a5N5-a4N4)
- res[6] = 1.5 * (m_ak[1] * (this->*f5)(xi, neta) - m_ak[0] * (this->*f4)(xi, neta));
+ res[6] = 1.5 * (m_ak[1] * (this->*f5)(xi, eta) - m_ak[0] * (this->*f4)(xi, eta));
// b4N4+b5N5
- res[7] = m_bk[0] * (this->*f4)(xi, neta) + m_bk[1] * (this->*f5)(xi, neta);
+ res[7] = m_bk[0] * (this->*f4)(xi, eta) + m_bk[1] * (this->*f5)(xi, eta);
// N3 - c4N4 - c5N5
- res[8] = (this->*f3)(xi, neta) - m_ck[0] * (this->*f4)(xi, neta) - m_ck[1] * (this->*f5)(xi, neta);
+ res[8] = (this->*f3)(xi, eta) - m_ck[0] * (this->*f4)(xi, eta) - m_ck[1] * (this->*f5)(xi, eta);
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> batozFy(double xi, double eta,
+ 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) 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));
+ res[0] = 1.5 * (m_dk[2] * (this->*f6)(xi, eta) - m_dk[1] * (this->*f5)(xi, eta));
// -N1 + e5N5 + e6N6
- res[1] = -(this->*f1)(xi, neta) + m_ek[1] * (this->*f5)(xi, neta) + m_ek[2] * (this->*f6)(xi, neta);
+ res[1] = -(this->*f1)(xi, eta) + m_ek[1] * (this->*f5)(xi, eta) + m_ek[2] * (this->*f6)(xi, eta);
// -b5N5-b6N6
- res[2] = -m_bk[1] * (this->*f5)(xi, neta) - m_bk[2] * (this->*f6)(xi, neta);
+ res[2] = -m_bk[1] * (this->*f5)(xi, eta) - m_bk[2] * (this->*f6)(xi, eta);
// 1.5 (d4N4-d6N6)
- res[3] = 1.5 * (m_dk[0] * (this->*f4)(xi, neta) - m_dk[2] * (this->*f6)(xi, neta));
+ res[3] = 1.5 * (m_dk[0] * (this->*f4)(xi, eta) - m_dk[2] * (this->*f6)(xi, eta));
// -N2 + e6N6 + e4N4
- res[4] = -(this->*f2)(xi, neta) + m_ek[2] * (this->*f6)(xi, neta) + m_ek[0] * (this->*f4)(xi, neta);
+ res[4] = -(this->*f2)(xi, eta) + m_ek[2] * (this->*f6)(xi, eta) + m_ek[0] * (this->*f4)(xi, eta);
// -b6N6-b4N4
- res[5] = -m_bk[2] * (this->*f6)(xi, neta) - m_bk[0] * (this->*f4)(xi, neta);
+ res[5] = -m_bk[2] * (this->*f6)(xi, eta) - m_bk[0] * (this->*f4)(xi, eta);
// 1.5 (d5N5-d4N4)
- res[6] = 1.5 * (m_dk[1] * (this->*f5)(xi, neta) - m_dk[0] * (this->*f4)(xi, neta));
+ res[6] = 1.5 * (m_dk[1] * (this->*f5)(xi, eta) - m_dk[0] * (this->*f4)(xi, eta));
// -N3 + e4N4 + e5N5
- res[7] = -(this->*f3)(xi, neta) + m_ek[0] * (this->*f4)(xi, neta) + m_ek[1] * (this->*f5)(xi, neta);
+ res[7] = -(this->*f3)(xi, eta) + m_ek[0] * (this->*f4)(xi, eta) + m_ek[1] * (this->*f5)(xi, eta);
// -b4N4-b5N5
- res[8] = -m_bk[0] * (this->*f4)(xi, neta) - m_bk[1] * (this->*f5)(xi, neta);
+ res[8] = -m_bk[0] * (this->*f4)(xi, eta) - m_bk[1] * (this->*f5)(xi, eta);
return res;
}
- Eigen::Matrix<double, 3, 9> batozStrainDisplacementNumericalDerivation(double xi, double neta)
+ Eigen::Matrix<double, 3, 9> batozStrainDisplacementNumericalDerivation(double xi, double eta) const
{
Eigen::Matrix<double, 3, 9> res;
- std::array<double, 9> dHx_dxi, dHx_dneta, dHy_dxi, dHy_dneta;
+ std::array<double, 9> dHx_dxi, dHx_deta, dHy_dxi, dHy_deta;
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)
+ dHx_dxi = batozFx(xi, eta, &MockFem2DElement::batozDN1Dxi , &MockFem2DElement::batozDN2Dxi ,
+ &MockFem2DElement::batozDN3Dxi , &MockFem2DElement::batozDN4Dxi ,
+ &MockFem2DElement::batozDN5Dxi ,
+ &MockFem2DElement::batozDN6Dxi);
+ dHx_deta = batozFx(xi, eta, &MockFem2DElement::batozDN1Deta, &MockFem2DElement::batozDN2Deta,
+ &MockFem2DElement::batozDN3Deta, &MockFem2DElement::batozDN4Deta,
+ &MockFem2DElement::batozDN5Deta,
+ &MockFem2DElement::batozDN6Deta);
+ dHy_dxi = batozFy(xi, eta, &MockFem2DElement::batozDN1Dxi, &MockFem2DElement::batozDN2Dxi ,
+ &MockFem2DElement::batozDN3Dxi, &MockFem2DElement::batozDN4Dxi,
+ &MockFem2DElement::batozDN5Dxi,
+ &MockFem2DElement::batozDN6Dxi);
+ dHy_deta = batozFy(xi, eta, &MockFem2DElement::batozDN1Deta, &MockFem2DElement::batozDN2Deta,
+ &MockFem2DElement::batozDN3Deta, &MockFem2DElement::batozDN4Deta,
+ &MockFem2DElement::batozDN5Deta,
+ &MockFem2DElement::batozDN6Deta);
+
+ 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(0, i) = coefficient * (m_yij[1] * dHx_dxi[i] + m_yij[2] * dHx_deta[i]);
+ res(1, i) = coefficient * (-m_xij[1] * dHy_dxi[i] - m_xij[2] * dHy_deta[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]);
+ (-m_xij[1] * dHx_dxi[i] - m_xij[2] * dHx_deta[i] + m_yij[1] * dHy_dxi[i] +
+ m_yij[2] * dHy_deta[i]);
}
return res;
@@ -461,7 +536,7 @@ public:
Quaterniond m_rotation, m_expectedRotation;
Eigen::Matrix<double, 18, 1> m_expectedX0;
- virtual void SetUp() override
+ void SetUp() override
{
using SurgSim::Math::getSubVector;
@@ -500,20 +575,128 @@ public:
m_expectedRotation = m_rotation;
}
- void getExpectedLocalMassMatrix(Eigen::Ref<SurgSim::Math::Matrix> mass)
+ // Useful method to numerically evaluate the 9x9 matrix d^T.d on a given point on the triangle
+ Matrix evaluate_dTd_at(const MockFem2DElement& fem2DElement, double xi, double eta)
{
- // This is hard-coded for density(rho)=1000 A=0.5 thickness=1e-2
- mass.setIdentity();
- for (size_t nodeId = 0; nodeId < 3; ++nodeId)
+ SurgSim::Math::Vector d(9); // column vector
+
+ const double xi2 = xi * xi;
+ const double xi3 = xi2 * xi;
+ const double eta2 = eta * eta;
+ const double eta3 = eta2 * eta;
+ const double lambda = 1.0 - xi - eta;
+ const double lambda2 = lambda * lambda;
+ const double lambda3 = lambda2 * lambda;
+ const double xiEtaLambda = xi * eta * lambda;
+
+ const double N1 = 3.0 * lambda2 - 2.0 * lambda3 + 2.0 * xiEtaLambda;
+ const double N2 = lambda2 * xi + xiEtaLambda / 2.0;
+ const double N3 = lambda2 * eta + xiEtaLambda / 2.0;
+ const double N4 = 3.0 * xi2 - 2.0 * xi3 + 2.0 * xiEtaLambda;
+ const double N5 = xi2 * (xi - 1.0) - xiEtaLambda;
+ const double N6 = xi2 * eta + xiEtaLambda / 2.0;
+ const double N7 = 3.0 * eta2 - 2.0 * eta3 + 2.0 * xiEtaLambda;
+ const double N8 = eta2 * xi + xiEtaLambda / 2.0;
+ const double N9 = eta2 * (eta - 1.0) - xiEtaLambda;
+
+ // x0 = y0 = y1 = 0.0
+ const double x1 = -fem2DElement.get_xij(2); // x0 - x1 = -x1
+ const double x2 = fem2DElement.get_xij(1); // x2 - x0 = x2
+ const double y2 = fem2DElement.get_yij(1); // y2 - y0 = y2
+
+ d << N1, N3* y2, -N2* x1 - N3* x2, N4, N6* y2, -N5* x1 - N6* x2, N7, N9* y2, -N8* x1 - N9* x2;
+
+ return d * d.transpose();
+ }
+
+ // Useful method to numerically evaluate the 9x9 matrix Hx.Hx^T on a given point on the triangle
+ Matrix evaluate_HxHxT_at(const MockFem2DElement& fem2DElement, double xi, double eta)
+ {
+ auto Hx_array = fem2DElement.batozHx(xi, eta);
+ SurgSim::Math::Vector Hx(9); // column vector
+ Hx << Hx_array[0], Hx_array[1], Hx_array[2],
+ Hx_array[3], Hx_array[4], Hx_array[5],
+ Hx_array[6], Hx_array[7], Hx_array[8];
+ return Hx * Hx.transpose();
+ }
+
+ // Useful method to numerically evaluate the 9x9 matrix Hy.Hy^T on a given point on the triangle
+ Matrix evaluate_HyHyT_at(const MockFem2DElement& fem2DElement, double xi, double eta)
+ {
+ auto Hy_array = fem2DElement.batozHy(xi, eta);
+ SurgSim::Math::Vector Hy(9); // column vector
+ Hy << Hy_array[0], Hy_array[1], Hy_array[2],
+ Hy_array[3], Hy_array[4], Hy_array[5],
+ Hy_array[6], Hy_array[7], Hy_array[8];
+ return Hy * Hy.transpose();
+ }
+
+ // Useful method to numerically evaluate the plate mass matrix of an element
+ // This method uses a Gauss quadrature rules on the triangle to numerically evaluate the vaious integral terms.
+ void numericallyEvaluatePlateMassMatrix(const MockFem2DElement& fem2DElement,
+ Eigen::Ref<Matrix> mass)
+ {
+ // M = 2.A.rho.h \int_0^1 \int_0^{1-eta} d^T.d dxi deta
+ // + 2.A.h^3/12.rho \int_0^1 \int_0^{1-eta} Hx.Hx^T dxi deta
+ // + 2.A.h^3/12.rho \int_0^1 \int_0^{1-eta} Hy.Hy^T dxi deta
+ const double A = fem2DElement.getRestArea();
+ const double rho = fem2DElement.getMassDensity();
+ const double h = fem2DElement.getThickness();
+ const double coefUz = 2.0 * A * rho * h;
+ const double coefUtheta = coefUz * h * h / 12.0;
+
+ // http://math2.uncc.edu/~shaodeng/TEACHING/math5172/Lectures/Lect_15.PDF
+ // "Quadrature Formulas in Two Dimensions"
+ // \int_0^1 \int_0^{1-eta} f(xi, eta) dxi deta = 1/2 sum_i w[i] f(xi[i], eta[i])
+ const double half = 1.0 / 2.0;
+
+ // Note that matrix d contains monomial terms up to degree 3,
+ // therefore dT.d contains monomial terms up to degree 6.
+ // Exact integration of such functions over the triangle requires a Gauss-Legendre quadrature with 12 points:
+ for (size_t pointId = 0; pointId < 12; ++pointId)
{
- 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);
+ const double& weight = gaussQuadrature2DTriangle12Points[pointId].weight;
+ const double& xi = gaussQuadrature2DTriangle12Points[pointId].coordinateXi;
+ const double& eta = gaussQuadrature2DTriangle12Points[pointId].coordinateEta;
+ mass += coefUz * (half * weight * evaluate_dTd_at(fem2DElement, xi, eta));
+ }
+
+ // Note that Hx and Hy are of degree 2, therefore Hx.Hx^T and Hy.Hy^T are of degree 4.
+ // Exact integration of such functions over the triangle requires a Gauss-Legendre quadrature with 6 points:
+ for (size_t pointId = 0; pointId < 6; ++pointId)
+ {
+ const double& weight = gaussQuadrature2DTriangle6Points[pointId].weight;
+ const double& xi = gaussQuadrature2DTriangle6Points[pointId].coordinateXi;
+ const double& eta = gaussQuadrature2DTriangle6Points[pointId].coordinateEta;
+ mass += coefUtheta * (half * weight * evaluate_HxHxT_at(fem2DElement, xi, eta));
+ mass += coefUtheta * (half * weight * evaluate_HyHyT_at(fem2DElement, xi, eta));
}
}
- void getExpectedLocalStiffnessMatrix(Eigen::Ref<SurgSim::Math::Matrix> stiffness)
+ void getExpectedLocalMassMatrix(Eigen::Ref<Matrix> mass)
+ {
+ typedef Eigen::Matrix<double, 9, 9> Matrix99Type;
+ typedef Eigen::Matrix<double, 6, 6> Matrix66Type;
+
+ Matrix66Type membraneMass = getMembraneLocalMassMatrix();
+ Matrix99Type plateMass = getPlateLocalMassMatrix();
+
+ // Assemble the membrane and plane stiffness
+ mass.setIdentity(); // The drilling dof will have an independent dof of mass 1kg.
+ for (size_t row = 0; row < 3; ++row)
+ {
+ for (size_t column = 0; column < 3; ++column)
+ {
+ // Membrane part
+ mass.block<2, 2>(6 * row, 6 * column) = membraneMass.block<2, 2>(2 * row, 2 * column);
+
+ // Thin-plate part
+ mass.block<3, 3>(6 * row + 2, 6 * column + 2) = plateMass.block<3, 3>(3 * row, 3 * column);
+ }
+ }
+ }
+
+ void getExpectedLocalStiffnessMatrix(Eigen::Ref<Matrix> stiffness)
{
typedef Eigen::Matrix<double, 9, 9> Matrix99Type;
typedef Eigen::Matrix<double, 6, 6> Matrix66Type;
@@ -523,15 +706,15 @@ public:
// Assemble the membrane and plane stiffness
stiffness.setIdentity();
- for(size_t row = 0; row < 3; ++row)
+ for (size_t row = 0; row < 3; ++row)
{
- for(size_t column = 0; column < 3; ++column)
+ 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);
+ stiffness.block<2, 2>(6 * row, 6 * column) = membraneStiffness.block<2, 2>(2 * row, 2 * column);
// Thin-plate part
- stiffness.block(6 * row + 2, 6 * column + 2, 3, 3) = plateStiffness.block(3 * row, 3 * column, 3, 3);
+ stiffness.block<3, 3>(6 * row + 2, 6 * column + 2) = plateStiffness.block<3, 3>(3 * row, 3 * column);
}
}
}
@@ -564,9 +747,9 @@ public:
// 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));
+ 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];
@@ -617,6 +800,40 @@ public:
return stiffness;
}
+ Eigen::Matrix<double, 6, 6> getMembraneLocalMassMatrix()
+ {
+ typedef Eigen::Matrix<double, 6, 6> Matrix66Type;
+
+ Matrix66Type membraneMassMatrix = Matrix66Type::Zero();
+ double m = m_rho * m_thickness * m_A;
+
+ membraneMassMatrix.block<2, 2>(0, 0).diagonal().setConstant(m / 6.0);
+ membraneMassMatrix.block<2, 2>(0, 2).diagonal().setConstant(m / 12.0);
+ membraneMassMatrix.block<2, 2>(0, 4).diagonal().setConstant(m / 12.0);
+
+ membraneMassMatrix.block<2, 2>(2, 0).diagonal().setConstant(m / 12.0);
+ membraneMassMatrix.block<2, 2>(2, 2).diagonal().setConstant(m / 6.0);
+ membraneMassMatrix.block<2, 2>(2, 4).diagonal().setConstant(m / 12.0);
+
+ membraneMassMatrix.block<2, 2>(4, 0).diagonal().setConstant(m / 12.0);
+ membraneMassMatrix.block<2, 2>(4, 2).diagonal().setConstant(m / 12.0);
+ membraneMassMatrix.block<2, 2>(4, 4).diagonal().setConstant(m / 6.0);
+
+ return membraneMassMatrix;
+ }
+
+ Eigen::Matrix<double, 9, 9> getPlateLocalMassMatrix()
+ {
+ typedef Eigen::Matrix<double, 9, 9> Matrix99Type;
+
+ Matrix99Type plateMassMatrix = Matrix99Type::Zero();
+
+ std::shared_ptr<MockFem2DElement> element = getElement();
+ numericallyEvaluatePlateMassMatrix(*element, plateMassMatrix);
+
+ return plateMassMatrix;
+ }
+
std::shared_ptr<MockFem2DElement> getElement()
{
auto element = std::make_shared<MockFem2DElement>(m_nodeIds);
@@ -736,15 +953,15 @@ TEST_F(Fem2DElementTriangleTests, CoordinateTests)
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);
+ SurgSim::Framework::AssertionFailure);
EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateNegativeValue), \
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSize2), \
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSize4), \
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSumNot1), \
- SurgSim::Framework::AssertionFailure);
+ 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));
@@ -807,36 +1024,36 @@ TEST_F(Fem2DElementTriangleTests, StrainDisplacementPlateAtGaussPointTest)
// Validate the alternative technique against the numerical evaluation
EXPECT_TRUE(strainDisplacementExpected1[0].isApprox(strainDisplacementExpected2[0])) <<
- strainDisplacementExpected1[0] << std::endl <<
- strainDisplacementExpected2[0] << std::endl;
+ strainDisplacementExpected1[0] << std::endl <<
+ strainDisplacementExpected2[0] << std::endl;
EXPECT_TRUE(strainDisplacementExpected1[1].isApprox(strainDisplacementExpected2[1])) <<
- strainDisplacementExpected1[1] << std::endl <<
- strainDisplacementExpected2[1] << std::endl;
+ strainDisplacementExpected1[1] << std::endl <<
+ strainDisplacementExpected2[1] << std::endl;
EXPECT_TRUE(strainDisplacementExpected1[2].isApprox(strainDisplacementExpected2[2])) <<
- strainDisplacementExpected1[2] << std::endl <<
- strainDisplacementExpected2[2] << std::endl;
+ 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;
+ strainDisplacement[0] << std::endl <<
+ strainDisplacementExpected1[0] << std::endl;
EXPECT_TRUE(strainDisplacement[0].isApprox(strainDisplacementExpected2[0])) <<
- strainDisplacement[0] << std::endl <<
- strainDisplacementExpected2[0] << std::endl;
+ strainDisplacement[0] << std::endl <<
+ strainDisplacementExpected2[0] << std::endl;
EXPECT_TRUE(strainDisplacement[1].isApprox(strainDisplacementExpected1[1])) <<
- strainDisplacement[1] << std::endl <<
- strainDisplacementExpected1[1] << std::endl;
+ strainDisplacement[1] << std::endl <<
+ strainDisplacementExpected1[1] << std::endl;
EXPECT_TRUE(strainDisplacement[1].isApprox(strainDisplacementExpected2[1])) <<
- strainDisplacement[1] << std::endl <<
- strainDisplacementExpected2[1] << std::endl;
+ strainDisplacement[1] << std::endl <<
+ strainDisplacementExpected2[1] << std::endl;
EXPECT_TRUE(strainDisplacement[2].isApprox(strainDisplacementExpected1[2])) <<
- strainDisplacement[2] << std::endl <<
- strainDisplacementExpected1[2] << std::endl;
+ strainDisplacement[2] << std::endl <<
+ strainDisplacementExpected1[2] << std::endl;
EXPECT_TRUE(strainDisplacement[2].isApprox(strainDisplacementExpected2[2])) <<
- strainDisplacement[2] << std::endl <<
- strainDisplacementExpected2[2] << std::endl;
+ strainDisplacement[2] << std::endl <<
+ strainDisplacementExpected2[2] << std::endl;
}
namespace
@@ -847,8 +1064,8 @@ namespace
/// \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)
+ 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];
}
@@ -861,7 +1078,8 @@ TEST_F(Fem2DElementTriangleTests, MembraneShapeFunctionsTest)
std::shared_ptr<MockFem2DElement> tri = getElement();
EXPECT_TRUE(tri->getInitialPosition().isApprox(m_expectedX0)) <<
- "x0 = " << tri->getInitialPosition().transpose() << std::endl << "x0 expected = " << m_expectedX0.transpose();
+ "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;
@@ -902,9 +1120,9 @@ TEST_F(Fem2DElementTriangleTests, MembraneShapeFunctionsTest)
// 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 sp0p1 = 0; sp0p1 <= 1.0; sp0p1 += 0.1)
{
- for (double sp0p2 = 0; sp0p1 + sp0p2 <= 1.0; sp0p2+=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);
@@ -914,8 +1132,8 @@ TEST_F(Fem2DElementTriangleTests, MembraneShapeFunctionsTest)
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];
+ " 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];
}
}
}
@@ -972,23 +1190,23 @@ TEST_F(Fem2DElementTriangleTests, PlateShapeFunctionsTest)
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
+ // We should have the relation sum(Ni(xi, eta) = 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)
+ for (double eta = 0.0; xi + eta <= 1.0; eta += 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);
+ EXPECT_DOUBLE_EQ(1.0, tri->batozN1(xi, eta) + tri->batozN2(xi, eta) + tri->batozN3(xi, eta) + \
+ tri->batozN4(xi, eta) + tri->batozN5(xi, eta) + tri->batozN6(xi, eta)) <<
+ "For (xi = " << xi << ", eta = " << eta << "), " << std::endl <<
+ " N1 = " << tri->batozN1(xi, eta) << std::endl <<
+ " N2 = " << tri->batozN2(xi, eta) << std::endl <<
+ " N3 = " << tri->batozN3(xi, eta) << std::endl <<
+ " N4 = " << tri->batozN4(xi, eta) << std::endl <<
+ " N5 = " << tri->batozN5(xi, eta) << std::endl <<
+ " N6 = " << tri->batozN6(xi, eta) << std::endl <<
+ " N1+N2+N3+N4+N5+N6 = " <<
+ tri->batozN1(xi, eta) + tri->batozN2(xi, eta) + tri->batozN3(xi, eta) +
+ tri->batozN4(xi, eta) + tri->batozN5(xi, eta) + tri->batozN6(xi, eta);
}
}
}
@@ -997,38 +1215,115 @@ TEST_F(Fem2DElementTriangleTests, StiffnessMatrixTest)
{
std::shared_ptr<MockFem2DElement> tri = getElement();
- Eigen::Matrix<double, 18 ,18> expectedLocalStiffness;
+ 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;
+ "KLocal = " << std::endl << tri->getLocalStiffnessMatrix() << std::endl <<
+ "KLocal expected = " << std::endl << expectedLocalStiffness << std::endl;
- Eigen::Matrix<double, 18 ,18> R0 = tri->getInitialRotationTimes6();
+ 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;
+ "R0 = " << std::endl << R0 << std::endl <<
+ "KGlobal = " << std::endl << tri->getLocalStiffnessMatrix() << std::endl <<
+ "KGlobal expected = " << std::endl << expectedLocalStiffness << std::endl;
+}
+
+/// Evaluate a given polynomial at the given coordinates (x, y)
+/// \param degree The degree of the polynomial
+/// \param coefficients The vector of coefficients for all monomials in order
+/// [1, x, y,..., x^n, x^{n-1}y^1, ..., x^1y^{n-1}, x^0y^n]
+/// \param x,y The coordinates to evaluate the polynomial at
+/// \return the polynomial evaluation at the coordinates (x, y)
+static double evaluatePolynomial(size_t degree, const SurgSim::Math::Vector& coefficients, double x, double y)
+{
+ SURGSIM_ASSERT((degree + 1) * (degree + 2) / 2 == static_cast<size_t>(coefficients.size())) <<
+ "Invalid coefficients vector (of size " << coefficients.size() <<
+ ") provided for a polynomial of degree " << degree <<
+ ". Was expecting " << (degree + 1) * (degree + 2) / 2 << " coefficients";
+
+ SurgSim::Math::Vector monomials = SurgSim::Math::Vector::Zero(coefficients.size());
+
+ size_t monomialId = 0;
+ for (size_t d = 0; d <= degree; d++)
+ {
+ for (size_t monomialOfDegreed = 0; monomialOfDegreed <= d; monomialOfDegreed++)
+ {
+ monomials[monomialId] =
+ coefficients[monomialId] * pow(x, d - monomialOfDegreed) * pow(y, monomialOfDegreed);
+ monomialId++;
+ }
+ }
+
+ return monomials.sum();
+}
+
+TEST_F(Fem2DElementTriangleTests, TriangleIntegrationPolynomialOrder4Test)
+{
+ // Polynomial of order 4 on 2 variables:
+ // {1, 2x, 3y, 4x^2, 5xy, 6y^2, 7x^3, 8x^2y, 9xy^2, 10y^3, 11x^4, 12x^3y, 13x^2y^2, 14xy^3, 15y^4}
+ // \int_0^1 \int_0^{1-y} P(x, y) dx dy = 1679 / 360
+ SurgSim::Math::Vector polynomial(15);
+ polynomial.setLinSpaced(1.0, 15.0);
+
+ // http://math2.uncc.edu/~shaodeng/TEACHING/math5172/Lectures/Lect_15.PDF
+ // "Quadrature Formulas in Two Dimensions"
+ // \int_0^1 \int_0^{1-eta} f(xi, eta) dxi deta = 1/2 sum_i w[i] f(xi[i], eta[i])
+ const double half = 1.0 / 2.0;
+
+ // Note that Hx and Hy are of degree 2, therefore Hx.Hx^T and Hy.Hy^T are of degree 4.
+ // Exact integration of such functions over the triangle requires a Gauss-Legendre quadrature with 6 points:
+ double integral = 0.0;
+ for (size_t pointId = 0; pointId < 6; ++pointId)
+ {
+ const double& weight = gaussQuadrature2DTriangle6Points[pointId].weight;
+ const double& xi = gaussQuadrature2DTriangle6Points[pointId].coordinateXi;
+ const double& eta = gaussQuadrature2DTriangle6Points[pointId].coordinateEta;
+ integral += half * weight * evaluatePolynomial(4, polynomial, xi, eta);
+ }
+
+ EXPECT_NEAR(1679.0 / 360.0, integral, 1e-8);
+}
+
+TEST_F(Fem2DElementTriangleTests, TriangleIntegrationPolynomialOrder6Test)
+{
+ // Polynomial of order 6 on 2 variables:
+ // {1, 2x, 3y, 4x^2, 5xy, 6y^2, 7x^3, 8x^2y, 9xy^2, 10y^3, 11x^4, 12x^3y, 13x^2y^2, 14xy^3, 15y^4,
+ // 16x^5, 17x^4y, 18x^3y^2, 19x^2y^3, 20xy^4, 21y^5,
+ // 22x^6, 23x^5y, 24x^4y^2, 25x^3y^3, 26x^2y^4, 27xy^5, 28y^6}
+ // \int_0^1 \int_0^{1-y} P(x, y) dx dy = 9983 / 1440
+ SurgSim::Math::Vector polynomial(28);
+ polynomial.setLinSpaced(1.0, 28.0);
+
+ // http://math2.uncc.edu/~shaodeng/TEACHING/math5172/Lectures/Lect_15.PDF
+ // "Quadrature Formulas in Two Dimensions"
+ // \int_0^1 \int_0^{1-eta} f(xi, eta) dxi deta = 1/2 sum_i w[i] f(xi[i], eta[i])
+ const double half = 1.0 / 2.0;
+
+ // Note that matrix d contains monomial terms up to degree 3,
+ // therefore dT.d contains monomial terms up to degree 6.
+ // Exact integration of such functions over the triangle requires a Gauss-Legendre quadrature with 12 points:
+ double integral = 0.0;
+ for (size_t pointId = 0; pointId < 12; ++pointId)
+ {
+ const double& weight = gaussQuadrature2DTriangle12Points[pointId].weight;
+ const double& xi = gaussQuadrature2DTriangle12Points[pointId].coordinateXi;
+ const double& eta = gaussQuadrature2DTriangle12Points[pointId].coordinateEta;
+ integral += half * weight * evaluatePolynomial(6, polynomial, xi, eta);
+ }
+
+ EXPECT_NEAR(9983.0 / 1440.0, integral, 1e-6);
}
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->getLocalMassMatrix().isApprox(expectedMassMatrix)) <<
+ "Error = " << std::endl << tri->getLocalMassMatrix() - expectedMassMatrix << std::endl;
+
+ Eigen::Matrix<double, 18 , 18> R0 = tri->getInitialRotationTimes6();
EXPECT_TRUE(tri->getGlobalMassMatrix().isApprox(R0 * expectedMassMatrix * R0.transpose()));
}
@@ -1038,18 +1333,20 @@ TEST_F(Fem2DElementTriangleTests, ForceAndMatricesAPITest)
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);
+ const SparseMatrix::Index numDof = 6 * static_cast<SparseMatrix::Index>(m_restState.getNumNodes());
+ Vector forceVector(numDof);
+ Vector ones(numDof);
+ SparseMatrix massMatrix(numDof, numDof);
+ SparseMatrix dampingMatrix(numDof, numDof);
+ SparseMatrix stiffnessMatrix(numDof, numDof);
+ SparseMatrix zeroMatrix(numDof, numDof);
+ Matrix expectedMassMatrix(numDof, numDof);
+ Matrix expectedDampingMatrix(numDof, numDof);
+ Matrix expectedStiffnessMatrix(numDof, numDof);
+ Matrix zeros18x18 = SurgSim::Math::Matrix::Zero(18, 18);
// Assemble manually the expectedStiffnessMatrix
- Eigen::Matrix<double, 18 ,18> R0 = tri->getInitialRotationTimes6();
+ Eigen::Matrix<double, 18 , 18> R0 = tri->getInitialRotationTimes6();
Eigen::Matrix<double, 18, 18> expected18x18StiffnessMatrix;
getExpectedLocalStiffnessMatrix(expected18x18StiffnessMatrix);
expectedStiffnessMatrix.setZero();
@@ -1063,66 +1360,78 @@ TEST_F(Fem2DElementTriangleTests, ForceAndMatricesAPITest)
forceVector.setZero();
massMatrix.setZero();
+ tri->assembleMatrixBlocks(zeros18x18, tri->getNodeIds(), 6, &massMatrix, true);
+ massMatrix.makeCompressed();
dampingMatrix.setZero();
+ tri->assembleMatrixBlocks(zeros18x18, tri->getNodeIds(), 6, &dampingMatrix, true);
+ dampingMatrix.makeCompressed();
stiffnessMatrix.setZero();
+ tri->assembleMatrixBlocks(zeros18x18, tri->getNodeIds(), 6, &stiffnessMatrix, true);
+ stiffnessMatrix.makeCompressed();
+ zeroMatrix.setZero();
+
+ // Update the internal f, M, D, K variables.
+ tri->updateFMDK(m_restState, SurgSim::Math::ODEEQUATIONUPDATE_FMDK);
// No force should be produced when in rest state (x = x0) => F = K.(x-x0) = 0
- tri->addForce(m_restState, &forceVector);
+ tri->addForce(&forceVector);
EXPECT_TRUE(forceVector.isZero());
- tri->addMass(m_restState, &massMatrix);
- EXPECT_TRUE(massMatrix.isApprox(expectedMassMatrix));
+ tri->addMass(&massMatrix);
+ EXPECT_TRUE(massMatrix.isApprox(expectedMassMatrix)) << "MassMatrix = " << std::endl << massMatrix << std::endl <<
+ "ExpectedMassMatrix = " << std::endl << expectedMassMatrix << std::endl;
- tri->addDamping(m_restState, &dampingMatrix);
- EXPECT_TRUE(dampingMatrix.isZero());
+ tri->addDamping(&dampingMatrix);
+ EXPECT_TRUE(dampingMatrix.isApprox(zeroMatrix));
- tri->addStiffness(m_restState, &stiffnessMatrix);
+ tri->addStiffness(&stiffnessMatrix);
EXPECT_TRUE(stiffnessMatrix.isApprox(expectedStiffnessMatrix));
forceVector.setZero();
- massMatrix.setZero();
- dampingMatrix.setZero();
- stiffnessMatrix.setZero();
+ clearMatrix(&massMatrix);
+ clearMatrix(&dampingMatrix);
+ clearMatrix(&stiffnessMatrix);
- tri->addFMDK(m_restState, &forceVector, &massMatrix, &dampingMatrix, &stiffnessMatrix);
+ tri->addFMDK(&forceVector, &massMatrix, &dampingMatrix, &stiffnessMatrix);
EXPECT_TRUE(forceVector.isZero());
- EXPECT_TRUE(massMatrix.isApprox(expectedMassMatrix));
- EXPECT_TRUE(dampingMatrix.isZero());
+ EXPECT_TRUE(massMatrix.isApprox(expectedMassMatrix)) << "MassMatrix = " << std::endl << massMatrix << std::endl <<
+ "ExpectedMassMatrix = " << std::endl << expectedMassMatrix << std::endl;
+ EXPECT_TRUE(dampingMatrix.isApprox(zeroMatrix));
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++)
+ tri->addMatVec(1.0, 0.0, 0.0, ones, &forceVector);
+ for (SparseMatrix::Index 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++)
+ tri->addMatVec(0.0, 1.0, 0.0, ones, &forceVector);
+ for (SparseMatrix::Index 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++)
+ tri->addMatVec(0.0, 0.0, 1.0, ones, &forceVector);
+ for (SparseMatrix::Index 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++)
+ tri->addMatVec(1.0, 2.0, 3.0, ones, &forceVector);
+ for (SparseMatrix::Index 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();
+ 3.0 * expectedStiffnessMatrix.row(rowId).sum();
EXPECT_NEAR(expectedCoef, forceVector[rowId], epsilon * 10);
}
}
diff --git a/SurgSim/Physics/UnitTests/Fem2DLocalizationTest.cpp b/SurgSim/Physics/UnitTests/Fem2DLocalizationTest.cpp
new file mode 100644
index 0000000..da76155
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem2DLocalizationTest.cpp
@@ -0,0 +1,177 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Fem2DElementTriangle.h"
+#include "SurgSim/Physics/Fem2DLocalization.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
+#include "SurgSim/Physics/Fem3DRepresentation.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 Fem2DLocalizationTest : 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_invalidIndexLocalPosition.coordinate = SurgSim::Math::Vector::Zero(3);
+ m_invalidIndexLocalPosition.coordinate[0] = 0.4;
+ m_invalidIndexLocalPosition.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(Fem2DLocalizationTest, ConstructorTest)
+{
+ // IndexedLocalCoordinate pointing to a node (node index + empty coordinate) are invalid. It will failed,
+ // either because the index is out of bound or because the coordinates are the wrong size (empty)
+ // This is tested by m_invalidIndexLocalPosition and m_invalidCoordinateLocalPosition
+ ASSERT_THROW(std::make_shared<Fem2DLocalization>(m_fem, m_invalidIndexLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+
+ ASSERT_THROW(std::make_shared<Fem2DLocalization>(m_fem, m_invalidCoordinateLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+
+ ASSERT_NO_THROW(std::make_shared<Fem2DLocalization>(m_fem, m_validLocalPosition););
+}
+
+TEST_F(Fem2DLocalizationTest, IsValidRepresentation)
+{
+ Fem2DLocalization localization(m_fem, m_validLocalPosition);
+
+ ASSERT_TRUE(localization.isValidRepresentation(m_fem));
+
+ // nullptr is valid
+ ASSERT_TRUE(localization.isValidRepresentation(nullptr));
+
+ ASSERT_FALSE(localization.isValidRepresentation(std::make_shared<Fem1DRepresentation>("fem1d")));
+ ASSERT_FALSE(localization.isValidRepresentation(std::make_shared<Fem3DRepresentation>("fem3d")));
+}
+
+TEST_F(Fem2DLocalizationTest, ElementPose)
+{
+ SurgSim::Math::RigidTransform3d pose0, pose1;
+ SurgSim::Math::Vector3d edge0(1.0, 0.0, 0.0);
+ SurgSim::Math::Vector3d edge1 = SurgSim::Math::Vector3d(1.0, 1.0, 0.0).normalized();
+ SurgSim::Math::Vector3d normal(0.0, 0.0, 1.0);
+ SurgSim::Math::Vector3d binormal0 = edge0.cross(normal);
+ SurgSim::Math::Vector3d binormal1 = edge1.cross(normal);
+ SurgSim::Math::Matrix33d rotation;
+ rotation << edge0, normal, binormal0;
+ pose0 = SurgSim::Math::makeRigidTransform(rotation, SurgSim::Math::Vector3d(0.0, -1.0 / 3.0, 0.0));
+ rotation << edge1, normal, binormal1;
+ pose1 = SurgSim::Math::makeRigidTransform(rotation, SurgSim::Math::Vector3d(0.0, 1.0 / 3.0, 0.0));
+
+ {
+ SurgSim::DataStructures::IndexedLocalCoordinate testPosition1;
+ testPosition1.index = 0;
+ testPosition1.coordinate = SurgSim::Math::Vector::Zero(3);
+ testPosition1.coordinate[0] = 0.5;
+ testPosition1.coordinate[1] = 0.5;
+ Fem2DLocalization testLocalization1(m_fem, testPosition1);
+ auto pose = testLocalization1.getElementPose();
+ EXPECT_TRUE(pose0.isApprox(pose));
+ }
+
+ {
+ SurgSim::DataStructures::IndexedLocalCoordinate testPosition1;
+ testPosition1.index = 1;
+ testPosition1.coordinate = SurgSim::Math::Vector::Zero(3);
+ testPosition1.coordinate[0] = 0.5;
+ testPosition1.coordinate[1] = 0.5;
+ Fem2DLocalization testLocalization1(m_fem, testPosition1);
+ auto pose = testLocalization1.getElementPose();
+ EXPECT_TRUE(pose1.isApprox(pose));
+ }
+}
+
+} // namespace SurgSim
+} // namespace Physics
diff --git a/SurgSim/Physics/UnitTests/Fem2DMechanicalValidationTests.cpp b/SurgSim/Physics/UnitTests/Fem2DMechanicalValidationTests.cpp
index f3fcfe4..28717a6 100644
--- a/SurgSim/Physics/UnitTests/Fem2DMechanicalValidationTests.cpp
+++ b/SurgSim/Physics/UnitTests/Fem2DMechanicalValidationTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -75,7 +75,7 @@ private:
SurgSim::Math::Vector m_F, m_U;
protected:
- virtual void SetUp() override
+ void SetUp() override
{
m_fem = std::make_shared<Fem2DRepresentation>("Fem2D");
}
@@ -123,8 +123,7 @@ public:
// 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.setZero(m_fem->getInitialState()->getNumDof());
}
m_F.segment(m_fem->getNumDofPerNode() * nodeId, 3) = f;
}
@@ -168,10 +167,11 @@ public:
void solve()
{
m_fem->initialize(std::make_shared<SurgSim::Framework::Runtime>());
- Matrix K = m_fem->computeK(*(m_fem->getCurrentState()));
+ m_fem->updateFMDK(*(m_fem->getCurrentState()), Math::ODEEQUATIONUPDATE_K);
+ Matrix K = m_fem->getK();
m_fem->getCurrentState()->applyBoundaryConditionsToMatrix(&K);
m_fem->getCurrentState()->applyBoundaryConditionsToVector(&m_F);
- m_U = K.inverse() * m_F;
+ m_U = K.partialPivLu().solve(m_F);
}
double getUx(size_t nodeId) const
diff --git a/SurgSim/Physics/UnitTests/Fem2DPlyReaderDelegateTests.cpp b/SurgSim/Physics/UnitTests/Fem2DPlyReaderDelegateTests.cpp
index 1e3f5ea..8ea0893 100644
--- a/SurgSim/Physics/UnitTests/Fem2DPlyReaderDelegateTests.cpp
+++ b/SurgSim/Physics/UnitTests/Fem2DPlyReaderDelegateTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -18,10 +18,8 @@
#include "SurgSim/Framework/Runtime.h"
#include "SurgSim/Math/OdeState.h"
#include "SurgSim/Math/Vector.h"
-#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/Fem.h"
#include "SurgSim/Physics/Fem2DPlyReaderDelegate.h"
-#include "SurgSim/Physics/Fem2DRepresentation.h"
-#include "SurgSim/Physics/Fem2DElementTriangle.h"
namespace SurgSim
{
@@ -32,58 +30,75 @@ using SurgSim::DataStructures::PlyReader;
TEST(Fem2DRepresentationReaderTests, DelegateTest)
{
- auto femRepresentation = std::make_shared<Fem2DRepresentation>("Representation");
+ auto fem = std::make_shared<Fem2D>();
auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
- femRepresentation->setFilename("PlyReaderTests/Fem2D.ply");
- ASSERT_TRUE(femRepresentation->initialize(runtime));
+ fem->load("PlyReaderTests/Fem2D.ply");
// 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)));
+ EXPECT_TRUE(vertex0.isApprox(fem->getVertex(0).position));
+ EXPECT_TRUE(vertex5.isApprox(fem->getVertex(5).position));
// Number of triangles
- ASSERT_EQ(3u, femRepresentation->getNumFemElements());
+ ASSERT_EQ(3u, fem->getNumElements());
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())));
+ std::begin(fem->getElement(0)->nodeIds)));
EXPECT_TRUE(std::equal(std::begin(triangle2), std::end(triangle2),
- std::begin(femRepresentation->getFemElement(2)->getNodeIds())));
+ std::begin(fem->getElement(2)->nodeIds)));
// Boundary conditions
- ASSERT_EQ(2u * 6u, femRepresentation->getInitialState()->getNumBoundaryConditions());
+ ASSERT_EQ(2u, fem->getBoundaryConditions().size());
+
+ EXPECT_EQ(3, fem->getBoundaryCondition(0));
+ EXPECT_EQ(2, fem->getBoundaryCondition(1));
+
+ // Material
+ for (size_t i = 0; i < fem->getNumElements(); ++i)
+ {
+ auto element = fem->getElement(i);
+ EXPECT_DOUBLE_EQ(0.2, element->massDensity);
+ EXPECT_DOUBLE_EQ(0.3, element->poissonRatio);
+ EXPECT_DOUBLE_EQ(0.4, element->youngModulus);
+ }
+}
- // Boundary condition 0 is on node 8
- size_t boundaryNode0 = 3;
- size_t boundaryNode1 = 2;
+TEST(Fem2DRepresentationReaderTests, PerElementMaterial)
+{
+ auto fem = std::make_shared<Fem2D>();
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
- 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));
+ fem->load("PlyReaderTests/Fem2DMaterial.ply");
// Material
- for (size_t i = 0; i < femRepresentation->getNumFemElements(); ++i)
+ double value = 1.0;
+ for (size_t i = 0; i < fem->getNumElements(); ++i)
+ {
+ auto element = fem->getElement(i);
+ EXPECT_DOUBLE_EQ(value++, element->massDensity);
+ EXPECT_DOUBLE_EQ(value++, element->poissonRatio);
+ EXPECT_DOUBLE_EQ(value++, element->youngModulus);
+ }
+}
+
+TEST(Fem2DRepresentationReaderTests, NoMaterials)
+{
+ auto fem = std::make_shared<Fem2D>();
+ auto runtime = std::make_shared<Framework::Runtime>("config.txt");
+
+ ASSERT_NO_THROW(fem->load("PlyReaderTests/Fem2DNoMaterial.ply"));
+
+ for (auto element : fem->getElements())
{
- 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());
+ EXPECT_DOUBLE_EQ(0.0, element->massDensity);
+ EXPECT_DOUBLE_EQ(0.0, element->poissonRatio);
+ EXPECT_DOUBLE_EQ(0.0, element->youngModulus);
}
}
diff --git a/SurgSim/Physics/UnitTests/Fem2DRepresentationLocalizationTest.cpp b/SurgSim/Physics/UnitTests/Fem2DRepresentationLocalizationTest.cpp
deleted file mode 100644
index fa37fe6..0000000
--- a/SurgSim/Physics/UnitTests/Fem2DRepresentationLocalizationTest.cpp
+++ /dev/null
@@ -1,235 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
index 1ab700e..6fb2137 100644
--- a/SurgSim/Physics/UnitTests/Fem2DRepresentationTests.cpp
+++ b/SurgSim/Physics/UnitTests/Fem2DRepresentationTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -24,9 +24,10 @@
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
#include "SurgSim/Physics/Fem2DElementTriangle.h"
+#include "SurgSim/Physics/Fem2DLocalization.h"
#include "SurgSim/Physics/Fem2DRepresentation.h"
-#include "SurgSim/Physics/Fem2DRepresentationLocalization.h"
#include "SurgSim/Physics/UnitTests/MockObjects.h"
namespace SurgSim
@@ -40,12 +41,6 @@ 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");
@@ -110,6 +105,10 @@ TEST(Fem2DRepresentationTests, ExternalForceAPITest)
fem->setInitialState(initialState);
+ Math::SparseMatrix zeroMat(static_cast<SparseMatrix::Index>(fem->getNumDof()),
+ static_cast<SparseMatrix::Index>(fem->getNumDof()));
+ zeroMat.setZero();
+
// Vector initialized (properly sized and zeroed)
EXPECT_NE(0, fem->getExternalGeneralizedForce().size());
EXPECT_NE(0, fem->getExternalGeneralizedStiffness().rows());
@@ -122,8 +121,8 @@ TEST(Fem2DRepresentationTests, ExternalForceAPITest)
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());
+ EXPECT_TRUE(fem->getExternalGeneralizedStiffness().isApprox(zeroMat));
+ EXPECT_TRUE(fem->getExternalGeneralizedDamping().isApprox(zeroMat));
std::array<size_t, 3> element1NodeIds = {{0, 1, 2}};
auto element1 = std::make_shared<Fem2DElementTriangle>(element1NodeIds);
@@ -136,7 +135,7 @@ TEST(Fem2DRepresentationTests, ExternalForceAPITest)
femRepCoordinate.index = 0;
femRepCoordinate.coordinate = SurgSim::Math::Vector::Zero(3);
femRepCoordinate.coordinate[0] = 1.0;
- auto localization = std::make_shared<Fem2DRepresentationLocalization>(fem, femRepCoordinate);
+ auto localization = std::make_shared<Fem2DLocalization>(fem, femRepCoordinate);
auto wrongLocalizationType = std::make_shared<MockLocalization>();
Vector FLocalWrongSize = Vector::Ones(2 * fem->getNumDofPerNode());
@@ -147,38 +146,47 @@ TEST(Fem2DRepresentationTests, ExternalForceAPITest)
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;
+ SparseMatrix K(static_cast<SparseMatrix::Index>(fem->getNumDof()),
+ static_cast<SparseMatrix::Index>(fem->getNumDof()));
+ K.setZero();
+ Math::addSubMatrix(Klocal, 0, 0, &K, true);
+ K.makeCompressed();
+ SparseMatrix D(static_cast<SparseMatrix::Index>(fem->getNumDof()),
+ static_cast<SparseMatrix::Index>(fem->getNumDof()));
+ D.setZero();
+ Math::addSubMatrix(Dlocal, 0, 0, &D, true);
+ D.makeCompressed();
// Test invalid localization nullptr
ASSERT_THROW(fem->addExternalGeneralizedForce(nullptr, Flocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
ASSERT_THROW(fem->addExternalGeneralizedForce(nullptr, Flocal, Klocal, Dlocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
// Test invalid localization type
ASSERT_THROW(fem->addExternalGeneralizedForce(wrongLocalizationType, Flocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
ASSERT_THROW(fem->addExternalGeneralizedForce(wrongLocalizationType, Flocal, Klocal, Dlocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
// Test invalid force size
ASSERT_THROW(fem->addExternalGeneralizedForce(localization, FLocalWrongSize),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
ASSERT_THROW(fem->addExternalGeneralizedForce(localization, FLocalWrongSize, Klocal, Dlocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
// Test invalid stiffness size
ASSERT_THROW(fem->addExternalGeneralizedForce(localization, Flocal, KLocalWrongSize, Dlocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
// Test invalid damping size
ASSERT_THROW(fem->addExternalGeneralizedForce(localization, Flocal, Klocal, DLocalWrongSize),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
// Test valid call to addExternalGeneralizedForce
+ Math::SparseMatrix zeroMatrix(static_cast<SparseMatrix::Index>(fem->getNumDof()),
+ static_cast<SparseMatrix::Index>(fem->getNumDof()));
+ zeroMatrix.setZero();
fem->addExternalGeneralizedForce(localization, Flocal, Klocal, Dlocal);
EXPECT_FALSE(fem->getExternalGeneralizedForce().isZero());
- EXPECT_FALSE(fem->getExternalGeneralizedStiffness().isZero());
- EXPECT_FALSE(fem->getExternalGeneralizedDamping().isZero());
+ EXPECT_FALSE(fem->getExternalGeneralizedStiffness().isApprox(zeroMatrix));
+ EXPECT_FALSE(fem->getExternalGeneralizedDamping().isApprox(zeroMatrix));
EXPECT_TRUE(fem->getExternalGeneralizedForce().isApprox(F));
EXPECT_TRUE(fem->getExternalGeneralizedStiffness().isApprox(K));
EXPECT_TRUE(fem->getExternalGeneralizedDamping().isApprox(D));
@@ -190,25 +198,190 @@ TEST(Fem2DRepresentationTests, ExternalForceAPITest)
EXPECT_TRUE(fem->getExternalGeneralizedDamping().isApprox(2.0 * D));
}
+TEST(Fem2DRepresentationTests, LoadMeshTest)
+{
+ auto femRepresentation = std::make_shared<Fem2DRepresentation>("Representation");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+
+ femRepresentation->loadFem("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());
+ }
+}
+
+TEST(Fem2DRepresentationTests, CreateLocalizationTest)
+{
+ auto fem = std::make_shared<Fem2DRepresentation>("Representation");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ ASSERT_NO_THROW(fem->loadFem("PlyReaderTests/Fem2D.ply"));
+ ASSERT_NO_THROW(ASSERT_TRUE(fem->initialize(runtime)));
+
+ // Localization on an invalid node
+ {
+ SCOPED_TRACE("Invalid node");
+
+ SurgSim::DataStructures::Location location(1000);
+ std::shared_ptr<SurgSim::Physics::Localization> localization;
+ EXPECT_THROW(localization = fem->createLocalization(location), SurgSim::Framework::AssertionFailure);
+ }
+
+ // Localization on a valid node
+ {
+ SCOPED_TRACE("Valid node");
+
+ SurgSim::DataStructures::Location location(0);
+ std::shared_ptr<SurgSim::Physics::Fem2DLocalization> localization;
+ EXPECT_NO_THROW(localization =
+ std::dynamic_pointer_cast<SurgSim::Physics::Fem2DLocalization>(fem->createLocalization(location)););
+ EXPECT_TRUE(localization != nullptr);
+ EXPECT_TRUE(localization->getRepresentation() == fem);
+
+ SurgSim::Math::Vector3d globalPosition;
+ SurgSim::DataStructures::IndexedLocalCoordinate coordinate = localization->getLocalPosition();
+ EXPECT_NO_THROW(globalPosition = fem->getCurrentState()->getPosition(coordinate.index););
+ EXPECT_TRUE(globalPosition.isApprox(localization->calculatePosition()));
+ }
+
+ // In the 2d case, location of type triangle and element are the same and should behave the same.
+ // Hence, we are testing both but factorizing the code.
+ std::vector<std::pair<SurgSim::DataStructures::Location::Type, std::string>> locationTypesToTest;
+ locationTypesToTest.push_back(std::make_pair(SurgSim::DataStructures::Location::TRIANGLE, "triangle"));
+ locationTypesToTest.push_back(std::make_pair(SurgSim::DataStructures::Location::ELEMENT, "element"));
+ for (auto const& locationPairTypeString : locationTypesToTest)
+ {
+ auto const& locationType = locationPairTypeString.first;
+
+ SCOPED_TRACE("Location of type " + locationPairTypeString.second);
+
+ // Localization on an invalid triangle/element
+ {
+ SCOPED_TRACE("Invalid triangle/element index");
+
+ Vector barycentricCoordinates = Vector::Zero(3);
+ barycentricCoordinates[0] = 1.0;
+ SurgSim::DataStructures::IndexedLocalCoordinate coord(10000, barycentricCoordinates);
+ SurgSim::DataStructures::Location location(coord, locationType);
+ std::shared_ptr<SurgSim::Physics::Localization> localization;
+ EXPECT_THROW(localization = fem->createLocalization(location), SurgSim::Framework::AssertionFailure);
+ }
+
+ // Localization on an valid triangle/element but invalid barycentric coordinate size
+ {
+ SCOPED_TRACE("Invalid triangle/element barycentric coordinate size");
+
+ Vector barycentricCoordinates = Vector::Zero(4);
+ barycentricCoordinates[0] = 1.0;
+ SurgSim::DataStructures::IndexedLocalCoordinate coord(0, barycentricCoordinates);
+
+ SurgSim::DataStructures::Location location(coord, locationType);
+ std::shared_ptr<SurgSim::Physics::Localization> localization;
+ EXPECT_THROW(localization = fem->createLocalization(location), SurgSim::Framework::AssertionFailure);
+ }
+
+ // Localization on an valid triangle/element but invalid barycentric coordinate
+ {
+ SCOPED_TRACE("Invalid triangle/element barycentric coordinate");
+
+ Vector barycentricCoordinates = Vector::Ones(3);
+ SurgSim::DataStructures::IndexedLocalCoordinate coord(0, barycentricCoordinates);
+ SurgSim::DataStructures::Location location(coord, locationType);
+ std::shared_ptr<SurgSim::Physics::Localization> localization;
+ EXPECT_THROW(localization = fem->createLocalization(location), SurgSim::Framework::AssertionFailure);
+ }
+
+ // Localization on a valid triangle/element
+ {
+ SCOPED_TRACE("Valid triangle/element");
+
+ Vector barycentricCoordinates = Vector::Zero(3);
+ barycentricCoordinates.setConstant(1.0 / 3.0);
+ SurgSim::DataStructures::IndexedLocalCoordinate coord(0, barycentricCoordinates);
+ SurgSim::DataStructures::Location location(coord, locationType);
+ std::shared_ptr<SurgSim::Physics::Fem2DLocalization> localization;
+ EXPECT_NO_THROW(localization =
+ std::dynamic_pointer_cast<SurgSim::Physics::Fem2DLocalization>(
+ fem->createLocalization(location)));
+ EXPECT_TRUE(localization != nullptr);
+ EXPECT_TRUE(localization->getRepresentation() == fem);
+
+ SurgSim::DataStructures::IndexedLocalCoordinate coordinate = localization->getLocalPosition();
+ auto element = fem->getFemElement(coordinate.index);
+ SurgSim::Math::Vector3d globalPosition =
+ 1.0 / 3.0 * fem->getCurrentState()->getPosition(element->getNodeId(0)) +
+ 1.0 / 3.0 * fem->getCurrentState()->getPosition(element->getNodeId(1)) +
+ 1.0 / 3.0 * fem->getCurrentState()->getPosition(element->getNodeId(2));
+ EXPECT_TRUE(globalPosition.isApprox(localization->calculatePosition()));
+ }
+ }
+}
+
TEST(Fem2DRepresentationTests, SerializationTest)
{
auto fem2DRepresentation = std::make_shared<SurgSim::Physics::Fem2DRepresentation>("Test-Fem2D");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ const std::string filename = "PlyReaderTests/Fem2D.ply";
+ fem2DRepresentation->loadFem(filename);
+ auto collisionRepresentation = std::make_shared<DeformableCollisionRepresentation>("Collision");
+ fem2DRepresentation->setCollisionRepresentation(collisionRepresentation);
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_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());
+ EXPECT_EQ(filename, newRepresentation->getFem()->getValue<std::string>("FileName"));
}
} // namespace Physics
diff --git a/SurgSim/Physics/UnitTests/Fem3DConstraintFixedPointTests.cpp b/SurgSim/Physics/UnitTests/Fem3DConstraintFixedPointTests.cpp
new file mode 100644
index 0000000..27d86a5
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem3DConstraintFixedPointTests.cpp
@@ -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.
+
+#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/Fem3DElementTetrahedron.h"
+#include "SurgSim/Physics/Fem3DLocalization.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/FemConstraintFixedPoint.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> getTetrahedron(size_t nodeId0, size_t nodeId1,
+ size_t nodeId2, size_t nodeId3,
+ double massDensity,
+ double poissonRatio,
+ double youngModulus)
+{
+ std::array<size_t, 4> nodeIds = {{nodeId0, nodeId1, nodeId2, nodeId3}};
+ auto element = std::make_shared<Fem3DElementTetrahedron>(nodeIds);
+ element->setMassDensity(massDensity);
+ element->setPoissonRatio(poissonRatio);
+ element->setYoungModulus(youngModulus);
+ return element;
+}
+
+static std::shared_ptr<Fem3DRepresentation> getFem3d(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);
+
+ 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(getTetrahedron(0, 1, 2, 3, massDensity, poissonRatio, youngModulus));
+ fem->addFemElement(getTetrahedron(0, 1, 3, 4, massDensity, poissonRatio, youngModulus));
+ fem->addFemElement(getTetrahedron(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(Fem3DConstraintFixedPointTests, Constructor)
+{
+ ASSERT_NO_THROW(
+ { FemConstraintFixedPoint constraint; });
+}
+
+TEST(Fem3DConstraintFixedPointTests, Constants)
+{
+ FemConstraintFixedPoint constraint;
+
+ EXPECT_EQ(SurgSim::Physics::FIXED_3DPOINT, constraint.getConstraintType());
+ EXPECT_EQ(3u, constraint.getNumDof());
+}
+
+TEST(Fem3DConstraintFixedPointTests, 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 necessarily construct the physical parameters
+ // necessary to supply a realistic C. It only checks H and b.
+ FemConstraintFixedPoint constraint;
+
+ Vector3d actual;
+
+ // Setup parameters for FemConstraintFixedPoint::build
+ auto localization = std::make_shared<Fem3DLocalization>(getFem3d("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); // This weight is on node 4 (tetId 2, nodeId 2)
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST(Fem3DConstraintFixedPointTests, 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 necessarily construct the physical parameters
+ // necessary to supply a realistic C. It only checks H and b.
+ FemConstraintFixedPoint constraint;
+
+ Vector3d actual;
+
+ // Setup parameters for FemConstraintFixedPoint::build
+ auto localization = std::make_shared<Fem3DLocalization>(getFem3d("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); // This weight is on node 0 (tetId 2, nodeId 0)
+ SurgSim::Math::setSubMatrix(0.02 * dt * identity, 0, 1, 3, 3, &H); // This weight is on node 1 (tetId 2, nodeId 1)
+ SurgSim::Math::setSubMatrix(0.33 * dt * identity, 0, 4, 3, 3, &H); // This weight is on node 4 (tetId 2, nodeId 2)
+ SurgSim::Math::setSubMatrix(0.54 * dt * identity, 0, 5, 3, 3, &H); // This weight is on node 5 (tetId 2, nodeId 3)
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST(Fem3DConstraintFixedPointTests, 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 necessarily construct the physical parameters
+ // necessary to supply a realistic C. It only checks H and b.
+ FemConstraintFixedPoint constraint;
+
+ Vector3d actual;
+ Vector3d desired;
+
+ // Setup parameters for FemConstraintFixedPoint::build
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(18, 3, 1);
+
+ ConstraintData emptyConstraint;
+
+ auto localization = std::make_shared<Fem3DLocalization>(getFem3d("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); // This weight is on node 0 (tetId 2, nodeId 0)
+ SurgSim::Math::addSubMatrix( 0.02 * dt * identity, 0, 1, 3, 3, &H); // This weight is on node 1 (tetId 2, nodeId 1)
+ SurgSim::Math::addSubMatrix( 0.33 * dt * identity, 0, 4, 3, 3, &H); // This weight is on node 4 (tetId 2, nodeId 2)
+ SurgSim::Math::addSubMatrix( 0.54 * dt * identity, 0, 5, 3, 3, &H); // This weight is on node 5 (tetId 2, nodeId 3)
+ SurgSim::Math::addSubMatrix(-0.32 * dt * identity, 0, 0, 3, 3, &H); // This weight is on node 0 (tetId 1, nodeId 0)
+ SurgSim::Math::addSubMatrix(-0.05 * dt * identity, 0, 1, 3, 3, &H); // This weight is on node 1 (tetId 1, nodeId 1)
+ SurgSim::Math::addSubMatrix(-0.14 * dt * identity, 0, 3, 3, 3, &H); // This weight is on node 3 (tetId 1, nodeId 2)
+ SurgSim::Math::addSubMatrix(-0.49 * dt * identity, 0, 4, 3, 3, &H); // This weight is on node 4 (tetId 1, nodeId 3)
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/Fem3DConstraintFrictionalSlidingTests.cpp b/SurgSim/Physics/UnitTests/Fem3DConstraintFrictionalSlidingTests.cpp
new file mode 100644
index 0000000..e753028
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem3DConstraintFrictionalSlidingTests.cpp
@@ -0,0 +1,299 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/LinearSparseSolveAndInverse.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
+#include "SurgSim/Physics/Fem3DLocalization.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/FemConstraintFrictionalSliding.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/SlidingConstraintData.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+using SurgSim::Framework::Runtime;
+using SurgSim::Physics::Fem3DRepresentation;
+using SurgSim::Physics::FemConstraintFrictionalSliding;
+using SurgSim::Physics::Fem3DLocalization;
+using SurgSim::Physics::Fem3DElementTetrahedron;
+using SurgSim::Physics::MlcpPhysicsProblem;
+using SurgSim::Physics::SlidingConstraintData;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+
+namespace
+{
+ const double epsilon = 1e-10;
+ const double dt = 1e-3;
+};
+
+static void addTetrahedron(Fem3DRepresentation* fem,
+ size_t node0, size_t node1, size_t node2, size_t node3,
+ 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);
+ fem->addFemElement(element);
+}
+
+class Fem3DConstraintFrictionalSlidingTests : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ // Define plane with normal 'n' pointing against gravity.
+ m_slidingDirection = Vector3d(0.8539, 0.6289, -0.9978);
+ m_slidingDirection.normalize();
+
+ // Create mock FEM
+ m_fem = std::make_shared<Fem3DRepresentation>("Fem2dRepresentation");
+ 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);
+
+ addTetrahedron(m_fem.get(), 0, 1, 2, 3);
+ addTetrahedron(m_fem.get(), 0, 1, 3, 4);
+ addTetrahedron(m_fem.get(), 0, 1, 4, 5);
+
+ m_fem->setInitialState(state);
+ m_fem->setIntegrationScheme(SurgSim::Math::IntegrationScheme::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED);
+ 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 setSlidingConstraintAt(const IndexedLocalCoordinate& coord)
+ {
+ m_localization = std::make_shared<Fem3DLocalization>(m_fem, coord);
+ m_constraintData.setSlidingDirection(m_localization->calculatePosition(0.0), m_slidingDirection);
+ }
+
+ std::shared_ptr<Fem3DRepresentation> m_fem;
+ std::shared_ptr<Fem3DLocalization> m_localization;
+
+ Vector3d m_slidingDirection;
+ SlidingConstraintData m_constraintData;
+};
+
+TEST_F(Fem3DConstraintFrictionalSlidingTests, ConstructorTest)
+{
+ ASSERT_NO_THROW(
+ {
+ FemConstraintFrictionalSliding femContact;
+ });
+}
+
+TEST_F(Fem3DConstraintFrictionalSlidingTests, ConstraintConstantsTest)
+{
+ auto implementation = std::make_shared<FemConstraintFrictionalSliding>();
+
+ EXPECT_EQ(SurgSim::Physics::FRICTIONAL_SLIDING, implementation->getConstraintType());
+ EXPECT_EQ(3u, implementation->getNumDof());
+}
+
+TEST_F(Fem3DConstraintFrictionalSlidingTests, BuildMlcpTest)
+{
+ // This test verifies the build mlcp function works for a simple case.
+
+ auto implementation = std::make_shared<FemConstraintFrictionalSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 3, 1);
+
+ // Apply constraint purely to the first node of the 0th tetrahedron.
+ IndexedLocalCoordinate coord(0, Vector4d(1.0, 0.0, 0.0, 0.0));
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ Eigen::Matrix<double, 3, 18> H = Eigen::Matrix<double, 3, 18>::Zero();
+ H.block<1, 3>(0, 0) = (dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 0) = (dt * m_slidingDirection).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 18, 18> C = dt * m_fem->getM().toDense().inverse();
+
+ EXPECT_NEAR_EIGEN(C * H.transpose(), mlcpPhysicsProblem.CHt, epsilon);
+
+ EXPECT_NEAR_EIGEN(H * C * H.transpose(), mlcpPhysicsProblem.A, epsilon);
+
+ EXPECT_DOUBLE_EQ(0.5, mlcpPhysicsProblem.mu[0]);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST_F(Fem3DConstraintFrictionalSlidingTests, BuildMlcpCoordinateTest)
+{
+ // This test verifies the build mlcp function works for a more complicated case with different nodes.
+ auto implementation = std::make_shared<FemConstraintFrictionalSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 3, 1);
+
+ // Apply constraint to the four nodes of the 0th tetrahedron.
+ auto barycentric = Vector4d(0.25, 0.33, 0.28, 0.14);
+ IndexedLocalCoordinate coord(0, barycentric);
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ Eigen::Matrix<double, 3, 18> H = Eigen::Matrix<double, 3, 18>::Zero();
+ H.block<1, 3>(0, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 0) = (barycentric[0] * dt * m_slidingDirection).eval();
+ H.block<1, 3>(0, 3) = (barycentric[1] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 3) = (barycentric[1] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 3) = (barycentric[1] * dt * m_slidingDirection).eval();
+ H.block<1, 3>(0, 6) = (barycentric[2] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 6) = (barycentric[2] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 6) = (barycentric[2] * dt * m_slidingDirection).eval();
+ H.block<1, 3>(0, 9) = (barycentric[3] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 9) = (barycentric[3] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 9) = (barycentric[3] * dt * m_slidingDirection).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 18, 18> C = dt * m_fem->getM().toDense().inverse();
+
+ EXPECT_NEAR_EIGEN(C * H.transpose(), mlcpPhysicsProblem.CHt, epsilon);
+
+ EXPECT_NEAR_EIGEN(H * C * H.transpose(), mlcpPhysicsProblem.A, epsilon);
+
+ EXPECT_DOUBLE_EQ(0.5, mlcpPhysicsProblem.mu[0]);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+TEST_F(Fem3DConstraintFrictionalSlidingTests, BuildMlcpIndiciesTest)
+{
+ // This test verifies the build mlcp function works given a hypothetical, preexisting mlcp problem.
+ auto implementation = std::make_shared<FemConstraintFrictionalSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof() + 5, 4, 2);
+
+ // Suppose 5 dof and 1 constraint are defined elsewhere. Then H, CHt, HCHt, and b are prebuilt.
+ Eigen::SparseVector<double, Eigen::RowMajor, ptrdiff_t> localH;
+ localH.resize(5);
+ localH.reserve(5);
+ localH.insert(0) = 0.9478;
+ localH.insert(1) = -0.3807;
+ localH.insert(2) = 0.5536;
+ localH.insert(3) = -0.6944;
+ localH.insert(4) = 0.1815;
+
+ mlcpPhysicsProblem.H.coeffRef(0, 0) += localH.coeff(0);
+ mlcpPhysicsProblem.H.coeffRef(0, 1) += localH.coeff(1);
+ mlcpPhysicsProblem.H.coeffRef(0, 2) += localH.coeff(2);
+ mlcpPhysicsProblem.H.coeffRef(0, 3) += localH.coeff(3);
+ mlcpPhysicsProblem.H.coeffRef(0, 4) += localH.coeff(4);
+
+ 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 the Fem at 5th dof (1 or multiple representations exist before, covering a total of 5 dof)
+ // and the constraint at the index 1 (1 atomic constraint exists before this one)
+ size_t indexOfRepresentation = 5;
+ size_t indexOfConstraint = 1;
+
+ // Apply constraint to the four nodes of the 0th tetrahedron
+ auto barycentric = Vector4d(0.25, 0.33, 0.28, 0.14);
+ IndexedLocalCoordinate coord(0, barycentric);
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, indexOfRepresentation, indexOfConstraint,
+ SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ Eigen::Matrix<double, 3, 18> H = Eigen::Matrix<double, 3, 18>::Zero();
+ H.block<1, 3>(0, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 0) = (barycentric[0] * dt * m_slidingDirection).eval();
+ H.block<1, 3>(0, 3) = (barycentric[1] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 3) = (barycentric[1] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 3) = (barycentric[1] * dt * m_slidingDirection).eval();
+ H.block<1, 3>(0, 6) = (barycentric[2] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 6) = (barycentric[2] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 6) = (barycentric[2] * dt * m_slidingDirection).eval();
+ H.block<1, 3>(0, 9) = (barycentric[3] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 9) = (barycentric[3] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(2, 9) = (barycentric[3] * dt * m_slidingDirection).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H.block(indexOfConstraint, indexOfRepresentation, 3, 18), epsilon);
+
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 18, 18> C = dt * m_fem->getM().toDense().inverse();
+
+ EXPECT_NEAR_EIGEN(
+ C * H.transpose(), mlcpPhysicsProblem.CHt.block(indexOfRepresentation, indexOfConstraint, 18, 3), epsilon);
+
+ EXPECT_NEAR_EIGEN(
+ H * C * H.transpose(), mlcpPhysicsProblem.A.block(indexOfConstraint, indexOfConstraint, 3, 3), epsilon);
+
+ EXPECT_DOUBLE_EQ(0.5, mlcpPhysicsProblem.mu[1]);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
diff --git a/SurgSim/Physics/UnitTests/Fem3DConstraintFrictionlessContactTests.cpp b/SurgSim/Physics/UnitTests/Fem3DConstraintFrictionlessContactTests.cpp
new file mode 100644
index 0000000..7b2e128
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem3DConstraintFrictionlessContactTests.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 "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/LinearSparseSolveAndInverse.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
+#include "SurgSim/Physics/Fem3DLocalization.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/FemConstraintFrictionlessContact.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::FemConstraintFrictionlessContact;
+using SurgSim::Physics::Fem3DLocalization;
+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;
+const double mlcpPrecision = 1e-04;
+};
+
+static void addTetraheadron(Fem3DRepresentation* fem,
+ size_t node0,
+ size_t node1,
+ size_t node2,
+ size_t node3,
+ 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);
+ fem->addFemElement(element);
+}
+
+class Fem3DConstraintFrictionlessContactTests : 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);
+ addTetraheadron(m_fem.get(), 0, 1, 3, 4);
+ addTetraheadron(m_fem.get(), 0, 1, 4, 5);
+
+ m_fem->setInitialState(state);
+ m_fem->setIntegrationScheme(SurgSim::Math::IntegrationScheme::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED);
+ 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<Fem3DLocalization>(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<Fem3DLocalization> m_localization;
+
+ IndexedLocalCoordinate m_coord;
+ Vector3d m_n;
+ ContactConstraintData m_constraintData;
+};
+
+TEST_F(Fem3DConstraintFrictionlessContactTests, ConstructorTest)
+{
+ ASSERT_NO_THROW(
+ {
+ FemConstraintFrictionlessContact femContact;
+ });
+}
+
+TEST_F(Fem3DConstraintFrictionlessContactTests, ConstraintConstantsTest)
+{
+ auto implementation = std::make_shared<FemConstraintFrictionlessContact>();
+
+ EXPECT_EQ(SurgSim::Physics::FRICTIONLESS_3DCONTACT, implementation->getConstraintType());
+ EXPECT_EQ(1u, implementation->getNumDof());
+}
+
+TEST_F(Fem3DConstraintFrictionlessContactTests, BuildMlcpTest)
+{
+ // This test verifies the build mlcp function works for a simple case.
+
+ auto implementation = std::make_shared<FemConstraintFrictionlessContact>();
+
+ // 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);
+
+ const Vector3d newPosition = Vector3d(0.30, -0.57, 0.40) - Vector3d::UnitY() * 9.81 * dt * dt;
+ EXPECT_NEAR(newPosition.dot(m_n), mlcpPhysicsProblem.b[0] + mlcpPrecision, 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}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 18, 18> denseMat = m_fem->getM();
+ Eigen::Matrix<double, 18, 18> C = dt * denseMat.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(Fem3DConstraintFrictionlessContactTests, BuildMlcpCoordinateTest)
+{
+ // This test verifies the build mlcp function works for a more complicated case with different nodes.
+
+ auto implementation = std::make_shared<FemConstraintFrictionlessContact>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 1, 1);
+
+ // Apply constraint to all nodes of an fem.
+ const Vector4d barycentric = Vector4d(0.25, 0.33, 0.28, 0.14);
+ IndexedLocalCoordinate coord(1, barycentric);
+ setContactAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ const Vector3d newPosition = (Vector3d(0.30, -0.57, 0.40) * barycentric[0] +
+ Vector3d(0.06, 0.63, -0.32) * barycentric[1] +
+ Vector3d(0.35, 0.52, 0.50) * barycentric[2] +
+ Vector3d(1.14, 0.66, 0.71) * barycentric[3]) -
+ Vector3d::UnitY() * 9.81 * dt * dt;
+ EXPECT_NEAR(newPosition.dot(m_n), mlcpPhysicsProblem.b[0] + mlcpPrecision, 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}
+ SurgSim::Math::Matrix C;
+ SurgSim::Math::SparseMatrix M(18, 18);
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ M = m_fem->getM();
+ SurgSim::Math::LinearSparseSolveAndInverseLU solver;
+ SurgSim::Math::Vector b = SurgSim::Math::Vector::Zero(18);
+ solver.setMatrix(M);
+ C = solver.getInverse();
+ C *= dt;
+
+ 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(Fem3DConstraintFrictionlessContactTests, BuildMlcpIndiciesTest)
+{
+ // This test verifies the build mlcp function works given a hypothetical, preexisting mlcp problem.
+
+ auto implementation = std::make_shared<FemConstraintFrictionlessContact>();
+
+ // 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::SparseVector<double, Eigen::RowMajor, ptrdiff_t> localH;
+ localH.resize(5);
+ localH.reserve(5);
+ localH.insert(0) = 0.9478;
+ localH.insert(1) = -0.3807;
+ localH.insert(2) = 0.5536;
+ localH.insert(3) = -0.6944;
+ localH.insert(4) = 0.1815;
+
+ mlcpPhysicsProblem.H.coeffRef(0, 0) += localH.coeff(0);
+ mlcpPhysicsProblem.H.coeffRef(0, 1) += localH.coeff(1);
+ mlcpPhysicsProblem.H.coeffRef(0, 2) += localH.coeff(2);
+ mlcpPhysicsProblem.H.coeffRef(0, 3) += localH.coeff(3);
+ mlcpPhysicsProblem.H.coeffRef(0, 4) += localH.coeff(4);
+
+ 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 the Fem at 5th dof (1 or multiple representations exist before, covering a total of 5 dof)
+ // and the constraint at the index 1 (1 atomic constraint exists before this one)
+ size_t indexOfRepresentation = 5;
+ size_t indexOfConstraint = 1;
+
+ // Apply constraint to all nodes of an fem.
+ const Vector4d barycentric = Vector4d(0.25, 0.33, 0.28, 0.14);
+ IndexedLocalCoordinate coord(1, barycentric);
+ setContactAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, indexOfRepresentation, indexOfConstraint,
+ SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ const Vector3d newPosition = (Vector3d(0.30, -0.57, 0.40) * barycentric[0] +
+ Vector3d(0.06, 0.63, -0.32) * barycentric[1] +
+ Vector3d(0.35, 0.52, 0.50) * barycentric[2] +
+ Vector3d(1.14, 0.66, 0.71) * barycentric[3]) -
+ Vector3d::UnitY() * 9.81 * dt * dt;
+ EXPECT_NEAR(newPosition.dot(m_n), mlcpPhysicsProblem.b[indexOfConstraint] + mlcpPrecision, 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);
+
+ SurgSim::Math::Matrix C;
+ SurgSim::Math::SparseMatrix M(18, 18);
+ m_fem->updateFMDK(*m_fem->getPreviousState(), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ M = m_fem->getM();
+ SurgSim::Math::LinearSparseSolveAndInverseLU solver;
+ SurgSim::Math::Vector b = SurgSim::Math::Vector::Zero(18);
+ solver.setMatrix(M);
+ C = solver.getInverse();
+ C *= dt;
+
+ 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/Fem3DConstraintFrictionlessSlidingTests.cpp b/SurgSim/Physics/UnitTests/Fem3DConstraintFrictionlessSlidingTests.cpp
new file mode 100644
index 0000000..79b30a5
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem3DConstraintFrictionlessSlidingTests.cpp
@@ -0,0 +1,299 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/LinearSparseSolveAndInverse.h"
+#include "SurgSim/Math/OdeState.h"
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
+#include "SurgSim/Physics/Fem3DLocalization.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
+#include "SurgSim/Physics/FemConstraintFrictionlessSliding.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/SlidingConstraintData.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+using SurgSim::Framework::Runtime;
+using SurgSim::Physics::Fem3DRepresentation;
+using SurgSim::Physics::FemConstraintFrictionlessSliding;
+using SurgSim::Physics::Fem3DLocalization;
+using SurgSim::Physics::Fem3DElementTetrahedron;
+using SurgSim::Physics::MlcpPhysicsProblem;
+using SurgSim::Physics::SlidingConstraintData;
+using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector4d;
+
+namespace
+{
+ const double epsilon = 1e-10;
+ const double dt = 1e-3;
+};
+
+static void addTetrahedron(Fem3DRepresentation* fem,
+ size_t node0, size_t node1, size_t node2, size_t node3,
+ 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);
+ fem->addFemElement(element);
+}
+
+class Fem3DConstraintFrictionlessSlidingTests : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ // Define plane with normal 'n' pointing against gravity.
+ m_slidingDirection = Vector3d(0.8539, 0.6289, -0.9978);
+ m_slidingDirection.normalize();
+
+ // Create mock FEM
+ m_fem = std::make_shared<Fem3DRepresentation>("Fem2dRepresentation");
+ 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);
+
+ addTetrahedron(m_fem.get(), 0, 1, 2, 3);
+ addTetrahedron(m_fem.get(), 0, 1, 3, 4);
+ addTetrahedron(m_fem.get(), 0, 1, 4, 5);
+
+ m_fem->setInitialState(state);
+ m_fem->setIntegrationScheme(SurgSim::Math::IntegrationScheme::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED);
+ 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 setSlidingConstraintAt(const IndexedLocalCoordinate& coord)
+ {
+ m_localization = std::make_shared<Fem3DLocalization>(m_fem, coord);
+ m_constraintData.setSlidingDirection(m_localization->calculatePosition(0.0), m_slidingDirection);
+ }
+
+ std::shared_ptr<Fem3DRepresentation> m_fem;
+ std::shared_ptr<Fem3DLocalization> m_localization;
+
+ Vector3d m_slidingDirection;
+ SlidingConstraintData m_constraintData;
+};
+
+TEST_F(Fem3DConstraintFrictionlessSlidingTests, ConstructorTest)
+{
+ ASSERT_NO_THROW(
+ {
+ FemConstraintFrictionlessSliding femContact;
+ });
+}
+
+TEST_F(Fem3DConstraintFrictionlessSlidingTests, ConstraintConstantsTest)
+{
+ auto implementation = std::make_shared<FemConstraintFrictionlessSliding>();
+
+ EXPECT_EQ(SurgSim::Physics::FRICTIONLESS_SLIDING, implementation->getConstraintType());
+ EXPECT_EQ(2u, implementation->getNumDof());
+}
+
+TEST_F(Fem3DConstraintFrictionlessSlidingTests, BuildMlcpTest)
+{
+ // This test verifies the build mlcp function works for a simple case.
+
+ auto implementation = std::make_shared<FemConstraintFrictionlessSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 2, 1);
+
+ // Apply constraint purely to the first node of the 0th tetrahedron.
+ IndexedLocalCoordinate coord(0, Vector4d(1.0, 0.0, 0.0, 0.0));
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ auto pose = m_constraintData.getPose().inverse().rotation();
+ EXPECT_TRUE((pose * m_slidingDirection).isApprox(Vector3d(1.0, 0.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[0]).isApprox(Vector3d(0.0, 1.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[1]).isApprox(Vector3d(0.0, 0.0, 1.0)));
+
+ Eigen::Matrix<double, 2, 18> H = Eigen::Matrix<double, 2, 18>::Zero();
+ H.block<1, 3>(0, 0) = (dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (dt * m_constraintData.getNormals()[1]).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 18, 18> C = dt * m_fem->getM().toDense().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(Fem3DConstraintFrictionlessSlidingTests, BuildMlcpCoordinateTest)
+{
+ // This test verifies the build mlcp function works for a more complicated case with different nodes.
+ auto implementation = std::make_shared<FemConstraintFrictionlessSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof(), 2, 1);
+
+ // Apply constraint to the four nodes of the 0th tetrahedron.
+ auto barycentric = Vector4d(0.25, 0.33, 0.28, 0.14);
+ IndexedLocalCoordinate coord(0, barycentric);
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ auto pose = m_constraintData.getPose().inverse().rotation();
+ EXPECT_TRUE((pose * m_slidingDirection).isApprox(Vector3d(1.0, 0.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[0]).isApprox(Vector3d(0.0, 1.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[1]).isApprox(Vector3d(0.0, 0.0, 1.0)));
+
+ Eigen::Matrix<double, 2, 18> H = Eigen::Matrix<double, 2, 18>::Zero();
+ H.block<1, 3>(0, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(0, 3) = (barycentric[1] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 3) = (barycentric[1] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(0, 6) = (barycentric[2] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 6) = (barycentric[2] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(0, 9) = (barycentric[3] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 9) = (barycentric[3] * dt * m_constraintData.getNormals()[1]).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ // C = dt * m^{-1}
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 18, 18> C = dt * m_fem->getM().toDense().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(Fem3DConstraintFrictionlessSlidingTests, BuildMlcpIndiciesTest)
+{
+ // This test verifies the build mlcp function works given a hypothetical, preexisting mlcp problem.
+ auto implementation = std::make_shared<FemConstraintFrictionlessSliding>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_fem->getNumDof() + 5, 3, 2);
+
+ // Suppose 5 dof and 1 constraint are defined elsewhere. Then H, CHt, HCHt, and b are prebuilt.
+ Eigen::SparseVector<double, Eigen::RowMajor, ptrdiff_t> localH;
+ localH.resize(5);
+ localH.reserve(5);
+ localH.insert(0) = 0.9478;
+ localH.insert(1) = -0.3807;
+ localH.insert(2) = 0.5536;
+ localH.insert(3) = -0.6944;
+ localH.insert(4) = 0.1815;
+
+ mlcpPhysicsProblem.H.coeffRef(0, 0) += localH.coeff(0);
+ mlcpPhysicsProblem.H.coeffRef(0, 1) += localH.coeff(1);
+ mlcpPhysicsProblem.H.coeffRef(0, 2) += localH.coeff(2);
+ mlcpPhysicsProblem.H.coeffRef(0, 3) += localH.coeff(3);
+ mlcpPhysicsProblem.H.coeffRef(0, 4) += localH.coeff(4);
+
+ 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 the Fem at 5th dof (1 or multiple representations exist before, covering a total of 5 dof)
+ // and the constraint at the index 1 (1 atomic constraint exists before this one)
+ size_t indexOfRepresentation = 5;
+ size_t indexOfConstraint = 1;
+
+ // Apply constraint to the four nodes of the 0th tetrahedron
+ auto barycentric = Vector4d(0.25, 0.33, 0.28, 0.14);
+ IndexedLocalCoordinate coord(0, barycentric);
+ setSlidingConstraintAt(coord);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, indexOfRepresentation, indexOfConstraint,
+ SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ auto pose = m_constraintData.getPose().inverse().rotation();
+ EXPECT_TRUE((pose * m_slidingDirection).isApprox(Vector3d(1.0, 0.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[0]).isApprox(Vector3d(0.0, 1.0, 0.0)));
+ EXPECT_TRUE((pose * m_constraintData.getNormals()[1]).isApprox(Vector3d(0.0, 0.0, 1.0)));
+
+ Eigen::Matrix<double, 2, 18> H = Eigen::Matrix<double, 2, 18>::Zero();
+ H.block<1, 3>(0, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 0) = (barycentric[0] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(0, 3) = (barycentric[1] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 3) = (barycentric[1] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(0, 6) = (barycentric[2] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 6) = (barycentric[2] * dt * m_constraintData.getNormals()[1]).eval();
+ H.block<1, 3>(0, 9) = (barycentric[3] * dt * m_constraintData.getNormals()[0]).eval();
+ H.block<1, 3>(1, 9) = (barycentric[3] * dt * m_constraintData.getNormals()[1]).eval();
+
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H.block(indexOfConstraint, indexOfRepresentation, 2, 18), epsilon);
+
+ m_fem->updateFMDK(*(m_fem->getPreviousState()), SurgSim::Math::ODEEQUATIONUPDATE_M);
+ Eigen::Matrix<double, 18, 18> C = dt * m_fem->getM().toDense().inverse();
+
+ EXPECT_NEAR_EIGEN(
+ C * H.transpose(), mlcpPhysicsProblem.CHt.block(indexOfRepresentation, indexOfConstraint, 18, 2), epsilon);
+
+ EXPECT_NEAR_EIGEN(
+ H * C * H.transpose(), mlcpPhysicsProblem.A.block(indexOfConstraint, indexOfConstraint, 2, 2), epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
diff --git a/SurgSim/Physics/UnitTests/Fem3DElementCorotationalTetrahedronTests.cpp b/SurgSim/Physics/UnitTests/Fem3DElementCorotationalTetrahedronTests.cpp
index 588c7fc..244f6c5 100644
--- a/SurgSim/Physics/UnitTests/Fem3DElementCorotationalTetrahedronTests.cpp
+++ b/SurgSim/Physics/UnitTests/Fem3DElementCorotationalTetrahedronTests.cpp
@@ -19,14 +19,17 @@
#include <array>
#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeEquation.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::Vector3d;
using SurgSim::Math::Matrix;
+using SurgSim::Math::Matrix33d;
+using SurgSim::Math::SparseMatrix;
namespace
{
@@ -37,42 +40,58 @@ const double epsilonAddMatVec = 1e-10;
class MockFem3DElementCorotationalTet : public Fem3DElementCorotationalTetrahedron
{
public:
- MockFem3DElementCorotationalTet(std::array<size_t, 4> nodeIds)
+ explicit MockFem3DElementCorotationalTet(std::array<size_t, 4> nodeIds)
: Fem3DElementCorotationalTetrahedron(nodeIds)
{
}
- const Eigen::Matrix<double,12 ,1>& getInitialPosition() const
+ const Eigen::Matrix<double, 12 , 1>& getInitialPosition() const
{
return m_x0;
}
- const SurgSim::Math::Matrix33d& getRotation() const
+ const Eigen::Matrix<double, 12, 12>& getNonRotatedMassMatrix() const
+ {
+ return m_MLinear;
+ }
+
+ const Eigen::Matrix<double, 12, 12>& getNonRotatedStiffnessMatrix() const
{
- return m_rotation;
+ return m_KLinear;
}
- const Eigen::Matrix<double, 12, 12>& getMassMatrix() const
+ const SurgSim::Math::Matrix getRotatedStiffnessMatrix(const SurgSim::Math::OdeState& state) const
+ {
+ return m_K;
+ }
+
+ const SurgSim::Math::Matrix getRotatedMassMatrix(const SurgSim::Math::OdeState& state) const
{
return m_M;
}
- const Eigen::Matrix<double, 12, 12>& getRotatedStiffness() const
+ /// Compute the rotation, mass and stiffness matrices of the element from the given state
+ /// \param state The state to compute the rotation and jacobians from
+ /// \param [out] R rotation matrix of the element in the given state (can be nullptr if not needed)
+ /// \param [out] Me, Ke Respectively the mass and stiffness matrices (Me and/or Ke be nullptr if not needed)
+ /// \note The model is not viscoelastic but purely elastic, so there is no damping matrix here.
+ void computeRotationMassAndStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::Matrix33d* R,
+ SurgSim::Math::Matrix* Me, SurgSim::Math::Matrix* Ke) const
{
- return m_corotationalStiffnessMatrix;
+ Fem3DElementCorotationalTetrahedron::computeRotationMassAndStiffness(state, R, Me, Ke);
}
- const Eigen::Matrix<double, 12, 12>& getNonRotatedStiffness() const
+ const SurgSim::Math::Matrix33d getRotation(const SurgSim::Math::OdeState& state) const
{
- return m_K;
+ return m_R;
}
- const SurgSim::Math::Matrix44d& getVinverse() const
+ const SurgSim::Math::Matrix44d getVInverse() const
{
return m_Vinverse;
}
- void setupInitialParams(const SurgSim::Math::OdeState &state,
+ void setupInitialParams(const SurgSim::Math::OdeState& state,
double massDensity,
double poissonRatio,
double youngModulus)
@@ -89,14 +108,14 @@ 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;
+ SurgSim::Math::OdeState m_restState, m_invalidState, 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
+ void SetUp() override
{
m_nodeIds[0] = 3;
m_nodeIds[1] = 1;
@@ -105,27 +124,35 @@ public:
m_nodeIdsAsVector.assign(m_nodeIds.cbegin(), m_nodeIds.cend());
m_restState.setNumDof(3, 15);
+ m_invalidState.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) }};
+ Vector& invalidx0 = m_invalidState.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];
+ SurgSim::Math::getSubVector(invalidx0, m_nodeIds[nodeId], 3) = points[nodeId];
}
+ // In the invalid state, the tetrahedron is degenerated to a triangle (last 2 points are equal)
+ SurgSim::Math::getSubVector(invalidx0, m_nodeIds[3], 3) = points[2];
m_rho = 1000.0;
m_E = 1e6;
m_nu = 0.45;
- Vector3d axis(1.1,-2.2,-3.3);
+ Vector3d axis(1.0, 0.2, -0.3);
axis.normalize();
- m_rotation = SurgSim::Math::makeRotationMatrix(3.1415, axis);
+ m_rotation = SurgSim::Math::makeRotationMatrix(4.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_R12x12.block<3, 3>(3 * nodeId, 3 * nodeId) = m_rotation;
}
m_translation = Vector3d(1.2, 2.3, 3.4);
}
@@ -137,8 +164,11 @@ 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)}};
+ 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)
@@ -160,30 +190,27 @@ Eigen::Matrix<double, 12, 12> make12x12(const Eigen::Matrix<double, 3, 3>& 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;});
+ delete tet;
+ });
ASSERT_NO_THROW({std::shared_ptr<MockFem3DElementCorotationalTet> tet =
- std::make_shared<MockFem3DElementCorotationalTet>(m_nodeIds);});
+ 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());
+ ASSERT_NO_THROW(tet.setupInitialParams(m_restState, m_rho, m_nu, m_E));
- EXPECT_TRUE(tet.getRotatedStiffness().isApprox(tet.getNonRotatedStiffness()));
+ EXPECT_TRUE(tet.getRotation(m_restState).isIdentity());
+ EXPECT_TRUE(tet.getRotatedStiffnessMatrix(m_restState).isApprox(tet.getNonRotatedStiffnessMatrix()));
+ EXPECT_TRUE(tet.getRotatedMassMatrix(m_restState).isApprox(tet.getNonRotatedMassMatrix()));
// V^1 = (a b c d)^-1
// (1 1 1 1)
@@ -195,15 +222,21 @@ TEST_F(Fem3DElementCorotationalTetrahedronTests, InitializeTest)
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()));
+ EXPECT_TRUE(tet.getVInverse().isApprox(expectedV.inverse()));
+
+ MockFem3DElementCorotationalTet invalidTet(m_nodeIds);
+ ASSERT_THROW(invalidTet.setupInitialParams(m_invalidState, m_rho, m_nu, m_E),
+ SurgSim::Framework::AssertionFailure);
}
-TEST_F(Fem3DElementCorotationalTetrahedronTests, UpdateTest)
+TEST_F(Fem3DElementCorotationalTetrahedronTests, ComputeRotationMassAndStiffnessTest)
{
using SurgSim::Math::skew;
using SurgSim::Math::makeSkewSymmetricMatrix;
Eigen::Transform<double, 3, Eigen::Affine> transformation;
+ SurgSim::Math::Matrix33d R;
+ SurgSim::Math::Matrix M, K;
{
SCOPED_TRACE("No rotation, no translation");
@@ -214,9 +247,12 @@ TEST_F(Fem3DElementCorotationalTetrahedronTests, UpdateTest)
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()));
+ ASSERT_NO_THROW(tet.computeRotationMassAndStiffness(m_state, &R, &M, &K));
+ EXPECT_TRUE(R.isIdentity());
+ EXPECT_TRUE(K.isApprox(tet.getNonRotatedStiffnessMatrix())) << std::endl << "K:" << std::endl <<
+ K << std::endl << "NonRotatedStiffnessMatrix:" << std::endl <<
+ tet.getNonRotatedStiffnessMatrix() << std::endl;
+ EXPECT_TRUE(M.isApprox(tet.getNonRotatedMassMatrix()));
}
{
@@ -228,9 +264,10 @@ TEST_F(Fem3DElementCorotationalTetrahedronTests, UpdateTest)
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()));
+ ASSERT_NO_THROW(tet.computeRotationMassAndStiffness(m_state, &R, &M, &K));
+ EXPECT_TRUE(R.isIdentity());
+ EXPECT_TRUE(K.isApprox(tet.getNonRotatedStiffnessMatrix()));
+ EXPECT_TRUE(M.isApprox(tet.getNonRotatedMassMatrix()));
}
{
@@ -242,10 +279,18 @@ TEST_F(Fem3DElementCorotationalTetrahedronTests, UpdateTest)
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()));
+ ASSERT_NO_THROW(tet.computeRotationMassAndStiffness(m_state, &R, &M, &K));
+ EXPECT_TRUE(R.isApprox(m_rotation));
+ // The corotational stiffness has more terms than R.K0.R^t
+ // But, these terms are in the order of epsilon is no scaling is involved in the transformation.
+ // They seem to account for the deformation part of the transformation and not the rigid transformation.
+ // A pure transformation (rotation + translation) is fully taken into account by the term R.K0.R^t
+ EXPECT_TRUE(K.isApprox(m_R12x12 * tet.getNonRotatedStiffnessMatrix() * m_R12x12.transpose()))
+ << std::endl << "K:" << std::endl <<
+ K << std::endl << "NonRotatedStiffnessMatrix:" << std::endl <<
+ (m_R12x12 * tet.getNonRotatedStiffnessMatrix() * m_R12x12.transpose()) << std::endl;
+ EXPECT_FALSE(K.isApprox(tet.getNonRotatedStiffnessMatrix()));
+ EXPECT_TRUE(M.isApprox(m_R12x12 * tet.getNonRotatedMassMatrix() * m_R12x12.transpose()));
}
{
@@ -257,86 +302,152 @@ TEST_F(Fem3DElementCorotationalTetrahedronTests, UpdateTest)
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()));
+ ASSERT_NO_THROW(tet.computeRotationMassAndStiffness(m_state, &R, &M, &K));
+ EXPECT_TRUE(R.isApprox(m_rotation));
+ // The corotational stiffness has more terms than R.K0.R^t
+ // But, these terms are in the order of epsilon is no scaling is involved in the transformation.
+ // They seem to account for the deformation part of the transformation and not the rigid transformation.
+ // A pure transformation (rotation + translation) is fully taken into account by the term R.K0.R^t
+ EXPECT_TRUE(K.isApprox(m_R12x12 * tet.getNonRotatedStiffnessMatrix() * m_R12x12.transpose()));
+ EXPECT_FALSE(K.isApprox(tet.getNonRotatedStiffnessMatrix()));
+ EXPECT_TRUE(M.isApprox(m_R12x12 * tet.getNonRotatedMassMatrix() * 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.linear() = m_rotation * 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));
+ ASSERT_NO_THROW(tet.computeRotationMassAndStiffness(m_state, &R, &M, &K));
+ EXPECT_TRUE(R.isApprox(m_rotation));
+ // The corotational stiffness has more terms than R.K0.R^t
+ // But, these terms are in the order of epsilon is no scaling is involved in the transformation.
+ // They seem to account for the deformation part of the transformation and not the rigid transformation.
+ // A pure transformation (rotation + translation) is fully taken into account by the term R.K0.R^t
+ EXPECT_FALSE(K.isApprox(m_R12x12 * tet.getNonRotatedStiffnessMatrix() * m_R12x12.transpose()));
+ EXPECT_FALSE(K.isApprox(tet.getNonRotatedStiffnessMatrix()));
+ EXPECT_TRUE(M.isApprox(m_R12x12 * tet.getNonRotatedMassMatrix() * m_R12x12.transpose()));
}
}
namespace
{
void testAddStiffness(MockFem3DElementCorotationalTet* tet,
- const SurgSim::Math::OdeState& state0,
- const SurgSim::Math::RigidTransform3d& t,
- double scale)
+ 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));
+ tet->updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_K);
+
+ Matrix expectedK = Matrix::Zero(state.getNumDof(), state.getNumDof());
+ SurgSim::Math::addSubMatrix(scale * tet->getRotatedStiffnessMatrix(state), tet->getNodeIds(), 3, &expectedK);
+
+ SparseMatrix K(static_cast<SparseMatrix::Index>(state.getNumDof()),
+ static_cast<SparseMatrix::Index>(state.getNumDof()));
+ K.setZero();
+ Matrix zeroMatrix = Matrix::Zero(tet->getNumDofPerNode() * tet->getNumNodes(),
+ tet->getNumDofPerNode() * tet->getNumNodes());
+ tet->assembleMatrixBlocks(zeroMatrix, tet->getNodeIds(),
+ static_cast<SparseMatrix::Index>(tet->getNumDofPerNode()), &K, true);
+ K.makeCompressed();
+ tet->addStiffness(&K, scale);
- Matrix expectedK = Matrix::Zero(state0.getNumDof(), state0.getNumDof());
- SurgSim::Math::addSubMatrix(scale * tet->getRotatedStiffness(), tet->getNodeIds(), 3, &expectedK);
+ EXPECT_TRUE(K.isApprox(expectedK));
+}
+
+void testAddMass(MockFem3DElementCorotationalTet* tet,
+ const SurgSim::Math::OdeState& state0,
+ const SurgSim::Math::RigidTransform3d& t,
+ double scale)
+{
+ SurgSim::Math::OdeState state;
+
+ defineCurrentState(state0, &state, t, false);
+ tet->updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_M);
+
+ Eigen::Matrix<double, 12, 12> M0 = tet->getNonRotatedMassMatrix();
+ Matrix33d R = tet->getRotation(state);
+ Eigen::Matrix<double, 12, 12> R12x12 = make12x12(Eigen::Matrix<double, 3, 3>(R));
+
+ Matrix expectedM = Matrix::Zero(state.getNumDof(), state.getNumDof());
+ SurgSim::Math::addSubMatrix(scale * R12x12 * M0 * R12x12.transpose(), tet->getNodeIds(), 3, &expectedM);
+
+ SparseMatrix M(static_cast<SparseMatrix::Index>(state.getNumDof()),
+ static_cast<SparseMatrix::Index>(state.getNumDof()));
+ M.setZero();
+ Matrix zeroMatrix = Matrix::Zero(tet->getNumDofPerNode() * tet->getNumNodes(),
+ tet->getNumDofPerNode() * tet->getNumNodes());
+ tet->assembleMatrixBlocks(zeroMatrix, tet->getNodeIds(),
+ static_cast<SparseMatrix::Index>(tet->getNumDofPerNode()), &M, true);
+ M.makeCompressed();
+ tet->addMass(&M, scale);
+
+ EXPECT_TRUE(M.isApprox(expectedM));
+}
+
+void testAddFMDK(MockFem3DElementCorotationalTet* tet,
+ const SurgSim::Math::OdeState& state0,
+ const SurgSim::Math::RigidTransform3d& t)
+{
+ SurgSim::Math::OdeState state;
- Matrix K = Matrix::Zero(state0.getNumDof(), state0.getNumDof());
- tet->addStiffness(state, &K, scale);
+ defineCurrentState(state0, &state, t, false);
+ tet->updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_FMDK);
+
+ Eigen::Matrix<double, 12, 12> K0 = tet->getNonRotatedStiffnessMatrix();
+ Eigen::Matrix<double, 12, 12> M0 = tet->getNonRotatedMassMatrix();
+ Matrix33d R = tet->getRotation(state);
+ Eigen::Matrix<double, 12, 12> R12x12 = make12x12(Eigen::Matrix<double, 3, 3>(R));
+
+ Vector expectedF = Vector::Zero(state.getNumDof());
+ Matrix expectedM = Matrix::Zero(state.getNumDof(), state.getNumDof());
+ Matrix expectedK = Matrix::Zero(state.getNumDof(), state.getNumDof());
+ SurgSim::Math::addSubMatrix(tet->getRotatedStiffnessMatrix(state), tet->getNodeIds(), 3, &expectedK);
+ SurgSim::Math::addSubMatrix(R12x12 * M0 * R12x12.transpose(), tet->getNodeIds(), 3, &expectedM);
+
+ Eigen::Matrix<double, 12, 1> x;
+ SurgSim::Math::getSubVector(state.getPositions(), tet->getNodeIds(), 3, &x);
+ Eigen::Matrix<double, 12 , 1> f = - R12x12 * K0 * R12x12.transpose() * (x - (R12x12 * tet->getInitialPosition()));
+ SurgSim::Math::addSubVector(f, tet->getNodeIds(), 3, &expectedF);
+ Vector F = Vector::Zero(state.getNumDof());
+ SparseMatrix M(static_cast<SparseMatrix::Index>(state.getNumDof()),
+ static_cast<SparseMatrix::Index>(state.getNumDof()));
+ SparseMatrix D(static_cast<SparseMatrix::Index>(state.getNumDof()),
+ static_cast<SparseMatrix::Index>(state.getNumDof()));
+ SparseMatrix K(static_cast<SparseMatrix::Index>(state.getNumDof()),
+ static_cast<SparseMatrix::Index>(state.getNumDof()));
+ SparseMatrix zeroMatrix(static_cast<SparseMatrix::Index>(state.getNumDof()),
+ static_cast<SparseMatrix::Index>(state.getNumDof()));
+ Matrix zeroElementMatrix = Matrix::Zero(tet->getNumDofPerNode() * tet->getNumNodes(),
+ tet->getNumDofPerNode() * tet->getNumNodes());
+ M.setZero();
+ tet->assembleMatrixBlocks(zeroElementMatrix, tet->getNodeIds(),
+ static_cast<SparseMatrix::Index>(tet->getNumDofPerNode()), &M, true);
+ M.makeCompressed();
+ D.setZero();
+ tet->assembleMatrixBlocks(zeroElementMatrix, tet->getNodeIds(),
+ static_cast<SparseMatrix::Index>(tet->getNumDofPerNode()), &D, true);
+ D.makeCompressed();
+ K.setZero();
+ tet->assembleMatrixBlocks(zeroElementMatrix, tet->getNodeIds(),
+ static_cast<SparseMatrix::Index>(tet->getNumDofPerNode()), &K, true);
+ K.makeCompressed();
+ zeroMatrix.setZero();
+
+ tet->addFMDK(&F, &M, &D, &K);
+
+ EXPECT_TRUE(F.isApprox(expectedF));
+ EXPECT_TRUE(M.isApprox(expectedM));
+ EXPECT_TRUE(D.isApprox(zeroMatrix));
EXPECT_TRUE(K.isApprox(expectedK));
}
}; // anonymous namespace
@@ -370,6 +481,54 @@ TEST_F(Fem3DElementCorotationalTetrahedronTests, AddStiffnessTest)
}
}
+TEST_F(Fem3DElementCorotationalTetrahedronTests, AddMassTest)
+{
+ 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");
+ testAddMass(&tet, m_restState, RigidTransform3d::Identity(), 1.0);
+ }
+
+ {
+ SCOPED_TRACE("Without rotation, scale 0.4");
+ testAddMass(&tet, m_restState, RigidTransform3d::Identity(), 0.4);
+ }
+
+ {
+ SCOPED_TRACE("With rotation, scale 1.0");
+ testAddMass(&tet, m_restState, makeRigidTransform(m_rotation, Vector3d::Zero()), 1.0);
+ }
+
+ {
+ SCOPED_TRACE("With rotation, scale 0.4");
+ testAddMass(&tet, m_restState, makeRigidTransform(m_rotation, Vector3d::Zero()), 0.4);
+ }
+}
+
+TEST_F(Fem3DElementCorotationalTetrahedronTests, AddFMDKTest)
+{
+ 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");
+ testAddFMDK(&tet, m_restState, RigidTransform3d::Identity());
+ }
+
+ {
+ SCOPED_TRACE("With rotation");
+ testAddFMDK(&tet, m_restState, makeRigidTransform(m_rotation, Vector3d::Zero()));
+ }
+}
+
namespace
{
void testAddForce(MockFem3DElementCorotationalTet* tet,
@@ -378,18 +537,19 @@ void testAddForce(MockFem3DElementCorotationalTet* tet,
bool addLocalDeformation)
{
SurgSim::Math::OdeState statet;
- SurgSim::Math::Matrix K = tet->getNonRotatedStiffness();
+ SurgSim::Math::Matrix K = tet->getNonRotatedStiffnessMatrix();
// 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));
+ tet->updateFMDK(statet, SurgSim::Math::ODEEQUATIONUPDATE_F);
+
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();
+ SurgSim::Math::Matrix33d R = tet->getRotation(statet);
Eigen::Matrix<double, 12, 12> R12x12 = Eigen::Matrix<double, 12, 12>::Zero();
for (size_t nodeId = 0; nodeId < 4; ++nodeId)
{
@@ -397,20 +557,18 @@ void testAddForce(MockFem3DElementCorotationalTet* tet,
}
SurgSim::Math::Vector expectedF;
- expectedF.resize(statet.getNumDof());
- expectedF.setZero();
- Eigen::Matrix<double, 12 ,1> f = - R12x12 * K * R12x12.transpose() * (x - (R12x12 * x0));
+ expectedF.setZero(statet.getNumDof());
+ 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->getRotation(statet).isApprox(R));
+ EXPECT_TRUE(tet->getNonRotatedStiffnessMatrix().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);
+ F.setZero(statet.getNumDof());
+ tet->addForce(&F);
EXPECT_LT((F - expectedF).norm(), epsilonAddForce);
if (!addLocalDeformation)
{
@@ -421,9 +579,8 @@ void testAddForce(MockFem3DElementCorotationalTet* tet,
{
SCOPED_TRACE("Scale 0.4");
SurgSim::Math::Vector F;
- F.resize(statet.getNumDof());
- F.setZero();
- tet->addForce(statet, &F, 0.4);
+ F.setZero(statet.getNumDof());
+ tet->addForce(&F, 0.4);
EXPECT_LT((F - 0.4 * expectedF).norm(), epsilonAddForce);
if (!addLocalDeformation)
{
@@ -465,6 +622,8 @@ TEST_F(Fem3DElementCorotationalTetrahedronTests, AddForceTest)
TEST_F(Fem3DElementCorotationalTetrahedronTests, AddMatVecTest)
{
+ using SurgSim::Math::OdeEquationUpdate;
+
MockFem3DElementCorotationalTet tet(m_nodeIds);
tet.setupInitialParams(m_restState, m_rho, m_nu, m_E);
@@ -474,10 +633,11 @@ TEST_F(Fem3DElementCorotationalTetrahedronTests, AddMatVecTest)
SurgSim::Math::OdeState state;
defineCurrentState(m_restState, &state, transformation, true);
- ASSERT_NO_THROW(tet.update(state));
+ tet.updateFMDK(state, OdeEquationUpdate::ODEEQUATIONUPDATE_M | OdeEquationUpdate::ODEEQUATIONUPDATE_D
+ | OdeEquationUpdate::ODEEQUATIONUPDATE_K);
- Eigen::Matrix<double, 12, 12> M = tet.getMassMatrix();
- Eigen::Matrix<double, 12, 12> K = tet.getRotatedStiffness();
+ SurgSim::Math::Matrix M = tet.getRotatedMassMatrix(state);
+ SurgSim::Math::Matrix K = tet.getRotatedStiffnessMatrix(state);
SurgSim::Math::Vector ones = SurgSim::Math::Vector::Ones(state.getNumDof());
@@ -485,7 +645,7 @@ TEST_F(Fem3DElementCorotationalTetrahedronTests, AddMatVecTest)
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);
+ tet.addMatVec(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);
@@ -498,7 +658,7 @@ TEST_F(Fem3DElementCorotationalTetrahedronTests, AddMatVecTest)
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);
+ tet.addMatVec(0.0, 1.5, 0.0, ones, &result);
EXPECT_TRUE(result.isZero());
}
@@ -507,7 +667,7 @@ TEST_F(Fem3DElementCorotationalTetrahedronTests, AddMatVecTest)
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);
+ tet.addMatVec(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);
@@ -520,7 +680,7 @@ TEST_F(Fem3DElementCorotationalTetrahedronTests, AddMatVecTest)
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);
+ tet.addMatVec(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);
diff --git a/SurgSim/Physics/UnitTests/Fem3DElementCubeTests.cpp b/SurgSim/Physics/UnitTests/Fem3DElementCubeTests.cpp
index 977f2f0..01951d3 100644
--- a/SurgSim/Physics/UnitTests/Fem3DElementCubeTests.cpp
+++ b/SurgSim/Physics/UnitTests/Fem3DElementCubeTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -20,11 +20,14 @@
#include "SurgSim/Framework/Assert.h"
#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeEquation.h"
#include "SurgSim/Math/OdeState.h"
#include "SurgSim/Math/Vector.h"
#include "SurgSim/Physics/Fem3DElementCube.h"
+using SurgSim::Math::clearMatrix;
using SurgSim::Math::Matrix;
+using SurgSim::Math::SparseMatrix;
using SurgSim::Math::Vector;
using SurgSim::Math::Vector3d;
using SurgSim::Physics::Fem3DElementCube;
@@ -32,13 +35,13 @@ using SurgSim::Physics::Fem3DElementCube;
namespace
{
/// Epsilon used in this unit test, resulting from a trial and error test.
-const double epsilon = 2.6e-9;
+const double epsilon = 3.6e-9;
};
class MockFem3DElementCube : public Fem3DElementCube
{
public:
- MockFem3DElementCube(std::array<size_t, 8> nodeIds) : Fem3DElementCube(nodeIds)
+ explicit MockFem3DElementCube(std::array<size_t, 8> nodeIds) : Fem3DElementCube(nodeIds)
{
}
@@ -101,8 +104,7 @@ public:
using SurgSim::Math::getSubMatrix;
using SurgSim::Math::addSubMatrix;
- Eigen::Matrix<double, 24, 24> K;
- K.setZero();
+ Eigen::Matrix<double, 24, 24> K = Eigen::Matrix<double, 24, 24>::Zero();
{
// Expected stiffness matrix given in
// "Physically-Based Simulation of Objects Represented by Surface Meshes"
@@ -122,37 +124,37 @@ public:
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, 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, 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, 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, 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, 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;
+ getSubMatrix(K, 7, 7, 3, 3)(1, 2) = e;
+ getSubMatrix(K, 7, 7, 3, 3)(2, 1) = e;
// Edges
{
@@ -373,8 +375,7 @@ public:
{
using SurgSim::Math::addSubMatrix;
- Eigen::Matrix<double, 24, 24> M;
- M.setZero();
+ Eigen::Matrix<double, 24, 24> M = Eigen::Matrix<double, 24, 24>::Zero();
// "Physically-Based Simulation of Objects Represented by Surface Meshes"
// Muller, Techner, Gross, CGI 2004
@@ -391,14 +392,14 @@ public:
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(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(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(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);
@@ -411,7 +412,7 @@ public:
// Symmetry
for (size_t row = 0; row < 24; ++row)
{
- for (size_t col = row+1; col < 24; ++col)
+ for (size_t col = row + 1; col < 24; ++col)
{
M(col, row) = M(row, col);
}
@@ -421,11 +422,10 @@ public:
addSubMatrix(M, nodeIdsVectorForm, 3 , &m_expectedMassMatrix);
}
- virtual void SetUp() override
+ 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();
@@ -438,14 +438,14 @@ public:
// | 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, 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);
+ 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"
@@ -469,26 +469,21 @@ public:
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);
+ m_expectedMassMatrix.setZero(3 * 8, 3 * 8);
+ m_expectedDampingMatrix.setZero(3 * 8, 3 * 8);
+ m_expectedStiffnessMatrix.setZero(3 * 8, 3 * 8);
+ m_vectorOnes.setOnes(3 * 8);
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)
+ 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}};
@@ -527,7 +522,8 @@ 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);});
+ std::make_shared<MockFem3DElementCube>(m_nodeIds);
+ });
}
TEST_F(Fem3DElementCubeTests, InitializeTest)
@@ -578,7 +574,7 @@ TEST_F(Fem3DElementCubeTests, VolumeTest)
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);
+ ASSERT_THROW(cube->initialize(m_restState), SurgSim::Framework::AssertionFailure);
}
{
@@ -587,7 +583,7 @@ TEST_F(Fem3DElementCubeTests, VolumeTest)
// 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);
+ ASSERT_THROW(cube->initialize(m_restState), SurgSim::Framework::AssertionFailure);
}
}
@@ -659,7 +655,8 @@ TEST_F(Fem3DElementCubeTests, ShapeFunctionsTest)
cube->initialize(m_restState);
EXPECT_TRUE(cube->getInitialPosition().isApprox(m_expectedX0)) <<
- "x0 = " << cube->getInitialPosition().transpose() << std::endl << "x0 expected = " << m_expectedX0.transpose();
+ "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
@@ -689,66 +686,90 @@ TEST_F(Fem3DElementCubeTests, ShapeFunctionsTest)
EXPECT_NEAR(Ni_p0[0], 1.0, 1e-12);
for (size_t i = 0; i < 8; ++i)
{
- if (i == 0) continue;
+ 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;
+ 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;
+ 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;
+ 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;
+ 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;
+ 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;
+ 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;
+ 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 epsilon = -1.0; epsilon <= 1.0; epsilon += 0.1)
{
- for (double eta = -1.0; eta <= 1.0; eta+=0.1)
+ for (double eta = -1.0; eta <= 1.0; eta += 0.1)
{
- for (double mu = -1.0; mu <= 1.0; mu+=0.1)
+ for (double mu = -1.0; mu <= 1.0; mu += 0.1)
{
double Ni_p[8];
double sum = 0.0;
@@ -758,11 +779,16 @@ TEST_F(Fem3DElementCubeTests, ShapeFunctionsTest)
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];
+ " 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];
}
}
}
@@ -811,7 +837,7 @@ TEST_F(Fem3DElementCubeTests, CoordinateTests)
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));
+ cube->computeCartesianCoordinate(m_restState, nodePositions), epsilon));
}
{
@@ -819,7 +845,7 @@ TEST_F(Fem3DElementCubeTests, CoordinateTests)
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));
+ 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)
@@ -843,39 +869,51 @@ TEST_F(Fem3DElementCubeTests, ForceAndMatricesTest)
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();
+ Vector forceVector = Vector::Zero(3 * 8);
+ SparseMatrix massMatrix(3 * 8, 3 * 8);
+ SparseMatrix dampingMatrix(3 * 8, 3 * 8);
+ SparseMatrix stiffnessMatrix(3 * 8, 3 * 8);
+ Matrix zeroMatrix = Matrix::Zero(cube->getNumDofPerNode() * cube->getNumNodes(),
+ cube->getNumDofPerNode() * cube->getNumNodes());
massMatrix.setZero();
+ cube->assembleMatrixBlocks(zeroMatrix, cube->getNodeIds(),
+ static_cast<SparseMatrix::Index>(cube->getNumDofPerNode()), &massMatrix, true);
+ massMatrix.makeCompressed();
dampingMatrix.setZero();
+ cube->assembleMatrixBlocks(zeroMatrix, cube->getNodeIds(),
+ static_cast<SparseMatrix::Index>(cube->getNumDofPerNode()), &dampingMatrix, true);
+ dampingMatrix.makeCompressed();
stiffnessMatrix.setZero();
+ cube->assembleMatrixBlocks(zeroMatrix, cube->getNodeIds(),
+ static_cast<SparseMatrix::Index>(cube->getNumDofPerNode()), &stiffnessMatrix, true);
+ stiffnessMatrix.makeCompressed();
+
+ // Update the internal f, M, D, K variables.
+ cube->updateFMDK(m_restState, SurgSim::Math::ODEEQUATIONUPDATE_FMDK);
// No force should be produced when in rest state (x = x0) => F = K.(x-x0) = 0
- cube->addForce(m_restState, &forceVector);
+ cube->addForce(&forceVector);
EXPECT_TRUE(forceVector.isZero());
- cube->addMass(m_restState, &massMatrix);
+ cube->addMass(&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;
+ "Expected mass matrix :" << std::endl << m_expectedMassMatrix << std::endl << std::endl <<
+ "Mass matrix :" << std::endl << massMatrix << std::endl;
- cube->addDamping(m_restState, &dampingMatrix);
+ cube->addDamping(&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;
+ cube->addStiffness(&stiffnessMatrix);
+ EXPECT_TRUE(stiffnessMatrix.isApprox(m_expectedStiffnessMatrix)) <<
+ "Expected stiffness matrix :" << std::endl << m_expectedStiffnessMatrix << std::endl << std::endl <<
+ "Stiffness matrix :" << std::endl << stiffnessMatrix << std::endl;
forceVector.setZero();
- massMatrix.setZero();
- dampingMatrix.setZero();
- stiffnessMatrix.setZero();
+ clearMatrix(&massMatrix);
+ clearMatrix(&dampingMatrix);
+ clearMatrix(&stiffnessMatrix);
- cube->addFMDK(m_restState, &forceVector, &massMatrix, &dampingMatrix, &stiffnessMatrix);
+ cube->addFMDK(&forceVector, &massMatrix, &dampingMatrix, &stiffnessMatrix);
EXPECT_TRUE(forceVector.isZero());
EXPECT_TRUE(massMatrix.isApprox(m_expectedMassMatrix));
EXPECT_TRUE(dampingMatrix.isApprox(m_expectedDampingMatrix));
@@ -883,33 +921,33 @@ TEST_F(Fem3DElementCubeTests, ForceAndMatricesTest)
// Test addMatVec API with Mass component only
forceVector.setZero();
- cube->addMatVec(m_restState, 1.0, 0.0, 0.0, m_vectorOnes, &forceVector);
+ cube->addMatVec(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);
+ cube->addMatVec(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);
+ cube->addMatVec(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);
+ cube->addMatVec(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();
+ 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
index badf494..371f9f1 100644
--- a/SurgSim/Physics/UnitTests/Fem3DElementTetrahedronTests.cpp
+++ b/SurgSim/Physics/UnitTests/Fem3DElementTetrahedronTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -20,14 +20,17 @@
#include "SurgSim/Framework/Assert.h"
#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Math/OdeEquation.h"
#include "SurgSim/Math/OdeState.h"
#include "SurgSim/Math/Vector.h"
#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
using SurgSim::Physics::Fem3DElementTetrahedron;
+using SurgSim::Math::clearMatrix;
using SurgSim::Math::Vector3d;
using SurgSim::Math::Vector;
using SurgSim::Math::Matrix;
+using SurgSim::Math::SparseMatrix;
namespace
{
@@ -36,19 +39,19 @@ namespace
/// \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 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;
+const double epsilon = 1e-8;
};
class MockFem3DElementTet : public Fem3DElementTetrahedron
{
public:
- MockFem3DElementTet(std::array<size_t, 4> nodeIds) : Fem3DElementTetrahedron(nodeIds)
+ explicit MockFem3DElementTet(std::array<size_t, 4> nodeIds) : Fem3DElementTetrahedron(nodeIds)
{
}
@@ -57,7 +60,7 @@ public:
return m_restVolume;
}
- void getShapeFunction(int i, double *ai, double *bi, double *ci, double *di) const
+ void getShapeFunction(int i, double* ai, double* bi, double* ci, double* di) const
{
*ai = m_ai[i];
*bi = m_bi[i];
@@ -70,7 +73,7 @@ public:
return m_x0;
}
- void setupInitialParams(const SurgSim::Math::OdeState &state,
+ void setupInitialParams(const SurgSim::Math::OdeState& state,
double massDensity,
double poissonRatio,
double youngModulus)
@@ -94,7 +97,7 @@ public:
SurgSim::Math::Matrix m_expectedStiffnessMatrix, m_expectedStiffnessMatrix2;
SurgSim::Math::Vector m_vectorOnes;
- virtual void SetUp() override
+ void SetUp() override
{
using SurgSim::Math::getSubVector;
using SurgSim::Math::getSubMatrix;
@@ -125,19 +128,13 @@ public:
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_expectedMassMatrix.setZero(3 * 15, 3 * 15);
+ m_expectedDampingMatrix.setZero(3 * 15, 3 * 15);
+ m_expectedStiffnessMatrix.setZero(3 * 15, 3 * 15);
+ m_expectedStiffnessMatrix2.setZero(3 * 15, 3 * 15);
+ m_vectorOnes.setOnes(3 * 15);
+
+ Eigen::Matrix<double, 12, 12> M = Eigen::Matrix<double, 12, 12>::Zero();
{
M.diagonal().setConstant(2.0);
M.block(0, 3, 9, 9).diagonal().setConstant(1.0);
@@ -150,10 +147,7 @@ public:
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();
+ Eigen::Matrix<double, 12, 12> K = Eigen::Matrix<double, 12, 12>::Zero();
{
// Calculation done by hand from
// http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch09.d/AFEM.Ch09.pdf
@@ -161,23 +155,33 @@ public:
// 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;
+ Eigen::Matrix<double, 6, 12> B = Eigen::Matrix<double, 6, 12>::Zero();
+ Eigen::Matrix<double, 6, 6> E = Eigen::Matrix<double, 6, 6>::Zero();
+
+ 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));
+ E *= m_E / ((1.0 + m_nu) * (1.0 - 2.0 * m_nu));
K = m_expectedVolume * B.transpose() * E * B;
}
@@ -185,7 +189,7 @@ public:
// 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 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();
@@ -193,17 +197,29 @@ public:
// 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(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;
@@ -213,8 +229,8 @@ public:
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.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;
@@ -231,7 +247,8 @@ 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);});
+ std::make_shared<MockFem3DElementTet>(m_nodeIds);
+ });
}
TEST_F(Fem3DElementTetrahedronTests, NodeIdsTest)
@@ -294,15 +311,15 @@ TEST_F(Fem3DElementTetrahedronTests, CoordinateTests)
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);
+ SurgSim::Framework::AssertionFailure);
EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateNegativeValue), \
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSize3), \
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSize5), \
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
EXPECT_THROW(ptA = element.computeCartesianCoordinate(m_restState, invalidNaturalCoordinateSumNot1), \
- SurgSim::Framework::AssertionFailure);
+ 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));
@@ -352,7 +369,8 @@ TEST_F(Fem3DElementTetrahedronTests, ShapeFunctionsTest)
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();
+ "x0 = " << tet.getInitialPosition().transpose() << std::endl << "x0 expected = " <<
+ m_expectedX0.transpose();
double ai[4], bi[4], ci[4], di[4];
double sumAi = 0.0;
@@ -404,11 +422,11 @@ TEST_F(Fem3DElementTetrahedronTests, ShapeFunctionsTest)
// 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 sp0p1 = 0; sp0p1 <= 1.0; sp0p1 += 0.1)
{
- for (double sp0p2 = 0; sp0p1 + sp0p2 <= 1.0; sp0p2+=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)
+ 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];
@@ -417,9 +435,9 @@ TEST_F(Fem3DElementTetrahedronTests, ShapeFunctionsTest)
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];
+ " 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];
}
}
}
@@ -462,39 +480,51 @@ TEST_F(Fem3DElementTetrahedronTests, ForceAndMatricesTest)
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();
+ Vector forceVector = Vector::Zero(3 * 15);
+ SparseMatrix massMatrix(3 * 15, 3 * 15);
+ SparseMatrix dampingMatrix(3 * 15, 3 * 15);
+ SparseMatrix stiffnessMatrix(3 * 15, 3 * 15);
+ Matrix zeroMatrix = Matrix::Zero(tet.getNumDofPerNode() * tet.getNumNodes(),
+ tet.getNumDofPerNode() * tet.getNumNodes());
massMatrix.setZero();
+ tet.assembleMatrixBlocks(zeroMatrix, tet.getNodeIds(),
+ static_cast<SparseMatrix::Index>(tet.getNumDofPerNode()), &massMatrix, true);
+ massMatrix.makeCompressed();
dampingMatrix.setZero();
+ tet.assembleMatrixBlocks(zeroMatrix, tet.getNodeIds(),
+ static_cast<SparseMatrix::Index>(tet.getNumDofPerNode()), &dampingMatrix, true);
+ dampingMatrix.makeCompressed();
stiffnessMatrix.setZero();
+ tet.assembleMatrixBlocks(zeroMatrix, tet.getNodeIds(),
+ static_cast<SparseMatrix::Index>(tet.getNumDofPerNode()), &stiffnessMatrix, true);
+ stiffnessMatrix.makeCompressed();
// Make sure that the 2 ways of computing the expected stiffness matrix gives the same result
EXPECT_TRUE(m_expectedStiffnessMatrix.isApprox(m_expectedStiffnessMatrix2));
+ // Update the internal f, M, D, K variables.
+ tet.updateFMDK(m_restState, SurgSim::Math::ODEEQUATIONUPDATE_FMDK);
+
// No force should be produced when in rest state (x = x0) => F = K.(x-x0) = 0
- tet.addForce(m_restState, &forceVector);
+ tet.addForce(&forceVector);
EXPECT_TRUE(forceVector.isZero());
- tet.addMass(m_restState, &massMatrix);
+ tet.addMass(&massMatrix);
EXPECT_TRUE(massMatrix.isApprox(m_expectedMassMatrix));
- tet.addDamping(m_restState, &dampingMatrix);
+ tet.addDamping(&dampingMatrix);
EXPECT_TRUE(dampingMatrix.isApprox(m_expectedDampingMatrix));
- tet.addStiffness(m_restState, &stiffnessMatrix);
+ tet.addStiffness(&stiffnessMatrix);
EXPECT_TRUE(stiffnessMatrix.isApprox(m_expectedStiffnessMatrix));
EXPECT_TRUE(stiffnessMatrix.isApprox(m_expectedStiffnessMatrix2));
forceVector.setZero();
- massMatrix.setZero();
- dampingMatrix.setZero();
- stiffnessMatrix.setZero();
+ clearMatrix(&massMatrix);
+ clearMatrix(&dampingMatrix);
+ clearMatrix(&stiffnessMatrix);
- tet.addFMDK(m_restState, &forceVector, &massMatrix, &dampingMatrix, &stiffnessMatrix);
+ tet.addFMDK(&forceVector, &massMatrix, &dampingMatrix, &stiffnessMatrix);
EXPECT_TRUE(forceVector.isZero());
EXPECT_TRUE(massMatrix.isApprox(m_expectedMassMatrix));
EXPECT_TRUE(dampingMatrix.isApprox(m_expectedDampingMatrix));
@@ -503,33 +533,33 @@ TEST_F(Fem3DElementTetrahedronTests, ForceAndMatricesTest)
// Test addMatVec API with Mass component only
forceVector.setZero();
- tet.addMatVec(m_restState, 1.0, 0.0, 0.0, m_vectorOnes, &forceVector);
+ tet.addMatVec(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);
+ tet.addMatVec(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);
+ tet.addMatVec(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);
+ tet.addMatVec(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();
+ 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/Fem3DLocalizationTest.cpp b/SurgSim/Physics/UnitTests/Fem3DLocalizationTest.cpp
new file mode 100644
index 0000000..2bcdb3d
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/Fem3DLocalizationTest.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.
+
+#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/Fem2DRepresentation.h"
+#include "SurgSim/Physics/Fem3DElementCube.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
+#include "SurgSim/Physics/Fem3DLocalization.h"
+#include "SurgSim/Physics/Fem3DRepresentation.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 Fem3DLocalizationTest : 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_invalidIndexLocalPosition.coordinate = SurgSim::Math::Vector::Zero(4);
+ m_invalidIndexLocalPosition.coordinate[0] = 0.4;
+ m_invalidIndexLocalPosition.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(Fem3DLocalizationTest, ConstructorTest)
+{
+ // IndexedLocalCoordinate pointing to a node (node index + empty coordinate) or a triangle
+ // (triangleID + coordinate of size 2) are invalid. Both will failed, either because the index is out of bound,
+ // or because the coordinates are the wrong size (empty or of size 2 do not correspond to any valid 3d FemElements)
+ // This is tested by m_invalidIndexLocalPosition and m_invalidCoordinateLocalPosition
+ ASSERT_THROW(std::make_shared<Fem3DLocalization>(m_fem, m_invalidIndexLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+
+ ASSERT_THROW(std::make_shared<Fem3DLocalization>(m_fem, m_invalidCoordinateLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+
+ ASSERT_NO_THROW(std::make_shared<Fem3DLocalization>(m_fem, m_validLocalPosition););
+
+ ASSERT_NO_THROW(std::make_shared<Fem3DLocalization>(m_fem3DCube, m_validLocalPositionForCube););
+}
+
+TEST_F(Fem3DLocalizationTest, IsValidRepresentation)
+{
+ Fem3DLocalization localization(m_fem, m_validLocalPosition);
+
+ ASSERT_TRUE(localization.isValidRepresentation(m_fem));
+
+ // nullptr is valid
+ ASSERT_TRUE(localization.isValidRepresentation(nullptr));
+
+ ASSERT_FALSE(localization.isValidRepresentation(std::make_shared<Fem1DRepresentation>("fem1d")));
+ ASSERT_FALSE(localization.isValidRepresentation(std::make_shared<Fem2DRepresentation>("fem2d")));
+}
+
+} // namespace SurgSim
+} // namespace Physics
diff --git a/SurgSim/Physics/UnitTests/Fem3DPlyReaderDelegateTests.cpp b/SurgSim/Physics/UnitTests/Fem3DPlyReaderDelegateTests.cpp
index 156464c..da11973 100644
--- a/SurgSim/Physics/UnitTests/Fem3DPlyReaderDelegateTests.cpp
+++ b/SurgSim/Physics/UnitTests/Fem3DPlyReaderDelegateTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -17,25 +17,15 @@
#include "SurgSim/DataStructures/PlyReader.h"
#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/Runtime.h"
#include "SurgSim/Math/OdeState.h"
#include "SurgSim/Math/Vector.h"
-#include "SurgSim/Physics/FemElement.h"
+#include "SurgSim/Physics/Fem.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
@@ -43,115 +33,133 @@ 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);
+ auto fem = std::make_shared<Fem3D>();
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
- ASSERT_TRUE(reader.parseWithDelegate(delegate));
+ fem->load("PlyReaderTests/Tetrahedron.ply");
// 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)));
+ EXPECT_TRUE(vertex0.isApprox(fem->getVertex(0).position));
+ EXPECT_TRUE(vertex25.isApprox(fem->getVertex(25).position));
// Tetrahedrons
- ASSERT_EQ(12u, fem->getNumFemElements());
+ ASSERT_EQ(12u, fem->getNumElements());
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())));
+ std::begin(fem->getElement(0)->nodeIds)));
EXPECT_TRUE(std::equal(std::begin(tetrahedron2), std::end(tetrahedron2),
- std::begin(fem->getFemElement(11)->getNodeIds())));
+ std::begin(fem->getElement(11)->nodeIds)));
// 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));
+ ASSERT_EQ(8u, fem->getBoundaryConditions().size());
+
+ EXPECT_EQ(8, fem->getBoundaryCondition(0));
+ EXPECT_EQ(5, fem->getBoundaryCondition(1));
+ EXPECT_EQ(3, fem->getBoundaryCondition(2));
+ EXPECT_EQ(2, fem->getBoundaryCondition(3));
+ EXPECT_EQ(7, fem->getBoundaryCondition(4));
+ EXPECT_EQ(1, fem->getBoundaryCondition(5));
+ EXPECT_EQ(6, fem->getBoundaryCondition(6));
+ EXPECT_EQ(11, fem->getBoundaryCondition(7));
+
// 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());
+ auto fem2 = fem->getElement(2);
+ EXPECT_DOUBLE_EQ(0.1432, fem2->massDensity);
+ EXPECT_DOUBLE_EQ(0.224, fem2->poissonRatio);
+ EXPECT_DOUBLE_EQ(0.472, fem2->youngModulus);
+
+ auto fem8 = fem->getElement(8);
+ EXPECT_DOUBLE_EQ(0.1432, fem8->massDensity);
+ EXPECT_DOUBLE_EQ(0.224, fem8->poissonRatio);
+ EXPECT_DOUBLE_EQ(0.472, fem8->youngModulus);
}
-TEST(Fem3DRepresentationReaderTests, Fem3DCubePlyReadTest)
+TEST(Fem3DRepresentationReaderTests, CubeMeshDelegateTest)
{
- auto fem = std::make_shared<Fem3DRepresentation>("Representation");
+ auto fem = std::make_shared<Fem3D>();
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
- 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));
+ fem->load("PlyReaderTests/Fem3DCube.ply");
// 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)));
+ EXPECT_TRUE(vertex0.isApprox(fem->getVertex(0).position));
+ EXPECT_TRUE(vertex5.isApprox(fem->getVertex(5).position));
// Cubes
- ASSERT_EQ(3u, fem->getNumFemElements());
+ ASSERT_EQ(3u, fem->getNumElements());
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())));
+ EXPECT_TRUE(std::equal(std::begin(cube0), std::end(cube0), std::begin(fem->getElement(0)->nodeIds)));
+ EXPECT_TRUE(std::equal(std::begin(cube2), std::end(cube2), std::begin(fem->getElement(2)->nodeIds)));
// Boundary conditions
- ASSERT_EQ(3u* 2u, fem->getInitialState()->getNumBoundaryConditions());
+ ASSERT_EQ(2u, fem->getBoundaryConditions().size());
+
+ EXPECT_EQ(9, fem->getBoundaryCondition(0));
+ EXPECT_EQ(5, fem->getBoundaryCondition(1));
- // Boundary condition 0 is on node 9
- size_t boundaryNode0 = 9;
- size_t boundaryNode1 = 5;
+ // Material
+ auto fem2 = fem->getElement(2);
+ EXPECT_DOUBLE_EQ(0.2, fem2->massDensity);
+ EXPECT_DOUBLE_EQ(0.3, fem2->poissonRatio);
+ EXPECT_DOUBLE_EQ(0.4, fem2->youngModulus);
+
+ auto fem1 = fem->getElement(1);
+ EXPECT_DOUBLE_EQ(0.2, fem1->massDensity);
+ EXPECT_DOUBLE_EQ(0.3, fem1->poissonRatio);
+ EXPECT_DOUBLE_EQ(0.4, fem1->youngModulus);
+}
+
+TEST(Fem3DRepresentationReaderTests, WrongPlyWithRotationDof)
+{
+ auto fem = std::make_shared<Fem3D>();
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
- 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));
+ ASSERT_NO_THROW(fem->load("PlyReaderTests/Wrong3DFileWithRotationData.ply"));
+}
+
+TEST(Fem3DRepresentationReaderTests, PerElementMaterial)
+{
+ auto fem = std::make_shared<Fem3D>();
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+
+ fem->load("PlyReaderTests/Fem3DCubeMaterial.ply");
// 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());
+ double value = 1.0;
+ for (size_t i = 0; i < fem->getNumElements(); ++i)
+ {
+ auto element = fem->getElement(i);
+ EXPECT_DOUBLE_EQ(value++, element->massDensity);
+ EXPECT_DOUBLE_EQ(value++, element->poissonRatio);
+ EXPECT_DOUBLE_EQ(value++, element->youngModulus);
+ }
+}
+
+TEST(Fem3DRepresentationReaderTests, NoMaterials)
+{
+ auto fem = std::make_shared<Fem3D>();
+ auto runtime = std::make_shared<Framework::Runtime>("config.txt");
+
+ ASSERT_NO_THROW(fem->load("PlyReaderTests/Fem3DCubeNoMaterial.ply"));
+
+ for (auto element : fem->getElements())
+ {
+ EXPECT_DOUBLE_EQ(0.0, element->massDensity);
+ EXPECT_DOUBLE_EQ(0.0, element->poissonRatio);
+ EXPECT_DOUBLE_EQ(0.0, element->youngModulus);
+ }
}
} // Physics
diff --git a/SurgSim/Physics/UnitTests/Fem3DRepresentationBilateral3DTests.cpp b/SurgSim/Physics/UnitTests/Fem3DRepresentationBilateral3DTests.cpp
deleted file mode 100644
index e7362cb..0000000
--- a/SurgSim/Physics/UnitTests/Fem3DRepresentationBilateral3DTests.cpp
+++ /dev/null
@@ -1,237 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 77335f9..0000000
--- a/SurgSim/Physics/UnitTests/Fem3DRepresentationContactTests.cpp
+++ /dev/null
@@ -1,275 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 38bfec0..0000000
--- a/SurgSim/Physics/UnitTests/Fem3DRepresentationLocalizationTest.cpp
+++ /dev/null
@@ -1,371 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
index 9f9a8fb..0424e98 100644
--- a/SurgSim/Physics/UnitTests/Fem3DRepresentationTests.cpp
+++ b/SurgSim/Physics/UnitTests/Fem3DRepresentationTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -14,25 +14,22 @@
// limitations under the License.
/// \file Fem3DRepresentationTests.cpp
-/// This file tests the non-abstract functionalities of the base class FemRepresentation
+/// This file tests the non-abstract functionalities of the base class fem3DRepresentation
#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/Framework/Runtime.h" ///< Used to initialize the Component Fem3DRepresentation
+#include "SurgSim/Math/MeshShape.h"
#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/Fem3DLocalization.h"
+#include "SurgSim/Physics/Fem3DRepresentation.h"
#include "SurgSim/Physics/UnitTests/MockObjects.h"
namespace SurgSim
@@ -80,7 +77,7 @@ public:
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_localization = std::make_shared<Fem3DLocalization>(m_fem, femRepCoordinate);
m_wrongLocalizationType = std::make_shared<MockLocalization>();
m_wrongLocalizationType->setRepresentation(m_fem);
@@ -91,7 +88,7 @@ protected:
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<Fem3DLocalization> m_localization;
std::shared_ptr<MockLocalization> m_wrongLocalizationType;
};
@@ -100,12 +97,6 @@ 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();
@@ -138,22 +129,15 @@ TEST_F(Fem3DRepresentationTests, TransformInitialStateTest)
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)
{
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
{
SCOPED_TRACE("Initialize with a valid file name");
createFem();
- m_fem->setFilename("PlyReaderTests/Tetrahedron.ply");
+ m_fem->loadFem("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"))));
+ ASSERT_NO_THROW(ASSERT_TRUE(m_fem->initialize(runtime)));
EXPECT_EQ(3u, m_fem->getNumDofPerNode());
EXPECT_EQ(3u * 26u, m_fem->getNumDof());
EXPECT_EQ(24u, m_fem->getInitialState()->getNumBoundaryConditions());
@@ -162,56 +146,41 @@ TEST_F(Fem3DRepresentationTests, DoInitializeTest)
{
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")));
+ EXPECT_ANY_THROW(m_fem->loadFem("Non existent fake name"));
}
{
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")));
+ // Fem3DRepresentation will not try to load file, but fem3DRepresentation::doInitialize() will throw.
+ EXPECT_ANY_THROW(m_fem->initialize(runtime));
}
{
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")));
+ EXPECT_ANY_THROW(m_fem->loadFem("PlyReaderTests/WrongPlyTetrahedron.ply"));
}
{
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")));
+ EXPECT_ANY_THROW(m_fem->loadFem("PlyReaderTests/WrongDataTetrahedron.ply"););
}
}
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"));
+ ASSERT_NO_THROW(createFem());
+ ASSERT_NO_THROW(m_fem->loadFem("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);
+ auto triangleMesh = std::make_shared<SurgSim::Math::MeshShape>();
+ triangleMesh->load("Geometry/wound_deformable.ply");
// 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));
+ collisionRepresentation->setShape(triangleMesh);
m_fem->setCollisionRepresentation(collisionRepresentation);
bool wokeUp = false;
@@ -240,29 +209,116 @@ TEST_F(Fem3DRepresentationTests, CreateLocalizationTest)
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()};
+ 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;
-
+ SurgSim::DataStructures::Location location(triangleLocalPosition,
+ SurgSim::DataStructures::Location::TRIANGLE);
+ std::shared_ptr<SurgSim::Physics::Fem3DLocalization> localization;
EXPECT_NO_THROW(localization =
- std::dynamic_pointer_cast<SurgSim::Physics::Fem3DRepresentationLocalization>(
- m_fem->createLocalization(location)););
+ std::dynamic_pointer_cast<SurgSim::Physics::Fem3DLocalization>(m_fem->createLocalization(location)););
EXPECT_TRUE(localization != nullptr);
+ EXPECT_TRUE(localization->getRepresentation() == m_fem);
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););
+ m_fem->getFemElement(coordinate.index)->computeCartesianCoordinate(
+ *m_fem->getCurrentState(), coordinate.coordinate););
EXPECT_EQ(3, globalPosition.size());
EXPECT_TRUE(globalPosition.isApprox(*point));
}
}
+
+ // Localization on an invalid node
+ {
+ SCOPED_TRACE("Invalid node");
+
+ SurgSim::DataStructures::Location location(1000);
+ std::shared_ptr<SurgSim::Physics::Localization> localization;
+ EXPECT_THROW(localization = m_fem->createLocalization(location), SurgSim::Framework::AssertionFailure);
+ }
+
+ // Localization on a valid node
+ {
+ SCOPED_TRACE("Valid node");
+
+ SurgSim::DataStructures::Location location(0);
+ std::shared_ptr<SurgSim::Physics::Fem3DLocalization> localization;
+ EXPECT_NO_THROW(localization =
+ std::dynamic_pointer_cast<SurgSim::Physics::Fem3DLocalization>(
+ m_fem->createLocalization(location)););
+ EXPECT_TRUE(localization != nullptr);
+ EXPECT_TRUE(localization->getRepresentation() == m_fem);
+
+ SurgSim::Math::Vector3d globalPosition;
+ SurgSim::DataStructures::IndexedLocalCoordinate coordinate = localization->getLocalPosition();
+ EXPECT_NO_THROW(globalPosition = m_fem->getCurrentState()->getPosition(coordinate.index););
+ EXPECT_TRUE(globalPosition.isApprox(localization->calculatePosition()));
+ }
+
+ // Localization on an invalid tetrahedron
+ {
+ SCOPED_TRACE("Invalid tetrahedron index");
+
+ Vector barycentricCoordinates = Vector::Zero(4);
+ barycentricCoordinates[0] = 1.0;
+ SurgSim::DataStructures::IndexedLocalCoordinate coord(10000, barycentricCoordinates);
+ SurgSim::DataStructures::Location location(coord, SurgSim::DataStructures::Location::ELEMENT);
+ std::shared_ptr<SurgSim::Physics::Localization> localization;
+ EXPECT_THROW(localization = m_fem->createLocalization(location), SurgSim::Framework::AssertionFailure);
+ }
+
+ // Localization on an valid tetrahedron but invalid barycentric coordinate size
+ {
+ SCOPED_TRACE("Invalid tetrahedron barycentric coordinate size");
+
+ Vector barycentricCoordinates = Vector::Zero(5);
+ barycentricCoordinates[0] = 1.0;
+ SurgSim::DataStructures::IndexedLocalCoordinate coord(0, barycentricCoordinates);
+ SurgSim::DataStructures::Location location(coord, SurgSim::DataStructures::Location::ELEMENT);
+ std::shared_ptr<SurgSim::Physics::Localization> localization;
+ EXPECT_THROW(localization = m_fem->createLocalization(location), SurgSim::Framework::AssertionFailure);
+ }
+
+ // Localization on an valid tetrahedron but invalid barycentric coordinate
+ {
+ SCOPED_TRACE("Invalid tetrahedron barycentric coordinate");
+
+ Vector barycentricCoordinates = Vector::Ones(4);
+ SurgSim::DataStructures::IndexedLocalCoordinate coord(0, barycentricCoordinates);
+ SurgSim::DataStructures::Location location(coord, SurgSim::DataStructures::Location::ELEMENT);
+ std::shared_ptr<SurgSim::Physics::Localization> localization;
+ EXPECT_THROW(localization = m_fem->createLocalization(location), SurgSim::Framework::AssertionFailure);
+ }
+
+ // Localization on a valid tetrahedron
+ {
+ SCOPED_TRACE("Valid tetrahedron");
+
+ Vector barycentricCoordinates = Vector::Zero(4);
+ barycentricCoordinates.setConstant(1.0 / 4.0);
+ SurgSim::DataStructures::IndexedLocalCoordinate coord(0, barycentricCoordinates);
+ SurgSim::DataStructures::Location location(coord, SurgSim::DataStructures::Location::ELEMENT);
+ std::shared_ptr<SurgSim::Physics::Fem3DLocalization> localization;
+ EXPECT_NO_THROW(localization =
+ std::dynamic_pointer_cast<SurgSim::Physics::Fem3DLocalization>(m_fem->createLocalization(location)));
+ EXPECT_TRUE(localization != nullptr);
+ EXPECT_TRUE(localization->getRepresentation() == m_fem);
+
+ SurgSim::DataStructures::IndexedLocalCoordinate coordinate = localization->getLocalPosition();
+ auto element = m_fem->getFemElement(coordinate.index);
+ SurgSim::Math::Vector3d globalPosition =
+ 1.0 / 4.0 * m_fem->getCurrentState()->getPosition(element->getNodeId(0)) +
+ 1.0 / 4.0 * m_fem->getCurrentState()->getPosition(element->getNodeId(1)) +
+ 1.0 / 4.0 * m_fem->getCurrentState()->getPosition(element->getNodeId(2)) +
+ 1.0 / 4.0 * m_fem->getCurrentState()->getPosition(element->getNodeId(3));
+ EXPECT_TRUE(globalPosition.isApprox(localization->calculatePosition()));
+ }
}
TEST_F(Fem3DRepresentationTests, ExternalForceAPITest)
@@ -279,6 +335,9 @@ TEST_F(Fem3DRepresentationTests, ExternalForceAPITest)
m_fem->setInitialState(m_initialState);
// Vector initialized (properly sized and zeroed)
+ Math::SparseMatrix zeroMatrix(static_cast<SparseMatrix::Index>(m_fem->getNumDof()),
+ static_cast<SparseMatrix::Index>(m_fem->getNumDof()));
+ zeroMatrix.setZero();
EXPECT_NE(0, m_fem->getExternalGeneralizedForce().size());
EXPECT_NE(0, m_fem->getExternalGeneralizedStiffness().rows());
EXPECT_NE(0, m_fem->getExternalGeneralizedStiffness().cols());
@@ -290,8 +349,8 @@ TEST_F(Fem3DRepresentationTests, ExternalForceAPITest)
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());
+ EXPECT_TRUE(m_fem->getExternalGeneralizedStiffness().isApprox(zeroMatrix));
+ EXPECT_TRUE(m_fem->getExternalGeneralizedDamping().isApprox(zeroMatrix));
addFemElement();
createLocalization();
@@ -311,31 +370,31 @@ TEST_F(Fem3DRepresentationTests, ExternalForceAPITest)
// Test invalid localization nullptr
ASSERT_THROW(m_fem->addExternalGeneralizedForce(nullptr, Flocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
ASSERT_THROW(m_fem->addExternalGeneralizedForce(nullptr, Flocal, Klocal, Dlocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
// Test invalid localization type
ASSERT_THROW(m_fem->addExternalGeneralizedForce(m_wrongLocalizationType, Flocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
ASSERT_THROW(m_fem->addExternalGeneralizedForce(m_wrongLocalizationType, Flocal, Klocal, Dlocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
// Test invalid force size
ASSERT_THROW(m_fem->addExternalGeneralizedForce(m_localization, FLocalWrongSize),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
ASSERT_THROW(m_fem->addExternalGeneralizedForce(m_localization, FLocalWrongSize, Klocal, Dlocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
// Test invalid stiffness size
ASSERT_THROW(m_fem->addExternalGeneralizedForce(m_localization, Flocal, KLocalWrongSize, Dlocal),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
// Test invalid damping size
ASSERT_THROW(m_fem->addExternalGeneralizedForce(m_localization, Flocal, Klocal, DLocalWrongSize),
- SurgSim::Framework::AssertionFailure);
+ 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_FALSE(m_fem->getExternalGeneralizedStiffness().isApprox(zeroMatrix));
+ EXPECT_FALSE(m_fem->getExternalGeneralizedDamping().isApprox(zeroMatrix));
EXPECT_TRUE(m_fem->getExternalGeneralizedForce().isApprox(F));
EXPECT_TRUE(m_fem->getExternalGeneralizedStiffness().isApprox(K));
EXPECT_TRUE(m_fem->getExternalGeneralizedDamping().isApprox(D));
@@ -347,11 +406,67 @@ TEST_F(Fem3DRepresentationTests, ExternalForceAPITest)
EXPECT_TRUE(m_fem->getExternalGeneralizedDamping().isApprox(2.0 * D));
}
+TEST_F(Fem3DRepresentationTests, LoadMeshTest)
+{
+ auto fem = std::make_shared<Fem3DRepresentation>("Representation");
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+
+ fem->loadFem("PlyReaderTests/Tetrahedron.ply");
+ fem->initialize(runtime);
+
+ // 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_F(Fem3DRepresentationTests, SerializationTest)
{
auto fem3DRepresentation = std::make_shared<SurgSim::Physics::Fem3DRepresentation>("Test-Fem3D");
- const std::string filename = "TestFilename";
- fem3DRepresentation->setFilename(filename);
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
+ const std::string filename = "PlyReaderTests/Fem3DCube.ply";
+ fem3DRepresentation->loadFem(filename);
auto collisionRepresentation = std::make_shared<DeformableCollisionRepresentation>("Collision");
fem3DRepresentation->setCollisionRepresentation(collisionRepresentation);
@@ -360,15 +475,13 @@ TEST_F(Fem3DRepresentationTests, SerializationTest)
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>>()));
+ ASSERT_NE(nullptr, newRepresentation);
EXPECT_EQ("SurgSim::Physics::Fem3DRepresentation", newRepresentation->getClassName());
- EXPECT_EQ(filename, newRepresentation->getValue<std::string>("Filename"));
+ EXPECT_EQ(filename, newRepresentation->getFem()->getValue<std::string>("FileName"));
}
} // namespace Physics
diff --git a/SurgSim/Physics/UnitTests/FemElementTests.cpp b/SurgSim/Physics/UnitTests/FemElementTests.cpp
index 90c0d0c..2abdf3e 100644
--- a/SurgSim/Physics/UnitTests/FemElementTests.cpp
+++ b/SurgSim/Physics/UnitTests/FemElementTests.cpp
@@ -20,6 +20,11 @@
#include "SurgSim/Physics/FemElement.h"
#include "SurgSim/Math/Vector.h"
#include "SurgSim/Math/Matrix.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+#include "SurgSim/Physics/Fem2DElementTriangle.h"
+#include "SurgSim/Physics/Fem3DElementCorotationalTetrahedron.h"
+#include "SurgSim/Physics/Fem3DElementCube.h"
+#include "SurgSim/Physics/Fem3DElementTetrahedron.h"
#include "SurgSim/Physics/UnitTests/MockObjects.h"
using SurgSim::Physics::FemElement;
@@ -133,16 +138,6 @@ TEST(FemElementTests, InitializeMethods)
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);
@@ -221,6 +216,91 @@ TEST(FemElementTests, IsValidCoordinate)
checkValidCoordinate(femElement, -0.01, 0.0, 1.01, e, false);
}
+TEST(FemElementTests, FactoryTest)
+{
+ auto mockElement = std::make_shared<FemElementStructs::FemElementParameter>();
+ mockElement->nodeIds.push_back(2);
+ FemElement::getFactory().registerClass<MockFemElement>("MockFemElement");
+
+ // Test with a mock FemElement
+ auto mockFem = FemElement::getFactory().create("MockFemElement", mockElement);
+ EXPECT_NE(nullptr, mockFem);
+ EXPECT_NE(nullptr, std::dynamic_pointer_cast<MockFemElement>(mockFem));
+
+ // Test with a 1D beam
+ auto beamElement = std::make_shared<FemElementStructs::FemElement1DParameter>();
+ beamElement->nodeIds.push_back(1);
+ beamElement->nodeIds.push_back(2);
+ beamElement->radius = 0.4;
+ beamElement->enableShear = false;
+ beamElement->massDensity = 0.4;
+ beamElement->poissonRatio = 0.4;
+ beamElement->youngModulus = 0.4;
+ static SurgSim::Physics::Fem1DElementBeam beam;
+ auto beamFem = FemElement::getFactory().create(beam.getClassName(), beamElement);
+ EXPECT_NE(nullptr, beamFem);
+ EXPECT_NE(nullptr, std::dynamic_pointer_cast<Fem1DElementBeam>(beamFem));
+ ASSERT_ANY_THROW(FemElement::getFactory().create("SurgSim::Physics::Fem1DElementBeam", mockElement));
+
+ // Test with a 2D triangle
+ auto triElement = std::make_shared<FemElementStructs::FemElement2DParameter>();
+ triElement->nodeIds.push_back(1);
+ triElement->nodeIds.push_back(2);
+ triElement->nodeIds.push_back(3);
+ triElement->thickness = 0.4;
+ triElement->massDensity = 0.4;
+ triElement->poissonRatio = 0.4;
+ triElement->youngModulus = 0.4;
+ static SurgSim::Physics::Fem2DElementTriangle triangle;
+ auto triFem = FemElement::getFactory().create(triangle.getClassName(), triElement);
+ EXPECT_NE(nullptr, triFem);
+ EXPECT_NE(nullptr, std::dynamic_pointer_cast<Fem2DElementTriangle>(triFem));
+ ASSERT_ANY_THROW(FemElement::getFactory().create("SurgSim::Physics::Fem2DElementTriangle", beamElement));
+
+ // Test with a 3D corotational tetrahedron
+ auto tetElement = std::make_shared<FemElementStructs::FemElement3DParameter>();
+ tetElement->nodeIds.push_back(1);
+ tetElement->nodeIds.push_back(2);
+ tetElement->nodeIds.push_back(3);
+ tetElement->nodeIds.push_back(1);
+ tetElement->massDensity = 0.4;
+ tetElement->poissonRatio = 0.4;
+ tetElement->youngModulus = 0.4;
+ static SurgSim::Physics::Fem3DElementCorotationalTetrahedron corotationalTetrahedron;
+ auto coTetFem = FemElement::getFactory().create(
+ corotationalTetrahedron.getClassName(), tetElement);
+ EXPECT_NE(nullptr, coTetFem);
+ EXPECT_NE(nullptr, std::dynamic_pointer_cast<Fem3DElementCorotationalTetrahedron>(coTetFem));
+ ASSERT_ANY_THROW(FemElement::getFactory().create(
+ "SurgSim::Physics::Fem3DElementCorotationalTetrahedron", triElement));
+
+ // Test with a 3D cube
+ auto cubeElement = std::make_shared<FemElementStructs::FemElement3DParameter>();
+ cubeElement->nodeIds.push_back(1);
+ cubeElement->nodeIds.push_back(2);
+ cubeElement->nodeIds.push_back(3);
+ cubeElement->nodeIds.push_back(4);
+ cubeElement->nodeIds.push_back(5);
+ cubeElement->nodeIds.push_back(6);
+ cubeElement->nodeIds.push_back(7);
+ cubeElement->nodeIds.push_back(8);
+ cubeElement->massDensity = 0.4;
+ cubeElement->poissonRatio = 0.4;
+ cubeElement->youngModulus = 0.4;
+ static SurgSim::Physics::Fem3DElementCube cube;
+ auto cubeFem = FemElement::getFactory().create(cube.getClassName(), cubeElement);
+ EXPECT_NE(nullptr, cubeFem);
+ EXPECT_NE(nullptr, std::dynamic_pointer_cast<Fem3DElementCube>(cubeFem));
+ ASSERT_ANY_THROW(FemElement::getFactory().create(cube.getClassName(), tetElement));
+
+ // Test with a 3D tetrahedron
+ static SurgSim::Physics::Fem3DElementTetrahedron tetrahedron;
+ auto tetFem = FemElement::getFactory().create(tetrahedron.getClassName(), tetElement);
+ EXPECT_NE(nullptr, tetFem);
+ EXPECT_NE(nullptr, std::dynamic_pointer_cast<Fem3DElementTetrahedron>(tetFem));
+ ASSERT_ANY_THROW(FemElement::getFactory().create(tetrahedron.getClassName(), cubeElement));
+}
+
} // namespace Physics
} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/FemLocalizationTest.cpp b/SurgSim/Physics/UnitTests/FemLocalizationTest.cpp
new file mode 100644
index 0000000..e96eee8
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/FemLocalizationTest.cpp
@@ -0,0 +1,291 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/UnitTests/MockObjects.h"
+#include "SurgSim/Physics/FemLocalization.h"
+#include "SurgSim/Physics/FemRepresentation.h"
+
+using SurgSim::DataStructures::IndexedLocalCoordinate;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+std::shared_ptr<MockFemElement> createFemElement(std::array<size_t, 2> nodes)
+{
+ auto element = std::make_shared<MockFemElement>();
+ for (const auto& node : nodes) element->addNode(node);
+ element->setMassDensity(1000.0);
+ element->setPoissonRatio(0.45);
+ element->setYoungModulus(1e6);
+ return element;
+}
+
+class FemLocalizationTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::getSubVector;
+
+ m_fem = std::make_shared<MockFemRepresentation>("MockFemRepresentation");
+ 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);
+
+ auto& v = state->getVelocities();
+ getSubVector(v, 0, 6).segment<3>(0) = Vector3d(0.0, 0.0, 0.0);
+ getSubVector(v, 1, 6).segment<3>(0) = Vector3d(0.0, 1.0, -1.0);
+ getSubVector(v, 2, 6).segment<3>(0) = Vector3d(-1.0, 1.0, 0.0);
+
+ // Define Beams
+ {
+ std::array<size_t, 2> nodes = {{0, 1}};
+ m_fem->addFemElement(createFemElement(nodes));
+ }
+
+ {
+ std::array<size_t, 2> nodes = {{1, 2}};
+ m_fem->addFemElement(createFemElement(nodes));
+ }
+
+ 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_invalidIndexLocalPosition.coordinate = SurgSim::Math::Vector::Zero(2);
+ m_invalidIndexLocalPosition.coordinate[0] = 0.4;
+ m_invalidIndexLocalPosition.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<MockFemRepresentation> m_fem;
+ SurgSim::DataStructures::IndexedLocalCoordinate m_validLocalPosition;
+ SurgSim::DataStructures::IndexedLocalCoordinate m_invalidIndexLocalPosition;
+ SurgSim::DataStructures::IndexedLocalCoordinate m_invalidCoordinateLocalPosition;
+};
+
+TEST_F(FemLocalizationTest, ConstructorTest)
+{
+ // IndexedLocalCoordinate pointing to a node (node index + empty coordinate) are invalid. It will failed,
+ // either because the index is out of bound or because the coordinates are the wrong size (empty)
+ // This is tested by m_invalidIndexLocalPosition and m_invalidCoordinateLocalPosition
+ ASSERT_THROW(
+ std::make_shared<FemLocalization>(m_fem, m_invalidIndexLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+
+ ASSERT_THROW(
+ std::make_shared<FemLocalization>(m_fem, m_invalidCoordinateLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+
+ ASSERT_NO_THROW(std::make_shared<FemLocalization>(m_fem, m_validLocalPosition););
+}
+
+TEST_F(FemLocalizationTest, SetGetRepresentation)
+{
+ FemLocalization 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(FemLocalizationTest, SetGetLocalization)
+{
+ using SurgSim::Math::Vector2d;
+ using SurgSim::Math::Vector3d;
+
+ {
+ SCOPED_TRACE("Uninitialized Representation");
+
+ // Uninitialized Representation
+ EXPECT_THROW(std::make_shared<FemLocalization>(nullptr, m_validLocalPosition),
+ SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("Incorrectly formed natural coordinate");
+
+ // Incorrectly formed natural coordinate
+ auto localization = std::make_shared<FemLocalization>(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<FemLocalization>(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<FemLocalization>(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(FemLocalizationTest, CalculatePositionTest)
+{
+ using SurgSim::Math::Vector;
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::Vector2d;
+
+ auto localization = std::make_shared<FemLocalization>(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));
+
+ // Out-Of-Range assertions
+ EXPECT_THROW(localization->calculatePosition(-0.01), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(localization->calculatePosition(1.01), SurgSim::Framework::AssertionFailure);
+}
+
+TEST_F(FemLocalizationTest, CalculateVelocityTest)
+{
+ using SurgSim::Math::Vector;
+ using SurgSim::Math::Vector3d;
+ using SurgSim::Math::Vector2d;
+
+ auto localization = std::make_shared<FemLocalization>(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->calculateVelocity(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(0u, Vector2d(0.0, 1.0)));
+ EXPECT_TRUE(Vector3d(0.0, 1.0, -1.0).isApprox(localization->calculateVelocity(), 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->calculateVelocity(), epsilon));
+
+ localization->setLocalPosition(IndexedLocalCoordinate(1u, Vector2d(0.0, 1.0)));
+ EXPECT_TRUE(Vector3d(-1.0, 1.0, 0.0).isApprox(localization->calculateVelocity(), 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->calculateVelocity(), 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->calculateVelocity(), epsilon));
+
+ // Out-Of-Range assertions
+ EXPECT_THROW(localization->calculateVelocity(-0.01), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(localization->calculateVelocity(1.01), SurgSim::Framework::AssertionFailure);
+}
+
+TEST_F(FemLocalizationTest, ElementPose)
+{
+ FemLocalization localization(m_fem, m_validLocalPosition);
+ EXPECT_THROW(localization.getElementPose(), SurgSim::Framework::AssertionFailure);
+}
+
+TEST_F(FemLocalizationTest, MoveClosestTo)
+{
+ FemLocalization localization(m_fem, m_validLocalPosition);
+ bool flag = false;
+ EXPECT_THROW(localization.moveClosestTo(SurgSim::Math::Vector3d::Zero(), &flag),
+ SurgSim::Framework::AssertionFailure);
+}
+
+} // namespace SurgSim
+} // namespace Physics
diff --git a/SurgSim/Physics/UnitTests/FemRepresentationParametersTest.cpp b/SurgSim/Physics/UnitTests/FemRepresentationParametersTest.cpp
deleted file mode 100644
index 4379332..0000000
--- a/SurgSim/Physics/UnitTests/FemRepresentationParametersTest.cpp
+++ /dev/null
@@ -1,183 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
index 1e0340e..c1ceecc 100644
--- a/SurgSim/Physics/UnitTests/FemRepresentationTests.cpp
+++ b/SurgSim/Physics/UnitTests/FemRepresentationTests.cpp
@@ -19,9 +19,12 @@
#include <gtest/gtest.h>
#include "SurgSim/DataStructures/IndexedLocalCoordinate.h"
-#include "SurgSim/Framework/Runtime.h" //< Used to initialize the Component Fem3DRepresentation
+#include "SurgSim/Framework/Runtime.h" ///< Used to initialize the Component Fem3DRepresentation
+#include "SurgSim/Physics/UnitTests/DeformableTestsUtility.h"
#include "SurgSim/Physics/UnitTests/MockObjects.h"
+using SurgSim::Math::OdeEquationUpdate;
+
namespace SurgSim
{
@@ -50,7 +53,7 @@ public:
std::shared_ptr<SurgSim::Math::OdeState> m_initialState;
protected:
- virtual void SetUp() override
+ void SetUp() override
{
m_rho = 2000.0;
m_nu = 0.45;
@@ -215,64 +218,25 @@ TEST_F(FemRepresentationTests, BeforeUpdateTest)
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());
- }
+ 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());
}
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);
@@ -284,23 +248,15 @@ TEST_F(FemRepresentationTests, ComputesWithNoGravityAndNoDampingTest)
{
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));
+ testOdeEquationUpdate(m_fem, *m_initialState, expectedF, m_expectedMass, m_expectedDamping,
+ m_expectedStiffness);
}
{
SCOPED_TRACE("With external force");
- std::shared_ptr<MockDeformableRepresentationLocalization> localization =
- std::make_shared<MockDeformableRepresentationLocalization>();
+ std::shared_ptr<MockDeformableLocalization> localization =
+ std::make_shared<MockDeformableLocalization>();
localization->setRepresentation(m_fem);
localization->setLocalNode(0);
Vector FextLocal = Vector::Ones(m_fem->getNumDofPerNode());
@@ -314,15 +270,8 @@ TEST_F(FemRepresentationTests, ComputesWithNoGravityAndNoDampingTest)
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));
+ testOdeEquationUpdate(m_fem, *m_initialState, expectedF + Fext, m_expectedMass, m_expectedDamping + Dext,
+ m_expectedStiffness + Kext);
}
}
@@ -330,9 +279,6 @@ 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);
@@ -346,24 +292,15 @@ TEST_F(FemRepresentationTests, ComputesWithNoGravityAndDampingTest)
{
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));
+ testOdeEquationUpdate(m_fem, *m_initialState, expectedF, m_expectedMass,
+ m_expectedDamping + m_expectedRayleighDamping, m_expectedStiffness);
}
{
SCOPED_TRACE("With external force");
- std::shared_ptr<MockDeformableRepresentationLocalization> localization =
- std::make_shared<MockDeformableRepresentationLocalization>();
+ std::shared_ptr<MockDeformableLocalization> localization =
+ std::make_shared<MockDeformableLocalization>();
localization->setRepresentation(m_fem);
localization->setLocalNode(0);
Vector FextLocal = Vector::Ones(m_fem->getNumDofPerNode());
@@ -377,25 +314,15 @@ TEST_F(FemRepresentationTests, ComputesWithNoGravityAndDampingTest)
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));
+ testOdeEquationUpdate(m_fem, *m_initialState, expectedF + Fext, m_expectedMass,
+ m_expectedDamping + m_expectedRayleighDamping + Dext, m_expectedStiffness + Kext);
}
}
TEST_F(FemRepresentationTests, ComputesWithGravityAndNoDampingTest)
{
using SurgSim::Math::Vector3d;
-
- Vector *F;
- Matrix *M, *D, *K;
+ using SurgSim::Math::SparseMatrix;
// Gravity, no Rayleigh damping
// computeF tests addFemElementsForce and addGravityForce
@@ -408,23 +335,15 @@ TEST_F(FemRepresentationTests, ComputesWithGravityAndNoDampingTest)
{
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));
+ testOdeEquationUpdate(m_fem, *m_initialState, expectedF, m_expectedMass, m_expectedDamping,
+ m_expectedStiffness);
}
{
SCOPED_TRACE("With external force");
- std::shared_ptr<MockDeformableRepresentationLocalization> localization =
- std::make_shared<MockDeformableRepresentationLocalization>();
+ std::shared_ptr<MockDeformableLocalization> localization =
+ std::make_shared<MockDeformableLocalization>();
localization->setRepresentation(m_fem);
localization->setLocalNode(0);
Vector FextLocal = Vector::Ones(m_fem->getNumDofPerNode());
@@ -438,24 +357,15 @@ TEST_F(FemRepresentationTests, ComputesWithGravityAndNoDampingTest)
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));
+ testOdeEquationUpdate(m_fem, *m_initialState, expectedF + Fext, m_expectedMass,
+ m_expectedDamping + Dext, m_expectedStiffness + Kext);
}
}
TEST_F(FemRepresentationTests, ComputesWithGravityAndDampingTest)
{
using SurgSim::Math::Vector3d;
-
- Vector *F;
- Matrix *M, *D, *K;
+ using SurgSim::Math::SparseMatrix;
// Gravity, Rayleigh damping
// computeF tests addFemElementsForce, addRayleighDampingForce and addGravityForce
@@ -466,29 +376,20 @@ TEST_F(FemRepresentationTests, ComputesWithGravityAndDampingTest)
m_fem->wakeUp();
SurgSim::Math::Vector expectedF = m_expectedFemElementsForce + m_expectedRayleighDampingForce +
- m_expectedGravityForce;
+ 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));
+ testOdeEquationUpdate(m_fem, *m_initialState, expectedF, m_expectedMass,
+ m_expectedDamping + m_expectedRayleighDamping, m_expectedStiffness);
}
{
SCOPED_TRACE("With external force");
- std::shared_ptr<MockDeformableRepresentationLocalization> localization =
- std::make_shared<MockDeformableRepresentationLocalization>();
+ std::shared_ptr<MockDeformableLocalization> localization =
+ std::make_shared<MockDeformableLocalization>();
localization->setRepresentation(m_fem);
localization->setLocalNode(0);
Vector FextLocal = Vector::Ones(m_fem->getNumDofPerNode());
@@ -502,16 +403,8 @@ TEST_F(FemRepresentationTests, ComputesWithGravityAndDampingTest)
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));
+ testOdeEquationUpdate(m_fem, *m_initialState, expectedF + Fext, m_expectedMass,
+ m_expectedDamping + m_expectedRayleighDamping + Dext, m_expectedStiffness + Kext);
}
}
@@ -557,6 +450,111 @@ TEST_F(FemRepresentationTests, DoInitializeTest)
}
}
+TEST_F(FemRepresentationTests, ComplianceWarpingTest)
+{
+ {
+ SCOPED_TRACE("MockFemRepresentation incomplete for compliance warping");
+ auto fem = std::make_shared<MockFemRepresentation>("fem");
+
+ EXPECT_NO_THROW(EXPECT_FALSE(fem->getComplianceWarping()));
+ EXPECT_NO_THROW(fem->setComplianceWarping(true));
+ EXPECT_NO_THROW(EXPECT_TRUE(fem->getComplianceWarping()));
+
+ // Setup the initial state
+ auto initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(fem->getNumDofPerNode(), 3);
+ 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);
+
+ fem->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ fem->wakeUp();
+
+ EXPECT_NO_THROW(EXPECT_TRUE(fem->getComplianceWarping()));
+ EXPECT_THROW(fem->setComplianceWarping(false), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(fem->setComplianceWarping(true), SurgSim::Framework::AssertionFailure);
+
+ // update() will call updateNodesRotations() which will raise an exception in this case.
+ // This method has not been overridden.
+ EXPECT_THROW(fem->update(1e-3), SurgSim::Framework::AssertionFailure);
+ }
+
+ {
+ SCOPED_TRACE("MockFemRepresentation complete for compliance warping");
+ auto fem = std::make_shared<MockFemRepresentationValidComplianceWarping>("fem");
+
+ EXPECT_NO_THROW(EXPECT_FALSE(fem->getComplianceWarping()));
+ EXPECT_NO_THROW(fem->setComplianceWarping(true));
+ EXPECT_NO_THROW(EXPECT_TRUE(fem->getComplianceWarping()));
+
+ // Setup the initial state
+ auto initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(fem->getNumDofPerNode(), 3);
+ 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);
+
+ fem->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ fem->wakeUp();
+
+ EXPECT_NO_THROW(EXPECT_TRUE(fem->getComplianceWarping()));
+ EXPECT_THROW(fem->setComplianceWarping(false), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(fem->setComplianceWarping(true), SurgSim::Framework::AssertionFailure);
+
+ // update() will call updateNodesRotations() which will not raise an exception in this case.
+ // This method has been overridden.
+ EXPECT_NO_THROW(fem->update(1e-3));
+
+ EXPECT_NO_THROW(EXPECT_TRUE((fem->applyCompliance(*initialState, Matrix::Identity(initialState->getNumDof(),
+ initialState->getNumDof())) / 1e-3).isIdentity()));
+ }
+}
+
+TEST_F(FemRepresentationTests, SerializationTest)
+{
+ auto fem = std::make_shared<MockFemRepresentation>("Test-Fem");
+
+ EXPECT_NO_THROW(fem->setValue("ComplianceWarping", true));
+ EXPECT_TRUE(fem->getComplianceWarping());
+ EXPECT_TRUE(fem->getValue<bool>("ComplianceWarping"));
+ EXPECT_NO_THROW(fem->setValue("ComplianceWarping", false));
+ EXPECT_FALSE(fem->getComplianceWarping());
+ EXPECT_FALSE(fem->getValue<bool>("ComplianceWarping"));
+
+ EXPECT_NO_THROW(fem->setValue("RayleighDampingMass", 1.1));
+ EXPECT_NO_THROW(fem->getValue<double>("RayleighDampingMass"));
+ EXPECT_DOUBLE_EQ(1.1, fem->getValue<double>("RayleighDampingMass"));
+
+ EXPECT_NO_THROW(fem->setValue("RayleighDampingStiffness", 2.2));
+ EXPECT_NO_THROW(fem->getValue<double>("RayleighDampingStiffness"));
+ EXPECT_DOUBLE_EQ(2.2, fem->getValue<double>("RayleighDampingStiffness"));
+}
+
+TEST_F(FemRepresentationTests, SetInitialStateTest)
+{
+ auto fem = std::make_shared<MockFemRepresentation>("Test-Fem");
+
+ EXPECT_FALSE(fem->hasSetInitialStateBeenCalled());
+ fem->setInitialState(m_initialState);
+ EXPECT_TRUE(fem->hasSetInitialStateBeenCalled());
+}
+
} // namespace Physics
} // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/FixedConstraintFixedPointTests.cpp b/SurgSim/Physics/UnitTests/FixedConstraintFixedPointTests.cpp
new file mode 100644
index 0000000..7693833
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/FixedConstraintFixedPointTests.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 <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/FixedConstraintFixedPoint.h"
+#include "SurgSim/Physics/FixedRepresentation.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(FixedConstraintFixedPointTests, Constructor)
+{
+ ASSERT_NO_THROW(
+ { FixedConstraintFixedPoint constraint; });
+}
+
+TEST(FixedConstraintFixedPointTests, Constants)
+{
+ FixedConstraintFixedPoint constraint;
+
+ EXPECT_EQ(SurgSim::Physics::FIXED_3DPOINT, constraint.getConstraintType());
+ EXPECT_EQ(3u, constraint.getNumDof());
+}
+
+TEST(FixedConstraintFixedPointTests, 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 necessarily construct the physical parameters
+ // necessary to supply a realistic C. It only checks H and b.
+ FixedConstraintFixedPoint constraint;
+
+ Vector3d actual = Vector3d(8.0, 6.4, 3.5);
+
+ // Setup parameters for FixedConstraintFixedPoint::build
+ auto localization = std::make_shared<FixedLocalization>(
+ 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(FixedConstraintFixedPointTests, 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 necessarily construct the physical parameters
+ // necessary to supply a realistic C. It only checks H and b.
+ FixedConstraintFixedPoint constraint;
+
+ Vector3d actual = Vector3d(8.0, 6.4, 3.5);
+ Vector3d desired = Vector3d(3.0, 7.7, 0.0);
+
+ // Setup parameters for FixedConstraintFixedPoint::build
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(0, 3, 1);
+
+ ConstraintData emptyConstraint;
+
+ auto localization = std::make_shared<FixedLocalization>(
+ 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/FixedConstraintFixedRotationVectorTests.cpp b/SurgSim/Physics/UnitTests/FixedConstraintFixedRotationVectorTests.cpp
new file mode 100644
index 0000000..ac3fc2b
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/FixedConstraintFixedRotationVectorTests.cpp
@@ -0,0 +1,106 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/MlcpConstraintType.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/RotationVectorConstraintData.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/FixedConstraintFixedRotationVector.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+const double epsilon = 1e-10;
+const double dt = 1e-3;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+TEST(FixedConstraintFixedRotationVectorTests, Constructor)
+{
+ ASSERT_NO_THROW(
+ { FixedConstraintFixedRotationVector constraint; });
+}
+
+TEST(FixedConstraintFixedRotationVectorTests, Constants)
+{
+ FixedConstraintFixedRotationVector constraint;
+
+ EXPECT_EQ(SurgSim::Physics::FIXED_3DROTATION_VECTOR, constraint.getConstraintType());
+ EXPECT_EQ(3u, constraint.getNumDof());
+}
+
+TEST(FixedConstraintFixedRotationVectorTests, BuildMlcp)
+{
+ using SurgSim::Framework::Runtime;
+
+ FixedConstraintFixedRotationVector constraint;
+
+ // Prepare the fem1d representation for this constraint type
+ auto fem1d = std::make_shared<Fem1DRepresentation>("fem1d");
+ auto initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(6, 2);
+ initialState->getPositions().setZero();
+ initialState->getPositions().segment<3>(6) = SurgSim::Math::Vector3d(1.0, 0.0, 0.0);
+ fem1d->setInitialState(initialState);
+ std::array<size_t, 2> nodeIds = { {0, 1} };
+ auto beam = std::make_shared<Fem1DElementBeam>(nodeIds);
+ beam->setMassDensity(900);
+ beam->setPoissonRatio(0.4);
+ beam->setRadius(0.1);
+ beam->setShearingEnabled(false);
+ beam->setYoungModulus(1e6);
+ fem1d->addFemElement(beam);
+ fem1d->initialize(std::make_shared<Runtime>()); // Initializes the beams initial rotation matrix
+
+ // Prepare the fixed representation for this constraint type
+ auto fixed = std::make_shared<MockFixedRepresentation>();
+ Vector3d centerOfMass = Vector3d(3.0, 2.42, 9.54);
+ SurgSim::Math::Quaterniond objectRotation = SurgSim::Math::Quaterniond(0.1, 0.35, 4.2, 5.0).normalized();
+ SurgSim::Math::RigidTransform3d objectPose = SurgSim::Math::makeRigidTransform(objectRotation, centerOfMass);
+ fixed->getCurrentState().setPose(objectPose);
+ auto localization = std::make_shared<FixedLocalization>(fixed);
+
+ // Prepare the specific constraint data
+ RotationVectorRigidFem1DConstraintData constraintData;
+ constraintData.setFem1DRotation(fem1d, 0);
+ constraintData.setRigidOrFixedRotation(fixed, fixed->getPose().linear());
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(0, 3, 1);
+
+ ASSERT_NO_THROW(constraint.build(
+ dt, constraintData, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE));
+
+ // Compare results
+ Eigen::AngleAxisd aa(objectPose.linear());
+ Eigen::Matrix<double, 3, 1> violation = aa.angle() * aa.axis();
+ EXPECT_NEAR_EIGEN(violation, mlcpPhysicsProblem.b, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/FixedConstraintFrictionlessContactTests.cpp b/SurgSim/Physics/UnitTests/FixedConstraintFrictionlessContactTests.cpp
new file mode 100644
index 0000000..e02ce8a
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/FixedConstraintFrictionlessContactTests.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 <memory>
+
+#include <gtest/gtest.h>
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/FixedConstraintFrictionlessContact.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
+#include "SurgSim/Physics/MlcpPhysicsProblem.h"
+#include "SurgSim/Physics/RigidConstraintFrictionlessContact.h"
+#include "SurgSim/Physics/RigidRepresentation.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 (FixedConstraintFrictionlessContactTests, 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);
+
+ auto loc = std::make_shared<FixedLocalization>(fixed);
+ loc->setLocalPosition(contactPosition);
+ std::shared_ptr<FixedConstraintFrictionlessContact> implementation =
+ std::make_shared<FixedConstraintFrictionlessContact>();
+
+ EXPECT_EQ(SurgSim::Physics::FRICTIONLESS_3DCONTACT, implementation->getConstraintType());
+ 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/FixedRepresentationBilateral3DTests.cpp b/SurgSim/Physics/UnitTests/FixedRepresentationBilateral3DTests.cpp
deleted file mode 100644
index 2739abc..0000000
--- a/SurgSim/Physics/UnitTests/FixedRepresentationBilateral3DTests.cpp
+++ /dev/null
@@ -1,116 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index deffeaa..0000000
--- a/SurgSim/Physics/UnitTests/FixedRepresentationContactTests.cpp
+++ /dev/null
@@ -1,87 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 215b8a9..0000000
--- a/SurgSim/Physics/UnitTests/FixedRepresentationLocalizationTest.cpp
+++ /dev/null
@@ -1,99 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
index d1565ad..cc57e3e 100644
--- a/SurgSim/Physics/UnitTests/FixedRepresentationTest.cpp
+++ b/SurgSim/Physics/UnitTests/FixedRepresentationTest.cpp
@@ -19,13 +19,15 @@
#include "SurgSim/Framework/BasicSceneElement.h"
#include "SurgSim/Framework/FrameworkConvert.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.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"
+#include "SurgSim/Physics/RigidState.h"
using SurgSim::Framework::BasicSceneElement;
using SurgSim::Math::Vector3d;
@@ -35,7 +37,7 @@ using SurgSim::Math::SphereShape;
using SurgSim::Physics::Representation;
using SurgSim::Physics::FixedRepresentation;
using SurgSim::Physics::RigidCollisionRepresentation;
-using SurgSim::Physics::RigidRepresentationState;
+using SurgSim::Physics::RigidState;
class FixedRepresentationTest : public ::testing::Test
{
@@ -56,15 +58,24 @@ public:
q.normalize();
t.setRandom();
m_currentTransformation = SurgSim::Math::makeRigidTransform(q, t);
- } while (m_initialTransformation.isApprox(m_currentTransformation));
+ }
+ 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);
+
+ m_runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ m_scene = m_runtime->getScene();
+
+ m_scene->addSceneElement(m_element);
}
+ std::shared_ptr<SurgSim::Framework::Runtime> m_runtime;
+ std::shared_ptr<SurgSim::Framework::Scene> m_scene;
+
std::shared_ptr<FixedRepresentation> m_fixedRepresentation;
std::shared_ptr<BasicSceneElement> m_element;
@@ -76,6 +87,7 @@ public:
// Identity pose (no translation/rotation)
RigidTransform3d m_identityTransformation;
+
};
TEST_F(FixedRepresentationTest, ConstructorTest)
@@ -89,7 +101,6 @@ TEST_F(FixedRepresentationTest, ResetStateTest)
m_fixedRepresentation->setIsGravityEnabled(false);
m_fixedRepresentation->setLocalPose(m_initialTransformation);
- m_element->initialize();
m_fixedRepresentation->wakeUp();
// Initial = Current = Previous = m_initialTransformation
@@ -164,7 +175,6 @@ TEST_F(FixedRepresentationTest, UpdateTest)
double dt = 1.0;
m_fixedRepresentation->setLocalPose(m_initialTransformation);
- m_element->initialize();
m_fixedRepresentation->wakeUp();
m_fixedRepresentation->setLocalPose(m_currentTransformation);
@@ -186,7 +196,7 @@ TEST_F(FixedRepresentationTest, SerializationTest)
auto rigidRepresentation = std::make_shared<FixedRepresentation>("TestFixedRepresentation");
YAML::Node node;
ASSERT_NO_THROW(node =
- YAML::convert<std::shared_ptr<SurgSim::Framework::Component>>::encode(rigidRepresentation));
+ YAML::convert<std::shared_ptr<SurgSim::Framework::Component>>::encode(rigidRepresentation));
std::shared_ptr<FixedRepresentation> newRepresentation;
EXPECT_NO_THROW(newRepresentation =
diff --git a/SurgSim/Physics/UnitTests/LinearSpringTest.cpp b/SurgSim/Physics/UnitTests/LinearSpringTest.cpp
index 20b69b1..c944b20 100644
--- a/SurgSim/Physics/UnitTests/LinearSpringTest.cpp
+++ b/SurgSim/Physics/UnitTests/LinearSpringTest.cpp
@@ -24,8 +24,11 @@
using SurgSim::Math::Matrix;
using SurgSim::Math::Matrix33d;
+using SurgSim::Math::Matrix66d;
+using SurgSim::Math::SparseMatrix;
using SurgSim::Math::Vector;
using SurgSim::Math::Vector3d;
+using SurgSim::Math::Vector6d;
using SurgSim::Physics::LinearSpring;
namespace
@@ -34,47 +37,45 @@ 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)
+Matrix66d 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;
+ 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;
+ K00 += damping * (u * (v1 - v0).transpose()) / m_l;
- Eigen::Matrix<double, 6, 6> K;
- K.setZero();
+ Matrix66d K = Matrix66d::Zero();
// Assembly stage in K
- SurgSim::Math::addSubMatrix( K00, 0, 0, 3, 3, &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);
+ 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)
+Matrix66d 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);
+ Matrix66d D = Matrix66d::Zero();
+ 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);
+ SurgSim::Math::addSubMatrix(D00, 1, 1, 3, 3, &D);
return D;
}
@@ -88,7 +89,7 @@ double f(size_t axis, const Vector3d p0, const Vector3d p1,
u /= m_l;
double elongationPosition = m_l - l0;
double elongationVelocity = (v1 - v0).dot(u);
- if (axis <=2)
+ if (axis <= 2)
{
return (stiffness * elongationPosition + damping * elongationVelocity) * u[axis];
}
@@ -98,11 +99,11 @@ double f(size_t axis, const Vector3d p0, const Vector3d p1,
}
}
-Eigen::Matrix<double, 6, 6> KNumerical(const Vector3d p0, const Vector3d p1,
- const Vector3d v0, const Vector3d v1,
- double l0, double stiffness, double damping)
+Matrix66d 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;
+ Matrix66d dfdx;
for (size_t row = 0; row < 6; row++)
{
@@ -113,13 +114,12 @@ Eigen::Matrix<double, 6, 6> KNumerical(const Vector3d p0, const Vector3d p1,
// (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();
+ Vector6d delta6D = Vector6d::Zero();
delta6D[col] = epsilonNumericalEvaluation;
double f_plus_delta = f(row, p0 + delta6D.segment(0, 3), p1 + delta6D.segment(3, 3), v0, v1,
- l0, stiffness, damping);
+ l0, stiffness, damping);
double f_minus_delta = f(row, p0 - delta6D.segment(0, 3), p1 - delta6D.segment(3, 3), v0, v1,
- l0, stiffness, damping);
+ l0, stiffness, damping);
dfdx(row, col) = (f_plus_delta - f_minus_delta) / (2.0 * epsilonNumericalEvaluation);
}
}
@@ -127,11 +127,11 @@ Eigen::Matrix<double, 6, 6> KNumerical(const Vector3d p0, const Vector3d p1,
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)
+Matrix66d 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;
+ Matrix66d dfdv;
for (size_t row = 0; row < 6; row++)
{
@@ -142,13 +142,12 @@ Eigen::Matrix<double, 6, 6> DNumerical(const Vector3d p0, const Vector3d p1,
// (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();
+ Vector6d delta6D = Vector6d::Zero();
delta6D[col] = epsilonNumericalEvaluation;
double f_plus_delta = f(row, p0, p1, v0 + delta6D.segment(0, 3), v1 + delta6D.segment(3, 3),
- l0, stiffness, damping);
+ l0, stiffness, damping);
double f_moins_delta = f(row, p0, p1, v0 - delta6D.segment(0, 3), v1 - delta6D.segment(3, 3),
- l0, stiffness, damping);
+ l0, stiffness, damping);
dfdv(row, col) = (f_plus_delta - f_moins_delta) / (2.0 * epsilonNumericalEvaluation);
}
}
@@ -161,7 +160,7 @@ Eigen::Matrix<double, 6, 6> DNumerical(const Vector3d p0, const Vector3d p1,
TEST(LinearSpringTests, Constructor)
{
ASSERT_NO_THROW({LinearSpring ls(0, 1);});
- ASSERT_NO_THROW({LinearSpring *ls = new LinearSpring(0, 1); delete ls;});
+ 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);});
}
@@ -170,15 +169,24 @@ TEST(LinearSpringTests, SetGetMethods)
LinearSpring ls(0, 1);
// Stiffness getter/setter
- ls.setStiffness(0.34);
+ ASSERT_THROW(ls.setStiffness(-0.34), SurgSim::Framework::AssertionFailure);
+ ASSERT_NO_THROW(ls.setStiffness(0.0));
+ ASSERT_DOUBLE_EQ(0.0, ls.getStiffness());
+ ASSERT_NO_THROW(ls.setStiffness(0.34));
ASSERT_DOUBLE_EQ(0.34, ls.getStiffness());
// Damping getter/setter
- ls.setDamping(0.45);
+ ASSERT_THROW(ls.setDamping(-0.45), SurgSim::Framework::AssertionFailure);
+ ASSERT_NO_THROW(ls.setDamping(0.0));
+ ASSERT_DOUBLE_EQ(0.0, ls.getDamping());
+ ASSERT_NO_THROW(ls.setDamping(0.45));
ASSERT_DOUBLE_EQ(0.45, ls.getDamping());
// Rest length getter/setter
- ls.setRestLength(1.23);
+ ASSERT_THROW(ls.setRestLength(-1.23), SurgSim::Framework::AssertionFailure);
+ ASSERT_NO_THROW(ls.setRestLength(0.0));
+ ASSERT_DOUBLE_EQ(0.0, ls.getRestLength());
+ ASSERT_NO_THROW(ls.setRestLength(1.23));
ASSERT_DOUBLE_EQ(1.23, ls.getRestLength());
// Operator ==/!= (with same node Ids)
@@ -207,6 +215,73 @@ TEST(LinearSpringTests, SetGetMethods)
ASSERT_TRUE(ls != ls3);
}
+namespace
+{
+void initializeTest(bool setStiffness, bool setDamping, bool setRestLength, bool expectException)
+{
+ LinearSpring ls(0, 1);
+ SurgSim::Math::OdeState state;
+ std::string scopeTrace;
+
+ state.setNumDof(3, 2);
+
+ if (setStiffness)
+ {
+ ls.setStiffness(1.23);
+ scopeTrace += "Stiffness set; ";
+ }
+ else
+ {
+ scopeTrace += "Stiffness unset; ";
+ }
+
+ if (setDamping)
+ {
+ ls.setDamping(1.23);
+ scopeTrace += "Damping set; ";
+ }
+ else
+ {
+ scopeTrace += "Damping unset; ";
+ }
+
+ if (setRestLength)
+ {
+ ls.setRestLength(1.23);
+ scopeTrace += "Rest length set; ";
+ }
+ else
+ {
+ scopeTrace += "Rest length unset; ";
+ }
+
+ SCOPED_TRACE(scopeTrace);
+ if (expectException)
+ {
+ EXPECT_THROW(ls.initialize(state), SurgSim::Framework::AssertionFailure);
+ }
+ else
+ {
+ EXPECT_NO_THROW(ls.initialize(state));
+ }
+}
+};
+TEST(LinearSpringTests, initializeTest)
+{
+ // (stiff damp restL| expectException)
+ initializeTest(false, false, false, true);
+
+ initializeTest(true, false, false, true);
+ initializeTest(false, true, false, true);
+ initializeTest(false, false, true, true);
+
+ initializeTest(true, true, false, true);
+ initializeTest(true, false, true, false);
+ initializeTest(false, true, true, true);
+
+ initializeTest(true, true, true, false);
+}
+
TEST(LinearSpringTests, computeMethods)
{
using SurgSim::Math::setSubVector;
@@ -228,71 +303,74 @@ TEST(LinearSpringTests, computeMethods)
// Calculating spring force
Vector3d expectedF3D(0.45563016177577925, 0.81221028838291076, 0.23772008440475439);
- Vector expectedF;
- expectedF.resize(6u);
+ Vector6d expectedF;
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;
+ "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);
+ 0.047808674990512064, 0.312562344690556770, 0.0021120285754494608,
+ 0.013992782924052311, 0.033501153469247251, 0.1987182250423049600;
+ Matrix66d expectedK;
+ 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);
+ setSubMatrix(expectedStiffness33, 1, 1, 3, 3, &expectedK);
+
+ Matrix zeroBlock = Matrix::Zero(6, 6);
+ SparseMatrix K(6, 6);
K.setZero();
+ SurgSim::Math::addSubMatrix(zeroBlock, 0, 0, &K, true);
+ K.makeCompressed();
+
ls.addStiffness(state, &K);
EXPECT_TRUE(K.isApprox(expectedK)) << " K = " << std::endl << K << std::endl <<
- "expectedK = " << std::endl << expectedK << 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);
+ 0.180267629566694950, 0.321346644010195360, 0.094052676295666937,
+ 0.052761257434154628, 0.094052676295666937, 0.027527612574341543;
+ Matrix66d expectedD;
+ 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);
+ setSubMatrix(expectedDamping33, 1, 1, 3, 3, &expectedD);
+
+ SparseMatrix D(6, 6);
D.setZero();
+ SurgSim::Math::addSubMatrix(zeroBlock, 0, 0, &D, true);
+ D.makeCompressed();
+
ls.addDamping(state, &D);
EXPECT_TRUE(D.isApprox(expectedD)) << " D = " << std::endl << D << std::endl <<
- "expectedD = " << std::endl << expectedD << 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();
+ Vector f = Vector::Zero(6);
+ SurgSim::Math::clearMatrix(&K);
+ SurgSim::Math::clearMatrix(&D);
ls.addFDK(state, &f, &D, &K);
EXPECT_TRUE(f.isApprox(expectedF)) << " F = " << f.transpose() << std::endl <<
- "expectedF = " << expectedF.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;
+ "expectedK = " << std::endl << expectedK << std::endl;
EXPECT_TRUE(D.isApprox(expectedD)) << " D = " << std::endl << D << std::endl <<
- "expectedD = " << std::endl << expectedD << std::endl;
+ "expectedD = " << std::endl << expectedD << std::endl;
}
// Test addMatVec method
- Vector ones(6), oneToSix(6);
- ones.setOnes();
- oneToSix.setLinSpaced(1.0, 6.0);
+ Vector ones = Vector::Ones(6);
+ Vector oneToSix = Vector::LinSpaced(6, 1.0, 6.0);
f.setZero();
ls.addMatVec(state, 1.0, 0.0, ones, &f);
@@ -301,8 +379,8 @@ TEST(LinearSpringTests, computeMethods)
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[" << row << "] = " << f[row] <<
+ " expectedValue = " << expectedD.row(row).sum();
}
}
f.setZero();
@@ -312,8 +390,8 @@ TEST(LinearSpringTests, computeMethods)
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[" << row << "] = " << f[row] <<
+ " expectedValue = " << expectedD.row(row).dot(oneToSix);
}
}
f.setZero();
@@ -323,8 +401,8 @@ TEST(LinearSpringTests, computeMethods)
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[" << row << "] = " << f[row] <<
+ " expectedValue = " << expectedK.row(row).sum();
}
}
f.setZero();
@@ -334,8 +412,8 @@ TEST(LinearSpringTests, computeMethods)
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[" << row << "] = " << f[row] <<
+ " expectedValue = " << expectedK.row(row).dot(oneToSix);
}
}
@@ -346,8 +424,8 @@ TEST(LinearSpringTests, computeMethods)
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[" << row << "] = " << f[row] <<
+ " expectedValue = " << 1.4 * expectedD.row(row).sum() + 4.1 * expectedK.row(row).sum();
}
}
f.setZero();
@@ -357,9 +435,11 @@ TEST(LinearSpringTests, computeMethods)
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);
+ 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);
}
}
}
@@ -383,20 +463,23 @@ TEST(LinearSpringTests, addStiffnessNumericalTest)
state.getVelocities().segment(0, 3) = v0;
state.getVelocities().segment(3, 3) = v1;
- Matrix K(6, 6);
+ Matrix zeroBlock = Matrix::Zero(6, 6);
+ SparseMatrix K(6, 6);
K.setZero();
+ SurgSim::Math::addSubMatrix(zeroBlock, 0, 0, &K, true);
+ K.makeCompressed();
+
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;
+ "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;
+ "K = " << std::endl << K << std::endl <<
+ "Knumeric = " << std::endl << Knumeric << std::endl;
}
TEST(LinearSpringTests, addDampingNumericalTest)
@@ -418,20 +501,22 @@ TEST(LinearSpringTests, addDampingNumericalTest)
state.getVelocities().segment(0, 3) = v0;
state.getVelocities().segment(3, 3) = v1;
- Matrix D(6, 6);
+ Matrix zeroBlock = Matrix::Zero(6, 6);
+ SparseMatrix D(6, 6);
D.setZero();
+ SurgSim::Math::addSubMatrix(zeroBlock, 0, 0, &D, true);
+ D.makeCompressed();
+
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;
+ "Dformal = " << std::endl << Dformal << std::endl <<
+ "Dnumeric = " << std::endl << 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;
+ "D = " << std::endl << D << std::endl <<
+ "Dnumeric = " << std::endl << Dnumeric << std::endl;
}
TEST(LinearSpringTests, addFDKNumericalTest)
@@ -453,33 +538,37 @@ TEST(LinearSpringTests, addFDKNumericalTest)
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();
+ Vector F = Vector::Zero(6);
+ Matrix zeroBlock = Matrix::Zero(6, 6);
+ SparseMatrix K(6, 6);
K.setZero();
+ SurgSim::Math::addSubMatrix(zeroBlock, 0, 0, &K, true);
+ K.makeCompressed();
+
+ SparseMatrix D(6, 6);
D.setZero();
+ SurgSim::Math::addSubMatrix(zeroBlock, 0, 0, &D, true);
+ D.makeCompressed();
+
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;
+ "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;
+ "K = " << std::endl << K << std::endl <<
+ "Knumeric = " << std::endl << 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;
+ "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;
+ "D = " << std::endl << D << std::endl <<
+ "Dnumeric = " << std::endl << Dnumeric << std::endl;
}
diff --git a/SurgSim/Physics/UnitTests/MassSpringConstraintFixedPointTest.cpp b/SurgSim/Physics/UnitTests/MassSpringConstraintFixedPointTest.cpp
new file mode 100644
index 0000000..1332252
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/MassSpringConstraintFixedPointTest.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 <memory>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Blocks/MassSpring1DRepresentation.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Math/RigidTransform.h"
+#include "SurgSim/Math/SparseMatrix.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/MassSpringConstraintFixedPoint.h"
+#include "SurgSim/Physics/MassSpringLocalization.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 MassSpringConstraintFixedPointTest : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ // 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_EULER_EXPLICIT_MODIFIED);
+
+ 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<MassSpringLocalization>(m_massSpring);
+ }
+
+ void setConstraintAtNode(size_t nodeId)
+ {
+ m_nodeId = nodeId;
+ m_localization->setLocalNode(nodeId);
+ }
+
+ ConstraintData 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<MassSpringLocalization> m_localization;
+};
+
+TEST_F(MassSpringConstraintFixedPointTest, ConstructorTest)
+{
+ ASSERT_NO_THROW({ MassSpringConstraintFixedPoint constraint; });
+
+ ASSERT_NE(nullptr, std::make_shared<MassSpringConstraintFixedPoint>());
+}
+
+TEST_F(MassSpringConstraintFixedPointTest, ConstraintConstantsTest)
+{
+ auto implementation = std::make_shared<MassSpringConstraintFixedPoint>();
+
+ EXPECT_EQ(SurgSim::Physics::FIXED_3DPOINT, implementation->getConstraintType());
+ EXPECT_EQ(3u, implementation->getNumDof());
+}
+
+TEST_F(MassSpringConstraintFixedPointTest, BuildMlcpTest)
+{
+ // Define constraint (frictionless non-penetration)
+ auto implementation = std::make_shared<MassSpringConstraintFixedPoint>();
+
+ // Initialize MLCP
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(m_massSpring->getNumDof(), 3, 1);
+
+ // Build MLCP for 0th node
+ setConstraintAtNode(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 new position. The constraint is
+ // U(t) = p(t) = 0
+ // U(1) = p(1)
+ Vector3d expectedViolation = m_extremities[0] - Vector3d::UnitY() * 9.81 * dt * dt;
+ EXPECT_TRUE(mlcpPhysicsProblem.b.isApprox(expectedViolation)) <<
+ "b = " << mlcpPhysicsProblem.b.transpose() <<
+ " expected = " << (-Vector3d::UnitY() * 9.81 * dt * dt).transpose();
+
+ // By definition, H = dU/dv.
+ // U(t+dt) = p(t+dt) = p(t) + dt.v(t) [using Backward Euler integration scheme for approximation]
+ //
+ // dU/dv = dt.Id(3x3)
+ // => H = dt.Id(3x3) for the constrained node and Zero(3x3) for all other nodes
+ SurgSim::Math::Matrix H = mlcpPhysicsProblem.H;
+ SurgSim::Math::Matrix expectedH = SurgSim::Math::Matrix::Zero(3, 6);
+ expectedH.block<3, 3>(0, 0) = SurgSim::Math::Matrix33d::Identity() * dt;
+ EXPECT_TRUE((H.isApprox(expectedH)));
+
+ // 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].Id(3x3) * F(0)
+ //
+ // Therefore C = dt/m.Id(6x6) for the entire system
+ // and
+ // CH^T = dt/m.(Id(3x3) 0 )* (dt.Id(3x3)) = dt^2/m (Id(3x3))
+ // ( 0 Id(3x3)) ( 0 ) ( 0 )
+ SurgSim::Math::Matrix expectedCHt = SurgSim::Math::Matrix::Zero(6, 3);
+ expectedCHt.block<3, 3>(0, 0) = SurgSim::Math::Matrix33d::Identity() * dt * dt / m_massPerNode;
+ EXPECT_TRUE(mlcpPhysicsProblem.CHt.isApprox(expectedCHt));
+
+ // And finally,
+ // HCHt = H * (dt/m).Id(6x6) * H^T = (dt.Id(3x3) 0) (dt^2/m.Id(3x3)) = dt^3/m.Id(3x3)
+ // ( 0 )
+ EXPECT_TRUE(mlcpPhysicsProblem.A.isApprox(dt * dt * dt / m_massPerNode * SurgSim::Math::Matrix33d::Identity()));
+
+ // 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(MassSpringConstraintFixedPointTest, BuildMlcpIndiciesTest)
+{
+ auto implementation = std::make_shared<MassSpringConstraintFixedPoint>();
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(11, 6, 3);
+
+ // Suppose 5 dof and 1 constraint are defined elsewhere. Then H, CHt, HCHt, and b are prebuilt.
+ Eigen::SparseVector<double, Eigen::RowMajor, ptrdiff_t> localH;
+ localH.resize(5);
+ localH.reserve(5);
+ localH.insert(0) = 0.9478;
+ localH.insert(1) = -0.3807;
+ localH.insert(2) = 0.5536;
+ localH.insert(3) = -0.6944;
+ localH.insert(4) = 0.1815;
+
+ mlcpPhysicsProblem.H.coeffRef(0, 0) += localH.coeff(0);
+ mlcpPhysicsProblem.H.coeffRef(0, 1) += localH.coeff(1);
+ mlcpPhysicsProblem.H.coeffRef(0, 2) += localH.coeff(2);
+ mlcpPhysicsProblem.H.coeffRef(0, 3) += localH.coeff(3);
+ mlcpPhysicsProblem.H.coeffRef(0, 4) += localH.coeff(4);
+
+ 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 (0-based).
+ size_t indexOfRepresentation = 5;
+ size_t indexOfConstraint = 1;
+
+ setConstraintAtNode(1);
+
+ implementation->build(dt, m_constraintData, m_localization,
+ &mlcpPhysicsProblem, indexOfRepresentation, indexOfConstraint, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
+
+ // b -> E -> [#constraints, 1]
+ const Vector3d newPosition = m_extremities[1] - Vector3d::UnitY() * 9.81 * dt * dt;
+ EXPECT_TRUE(mlcpPhysicsProblem.b.segment<3>(indexOfConstraint).isApprox(newPosition));
+
+ // H -> [#constraints, #dof]
+ SurgSim::Math::Matrix H = mlcpPhysicsProblem.H;
+ SurgSim::Math::Matrix expectedH = SurgSim::Math::Matrix::Zero(3, 6);
+ expectedH.block<3, 3>(0, 3) = SurgSim::Math::Matrix33d::Identity() * dt;
+ EXPECT_TRUE((H.block<3, 6>(indexOfConstraint, indexOfRepresentation).isApprox(expectedH))) << H;
+
+ // C -> [#dof, #dof]
+ // CHt -> [#dof, #constraints]
+ SurgSim::Math::Matrix CHt = mlcpPhysicsProblem.CHt;
+ SurgSim::Math::Matrix expectedCHt = SurgSim::Math::Matrix::Zero(6, 3);
+ expectedCHt.block<3, 3>(3, 0) = SurgSim::Math::Matrix33d::Identity() * dt * dt / m_massPerNode;
+ EXPECT_TRUE((CHt.block(indexOfRepresentation, indexOfConstraint, 6, 3).isApprox(expectedCHt))) << CHt;
+
+ // A -> HCHt -> [#constraints, #constraints]
+ SurgSim::Math::Matrix expectedA = dt * dt * dt / m_massPerNode * SurgSim::Math::Matrix33d::Identity();
+ EXPECT_TRUE(mlcpPhysicsProblem.A.block(1, 1, 3, 3).isApprox(expectedA));
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/MassSpringConstraintFrictionlessContactTest.cpp b/SurgSim/Physics/UnitTests/MassSpringConstraintFrictionlessContactTest.cpp
new file mode 100644
index 0000000..534be8f
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/MassSpringConstraintFrictionlessContactTest.cpp
@@ -0,0 +1,271 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/SparseMatrix.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/ContactConstraintData.h"
+#include "SurgSim/Physics/MassSpringConstraintFrictionlessContact.h"
+#include "SurgSim/Physics/MassSpringLocalization.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 MassSpringConstraintFrictionlessContactTest : 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_EULER_EXPLICIT_MODIFIED);
+
+ 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<MassSpringLocalization>(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<MassSpringLocalization> m_localization;
+};
+
+TEST_F(MassSpringConstraintFrictionlessContactTest, ConstructorTest)
+{
+ ASSERT_NO_THROW({ MassSpringConstraintFrictionlessContact massSpring; });
+
+ ASSERT_NE(nullptr, std::make_shared<MassSpringConstraintFrictionlessContact>());
+}
+
+TEST_F(MassSpringConstraintFrictionlessContactTest, ConstraintConstantsTest)
+{
+ auto implementation = std::make_shared<MassSpringConstraintFrictionlessContact>();
+
+ EXPECT_EQ(SurgSim::Physics::FRICTIONLESS_3DCONTACT, implementation->getConstraintType());
+ EXPECT_EQ(1u, implementation->getNumDof());
+}
+
+TEST_F(MassSpringConstraintFrictionlessContactTest, BuildMlcpTest)
+{
+ // Define constraint (frictionless non-penetration)
+ auto implementation = std::make_shared<MassSpringConstraintFrictionlessContact>();
+
+ // 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 projection of the new position onto the constraint normal. Note, we have
+ // defined the plane to intersect with p(0). The constraint is
+ // U(t) = n^t.p(t) >= 0
+ // U(1) = n^t.p(1)
+ const Vector3d newPosition = m_extremities[0] - Vector3d::UnitY() * 9.81 * dt * dt;
+ EXPECT_NEAR(newPosition.dot(m_n), 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
+ SurgSim::Math::Matrix h = mlcpPhysicsProblem.H;
+ EXPECT_NEAR(m_n[0], h(0, 0), epsilon);
+ EXPECT_NEAR(m_n[1], h(0, 1), epsilon);
+ EXPECT_NEAR(m_n[2], 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(MassSpringConstraintFrictionlessContactTest, BuildMlcpIndiciesTest)
+{
+ auto implementation = std::make_shared<MassSpringConstraintFrictionlessContact>();
+
+ 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::SparseVector<double, Eigen::RowMajor, ptrdiff_t> localH;
+ localH.resize(5);
+ localH.reserve(5);
+ localH.insert(0) = 0.9478;
+ localH.insert(1) = -0.3807;
+ localH.insert(2) = 0.5536;
+ localH.insert(3) = -0.6944;
+ localH.insert(4) = 0.1815;
+
+ mlcpPhysicsProblem.H.coeffRef(0, 0) += localH.coeff(0);
+ mlcpPhysicsProblem.H.coeffRef(0, 1) += localH.coeff(1);
+ mlcpPhysicsProblem.H.coeffRef(0, 2) += localH.coeff(2);
+ mlcpPhysicsProblem.H.coeffRef(0, 3) += localH.coeff(3);
+ mlcpPhysicsProblem.H.coeffRef(0, 4) += localH.coeff(4);
+
+ 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]
+ const Vector3d newPosition = m_extremities[1] - Vector3d::UnitY() * 9.81 * dt * dt;
+ EXPECT_NEAR(newPosition.dot(m_n), mlcpPhysicsProblem.b[indexOfConstraint], epsilon);
+
+ // H -> [#constraints, #dof]
+ SurgSim::Math::Matrix h = mlcpPhysicsProblem.H;
+ EXPECT_NEAR(m_n[0], h(indexOfConstraint, indexOfRepresentation + 3 * m_nodeId + 0), epsilon);
+ EXPECT_NEAR(m_n[1], h(indexOfConstraint, indexOfRepresentation + 3 * m_nodeId + 1), epsilon);
+ EXPECT_NEAR(m_n[2], 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/MassSpringLocalizationTest.cpp b/SurgSim/Physics/UnitTests/MassSpringLocalizationTest.cpp
new file mode 100644
index 0000000..f4fbb78
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/MassSpringLocalizationTest.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 <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+
+#include "SurgSim/Blocks/MassSpring1DRepresentation.h"
+#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/MassSpringLocalization.h"
+#include "SurgSim/Physics/MassSpringRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+TEST (MassSpringLocalizationTest, ConstructorTest)
+{
+ ASSERT_NO_THROW( {MassSpringLocalization localization;});
+
+ ASSERT_NO_THROW(
+ {
+ auto massSpring = std::make_shared<MassSpringRepresentation>("MassSpringRepresentation");
+ MassSpringLocalization localization(massSpring);
+ });
+}
+
+TEST (MassSpringLocalizationTest, SetGetRepresentationTest)
+{
+ MassSpringLocalization 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 (MassSpringLocalizationTest, CalculatePositionTest)
+{
+ 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
+
+ MassSpringLocalization localization = MassSpringLocalization(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));
+
+ // Out-Of-Range assertions
+ EXPECT_THROW(localization.calculatePosition(-0.01), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(localization.calculatePosition(1.01), SurgSim::Framework::AssertionFailure);
+}
+
+TEST(MassSpringLocalizationTest, CalculateVelocityTest)
+{
+ 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
+
+ auto state = std::make_shared<Math::OdeState>(*massSpring->getInitialState());
+ state->getVelocities().segment<3>(0) = Vector3d(0, 0, 0);
+ state->getVelocities().segment<3>(3) = Vector3d(1, 0, 0);
+ massSpring->setInitialState(state);
+
+ MassSpringLocalization localization = MassSpringLocalization(massSpring);
+
+ localization.setLocalNode(0);
+ ASSERT_EQ(0u, localization.getLocalNode());
+ ASSERT_TRUE(localization.calculateVelocity().isZero(epsilon));
+
+ localization.setLocalNode(1);
+ ASSERT_EQ(1u, localization.getLocalNode());
+ ASSERT_TRUE(localization.calculateVelocity().isApprox(Vector3d(1.0, 0.0, 0.0), epsilon));
+
+ // Out-Of-Range assertions
+ EXPECT_THROW(localization.calculateVelocity(-0.01), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(localization.calculateVelocity(1.01), SurgSim::Framework::AssertionFailure);
+}
+
+TEST (MassSpringLocalizationTest, IsValidRepresentationTest)
+{
+ MassSpringLocalization 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/MassSpringMechanicalValidationTests.cpp b/SurgSim/Physics/UnitTests/MassSpringMechanicalValidationTests.cpp
index 7f4da0f..e76c1da 100644
--- a/SurgSim/Physics/UnitTests/MassSpringMechanicalValidationTests.cpp
+++ b/SurgSim/Physics/UnitTests/MassSpringMechanicalValidationTests.cpp
@@ -158,7 +158,7 @@ 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);
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT);
m.setIsGravityEnabled(false);
@@ -189,7 +189,7 @@ TEST_F(MassSpringMechanicalValidationTests, OneSpringFrequencyTest)
// (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);
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED);
// 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);
@@ -265,7 +265,7 @@ TEST_F(MassSpringMechanicalValidationTests, FallingTest)
MockMassSpring m("MassSpring", m_poseRandom, m_numNodes, m_nodeBoundaryConditions, m_totalMass,
m_rayleighDampingMass, m_rayleighDampingStiffness, m_springStiffness, m_springDamping,
- SurgSim::Math::INTEGRATIONSCHEME_EXPLICIT_EULER);
+ SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT);
m.initialize(std::make_shared<SurgSim::Framework::Runtime>());
m.wakeUp();
@@ -299,20 +299,20 @@ 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);
+ runEnergyTest(SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT, 1);
}
{
SCOPED_TRACE("Testing energy decrease for Euler implicit");
// For Euler implicit, the energy should decrease
- runEnergyTest(SurgSim::Math::INTEGRATIONSCHEME_IMPLICIT_EULER, -1);
+ runEnergyTest(SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT, -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);
+ runEnergyTest(SurgSim::Math::INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED, 0);
}
}
diff --git a/SurgSim/Physics/UnitTests/MassSpringRepresentationContactTest.cpp b/SurgSim/Physics/UnitTests/MassSpringRepresentationContactTest.cpp
deleted file mode 100644
index 319fdc2..0000000
--- a/SurgSim/Physics/UnitTests/MassSpringRepresentationContactTest.cpp
+++ /dev/null
@@ -1,259 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index c659032..0000000
--- a/SurgSim/Physics/UnitTests/MassSpringRepresentationLocalizationTest.cpp
+++ /dev/null
@@ -1,107 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
index 71bf62c..72ca1c1 100644
--- a/SurgSim/Physics/UnitTests/MassSpringRepresentationTests.cpp
+++ b/SurgSim/Physics/UnitTests/MassSpringRepresentationTests.cpp
@@ -21,14 +21,15 @@
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/MassSpringLocalization.h"
#include "SurgSim/Physics/MassSpringRepresentation.h"
-#include "SurgSim/Physics/MassSpringRepresentationLocalization.h"
+#include "SurgSim/Physics/UnitTests/DeformableTestsUtility.h"
#include "SurgSim/Physics/UnitTests/MockObjects.h"
using SurgSim::Math::Vector;
using SurgSim::Math::Matrix;
+using SurgSim::Physics::MassSpringLocalization;
using SurgSim::Physics::MassSpringRepresentation;
-using SurgSim::Physics::MassSpringRepresentationLocalization;
using SurgSim::Physics::MockLocalization;
using SurgSim::Physics::MockMassSpring;
using SurgSim::Physics::MockSpring;
@@ -56,9 +57,9 @@ protected:
std::shared_ptr<MockMassSpring> m_massSpring;
std::shared_ptr<SurgSim::Math::OdeState> m_initialState;
- std::shared_ptr<SurgSim::Physics::MassSpringRepresentationLocalization> m_localization;
+ std::shared_ptr<SurgSim::Physics::MassSpringLocalization> m_localization;
- virtual void SetUp() override
+ void SetUp() override
{
m_massSpring = std::make_shared<MockMassSpring>();
@@ -124,7 +125,7 @@ protected:
void createLocalization()
{
- m_localization = std::make_shared<SurgSim::Physics::MassSpringRepresentationLocalization>();
+ m_localization = std::make_shared<SurgSim::Physics::MassSpringLocalization>();
m_localization->setRepresentation(m_massSpring);
m_localization->setLocalNode(0);
}
@@ -137,7 +138,8 @@ TEST_F(MassSpringRepresentationTests, Constructor)
ASSERT_NO_THROW({MassSpringRepresentation* m = new MassSpringRepresentation("MassSpring"); delete m;});
ASSERT_NO_THROW({std::shared_ptr<MassSpringRepresentation> m =
- std::make_shared<MassSpringRepresentation>("MassSpring");});
+ std::make_shared<MassSpringRepresentation>("MassSpring");
+ });
}
TEST_F(MassSpringRepresentationTests, SetGetMethods)
@@ -199,9 +201,6 @@ TEST_F(MassSpringRepresentationTests, SetGetMethods)
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)
@@ -265,10 +264,8 @@ TEST_F(MassSpringRepresentationTests, TransformInitialStateTest)
expectedV.segment<3>(numDofPerNode * nodeId) = initialPose.linear() * v.segment<3>(numDofPerNode * nodeId);
}
- // Initialize the component
+ // Initialize the component => apply the pose to the initial state
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));
@@ -277,11 +274,11 @@ TEST_F(MassSpringRepresentationTests, TransformInitialStateTest)
TEST_F(MassSpringRepresentationTests, ExternalForceAPITest)
{
using SurgSim::Math::Vector;
- using SurgSim::Math::Matrix;
+ using SurgSim::Math::SparseMatrix;
std::shared_ptr<MockMassSpring> massSpring = std::make_shared<MockMassSpring>();
- std::shared_ptr<MassSpringRepresentationLocalization> localization =
- std::make_shared<MassSpringRepresentationLocalization>();
+ std::shared_ptr<MassSpringLocalization> localization =
+ std::make_shared<MassSpringLocalization>();
std::shared_ptr<MockLocalization> wrongLocalizationType =
std::make_shared<MockLocalization>();
@@ -297,6 +294,9 @@ TEST_F(MassSpringRepresentationTests, ExternalForceAPITest)
massSpring->addSpring(std::make_shared<SurgSim::Physics::LinearSpring>(0, 1));
// Vector initialized (properly sized and zeroed)
+ SparseMatrix zeroMatrix(static_cast<SparseMatrix::Index>(massSpring->getNumDof()),
+ static_cast<SparseMatrix::Index>(massSpring->getNumDof()));
+ zeroMatrix.setZero();
EXPECT_NE(0, massSpring->getExternalForce().size());
EXPECT_NE(0, massSpring->getExternalStiffness().size());
EXPECT_NE(0, massSpring->getExternalDamping().size());
@@ -306,8 +306,8 @@ TEST_F(MassSpringRepresentationTests, ExternalForceAPITest)
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());
+ EXPECT_TRUE(massSpring->getExternalStiffness().isApprox(zeroMatrix));
+ EXPECT_TRUE(massSpring->getExternalDamping().isApprox(zeroMatrix));
localization->setRepresentation(massSpring);
localization->setLocalNode(0);
@@ -324,14 +324,14 @@ TEST_F(MassSpringRepresentationTests, ExternalForceAPITest)
expectedD.block(0, 0, massSpring->getNumDofPerNode(), massSpring->getNumDofPerNode()) = D;
ASSERT_THROW(massSpring->addExternalGeneralizedForce(nullptr, F, K, D),
- SurgSim::Framework::AssertionFailure);
+ SurgSim::Framework::AssertionFailure);
ASSERT_THROW(massSpring->addExternalGeneralizedForce(wrongLocalizationType, F, K, D),
- SurgSim::Framework::AssertionFailure);
+ 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_FALSE(massSpring->getExternalStiffness().isApprox(zeroMatrix));
+ EXPECT_FALSE(massSpring->getExternalDamping().isApprox(zeroMatrix));
EXPECT_TRUE(massSpring->getExternalForce().isApprox(expectedF));
EXPECT_TRUE(massSpring->getExternalStiffness().isApprox(expectedK));
EXPECT_TRUE(massSpring->getExternalDamping().isApprox(expectedD));
@@ -345,9 +345,7 @@ TEST_F(MassSpringRepresentationTests, ExternalForceAPITest)
TEST_F(MassSpringRepresentationTests, ComputesWithNoGravityAndNoDampingTest)
{
using SurgSim::Math::Vector3d;
-
- Vector *F;
- Matrix *M, *D, *K;
+ using SurgSim::Math::SparseMatrix;
addMasses();
addSprings();
@@ -362,16 +360,8 @@ TEST_F(MassSpringRepresentationTests, ComputesWithNoGravityAndNoDampingTest)
{
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));
+ testOdeEquationUpdate(m_massSpring, *m_initialState, m_expectedSpringsForce, m_expectedMass,
+ m_expectedDamping, m_expectedStiffness);
}
size_t numDofPerNode = m_massSpring->getNumDofPerNode();
@@ -390,24 +380,16 @@ TEST_F(MassSpringRepresentationTests, ComputesWithNoGravityAndNoDampingTest)
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));
+
+ testOdeEquationUpdate(m_massSpring, *m_initialState, m_expectedSpringsForce + externalForce, m_expectedMass,
+ m_expectedDamping + externalD, m_expectedStiffness + externalK);
}
}
TEST_F(MassSpringRepresentationTests, ComputesWithNoGravityAndDampingTest)
{
using SurgSim::Math::Vector3d;
-
- Vector *F;
- Matrix *M, *D, *K;
+ using SurgSim::Math::SparseMatrix;
addMasses();
addSprings();
@@ -426,17 +408,8 @@ TEST_F(MassSpringRepresentationTests, ComputesWithNoGravityAndDampingTest)
{
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));
+ testOdeEquationUpdate(m_massSpring, *m_initialState, expectedForce, m_expectedMass,
+ m_expectedDamping + m_expectedRayleighDamping, m_expectedStiffness);
}
size_t numDofPerNode = m_massSpring->getNumDofPerNode();
@@ -455,24 +428,16 @@ TEST_F(MassSpringRepresentationTests, ComputesWithNoGravityAndDampingTest)
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));
+
+ testOdeEquationUpdate(m_massSpring, *m_initialState, expectedForce + externalForce, m_expectedMass,
+ m_expectedDamping + m_expectedRayleighDamping + externalD, m_expectedStiffness + externalK);
}
}
TEST_F(MassSpringRepresentationTests, ComputesWithGravityAndNoDampingTest)
{
using SurgSim::Math::Vector3d;
-
- Vector *F;
- Matrix *M, *D, *K;
+ using SurgSim::Math::SparseMatrix;
addMasses();
addSprings();
@@ -489,16 +454,8 @@ TEST_F(MassSpringRepresentationTests, ComputesWithGravityAndNoDampingTest)
{
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));
+ testOdeEquationUpdate(m_massSpring, *m_initialState, expectedForce, m_expectedMass,
+ m_expectedDamping, m_expectedStiffness);
}
size_t numDofPerNode = m_massSpring->getNumDofPerNode();
@@ -517,23 +474,16 @@ TEST_F(MassSpringRepresentationTests, ComputesWithGravityAndNoDampingTest)
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));
+
+ testOdeEquationUpdate(m_massSpring, *m_initialState, expectedForce + externalForce, m_expectedMass,
+ m_expectedDamping + externalD, m_expectedStiffness + externalK);
}
}
TEST_F(MassSpringRepresentationTests, ComputesWithGravityAndDampingTest)
{
using SurgSim::Math::Vector3d;
-
- Vector *F;
- Matrix *M, *D, *K;
+ using SurgSim::Math::SparseMatrix;
addMasses();
addSprings();
@@ -548,22 +498,13 @@ TEST_F(MassSpringRepresentationTests, ComputesWithGravityAndDampingTest)
m_massSpring->wakeUp();
SurgSim::Math::Vector expectedForce = m_expectedSpringsForce + m_expectedRayleighDampingForce +
- m_expectedGravityForce;
+ 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));
+ testOdeEquationUpdate(m_massSpring, *m_initialState, expectedForce, m_expectedMass,
+ m_expectedDamping + m_expectedRayleighDamping, m_expectedStiffness);
}
size_t numDofPerNode = m_massSpring->getNumDofPerNode();
@@ -582,14 +523,8 @@ TEST_F(MassSpringRepresentationTests, ComputesWithGravityAndDampingTest)
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));
+
+ testOdeEquationUpdate(m_massSpring, *m_initialState, expectedForce + externalForce, m_expectedMass,
+ m_expectedDamping + m_expectedRayleighDamping + externalD, m_expectedStiffness + externalK);
}
}
diff --git a/SurgSim/Physics/UnitTests/MockObjects.cpp b/SurgSim/Physics/UnitTests/MockObjects.cpp
index 7d7d683..d99843f 100644
--- a/SurgSim/Physics/UnitTests/MockObjects.cpp
+++ b/SurgSim/Physics/UnitTests/MockObjects.cpp
@@ -14,6 +14,7 @@
// limitations under the License.
#include "SurgSim/Physics/UnitTests/MockObjects.h"
+#include "SurgSim/Math/Shape.h"
#include "SurgSim/Physics/FemPlyReaderDelegate.h"
namespace SurgSim
@@ -25,10 +26,10 @@ SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Physics::MockDeformable
MockDeformableRepresentation);
MockRepresentation::MockRepresentation(const std::string& name) :
- Representation(name),
- m_preUpdateCount(0),
- m_updateCount(0),
- m_postUpdateCount(0)
+ Representation(name),
+ m_preUpdateCount(0),
+ m_updateCount(0),
+ m_postUpdateCount(0)
{
}
@@ -36,11 +37,6 @@ MockRepresentation::~MockRepresentation()
{
}
-RepresentationType MockRepresentation::getType() const
-{
- return REPRESENTATION_TYPE_FIXED;
-}
-
void MockRepresentation::beforeUpdate(double dt)
{
m_preUpdateCount++;
@@ -71,85 +67,101 @@ int MockRepresentation::getPostUpdateCount() const
return m_postUpdateCount;
}
+std::shared_ptr<Localization> MockRepresentation::createLocalization(
+ const SurgSim::DataStructures::Location& location)
+{
+ return std::make_shared<MockLocalization>();
+}
MockRigidRepresentation::MockRigidRepresentation() : RigidRepresentation("MockRigidRepresentation")
{
}
-RigidRepresentationState& MockRigidRepresentation::getInitialState()
+RigidState& MockRigidRepresentation::getInitialState()
{
return m_initialState;
}
-RigidRepresentationState& MockRigidRepresentation::getCurrentState()
+RigidState& MockRigidRepresentation::getCurrentState()
{
return m_currentState;
}
-RigidRepresentationState& MockRigidRepresentation::getPreviousState()
+RigidState& MockRigidRepresentation::getPreviousState()
{
return m_previousState;
}
+MockFixedRepresentation::MockFixedRepresentation() : FixedRepresentation("MockFixedRepresentation")
+{
+}
+
+RigidState& MockFixedRepresentation::getInitialState()
+{
+ return m_initialState;
+}
+
+RigidState& MockFixedRepresentation::getCurrentState()
+{
+ return m_currentState;
+}
+
+RigidState& MockFixedRepresentation::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);
-}
+ m_f = Vector::LinSpaced(3, 1.0, 3.0);
+ m_M.resize(3, 3);
+ m_M.setIdentity();
-RepresentationType MockDeformableRepresentation::getType() const
-{
- return SurgSim::Physics::REPRESENTATION_TYPE_INVALID;
+ m_D.resize(3, 3);
+ m_D.setIdentity();
+
+ m_K.resize(3, 3);
+ m_K.setIdentity();
}
void MockDeformableRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
- SurgSim::Math::Vector& generalizedForce,
- const SurgSim::Math::Matrix& K,
- const SurgSim::Math::Matrix& D)
+ const Math::Vector& generalizedForce,
+ const Math::Matrix& K,
+ const Math::Matrix& D)
{
- std::shared_ptr<MockDeformableRepresentationLocalization> loc =
- std::dynamic_pointer_cast<MockDeformableRepresentationLocalization>(localization);
+ std::shared_ptr<MockDeformableLocalization> loc =
+ std::dynamic_pointer_cast<MockDeformableLocalization>(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;
+ Math::addSubMatrix(K, static_cast<SparseMatrix::Index>(loc->getLocalNode()),
+ static_cast<SparseMatrix::Index>(loc->getLocalNode()),
+ &m_externalGeneralizedStiffness, true);
+ Math::addSubMatrix(D, static_cast<SparseMatrix::Index>(loc->getLocalNode()),
+ static_cast<SparseMatrix::Index>(loc->getLocalNode()),
+ &m_externalGeneralizedDamping, true);
+ m_hasExternalGeneralizedForce = true;
}
-Vector& MockDeformableRepresentation::computeF(const OdeState& state)
+void MockDeformableRepresentation::computeF(const OdeState& state)
{
- return m_F;
}
-const Matrix& MockDeformableRepresentation::computeM(const OdeState& state)
+void MockDeformableRepresentation::computeM(const OdeState& state)
{
- return m_M;
}
-const Matrix& MockDeformableRepresentation::computeD(const OdeState& state)
+void MockDeformableRepresentation::computeD(const OdeState& state)
{
- return m_D;
}
-const Matrix& MockDeformableRepresentation::computeK(const OdeState& state)
+void MockDeformableRepresentation::computeK(const OdeState& state)
{
- return m_K;
}
-void MockDeformableRepresentation::computeFMDK(const OdeState& state,
- Vector** f,
- Matrix** M,
- Matrix** D,
- Matrix** K)
+void MockDeformableRepresentation::computeFMDK(const OdeState& state)
{
- *f = &m_F;
- *M = &m_M;
- *D = &m_D;
- *K = &m_K;
}
void MockDeformableRepresentation::transformState(std::shared_ptr<OdeState> state, const RigidTransform3d& transform)
@@ -175,8 +187,14 @@ void MockDeformableRepresentation::transformState(std::shared_ptr<OdeState> stat
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;
+
+ m_D.resize(6, 6);
+ m_D.setIdentity();
+ m_D *= 2.0;
+
+ m_K.resize(6, 6);
+ m_K.setIdentity();
+ m_K *= 3.0;
}
void MockSpring::addNode(size_t nodeId)
@@ -189,17 +207,48 @@ 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)
+void MockSpring::addDamping(const OdeState& state, SparseMatrix* D, double scale)
{
- SurgSim::Math::addSubMatrix(scale * m_D, m_nodeIds, 3, D);
+ Matrix scaledDense(m_D.rows(), m_D.cols());
+ scaledDense = scale * m_D;
+
+ int index1 = 0;
+ for (auto nodeId1 : m_nodeIds)
+ {
+ int index2 = 0;
+ for (auto nodeId2 : m_nodeIds)
+ {
+ Math::addSubMatrix(scaledDense.block(3 * index1, 3 * index2, 3, 3),
+ static_cast<SparseMatrix::Index>(nodeId1),
+ static_cast<SparseMatrix::Index>(nodeId2), D, false);
+ ++index2;
+ }
+ ++index1;
+ }
}
-void MockSpring::addStiffness(const OdeState& state, Matrix* K, double scale)
+void MockSpring::addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::SparseMatrix* K,
+ double scale)
{
- SurgSim::Math::addSubMatrix(scale * m_K, m_nodeIds, 3, K);
+ Matrix scaledDense(m_K.rows(), m_K.cols());
+ scaledDense = scale * m_K;
+
+ int index1 = 0;
+ for (auto nodeId1 : m_nodeIds)
+ {
+ int index2 = 0;
+ for (auto nodeId2 : m_nodeIds)
+ {
+ Math::addSubMatrix(scaledDense.block(3 * index1, 3 * index2, 3, 3),
+ static_cast<SparseMatrix::Index>(nodeId1),
+ static_cast<SparseMatrix::Index>(nodeId2), K, false);
+ ++index2;
+ }
+ ++index1;
+ }
}
-void MockSpring::addFDK(const OdeState& state, Vector* f, Matrix* D, Matrix* K)
+void MockSpring::addFDK(const OdeState& state, Vector* f, SparseMatrix* D, SparseMatrix* K)
{
addForce(state, f);
addDamping(state, D);
@@ -221,7 +270,7 @@ MockMassSpring::MockMassSpring(const std::string& name,
double rayleighDampingMass, double rayleighDampingStiffness,
double springStiffness, double springDamping,
SurgSim::Math::IntegrationScheme integrationScheme) :
- SurgSim::Physics::MassSpringRepresentation(name)
+ SurgSim::Physics::MassSpringRepresentation(name)
{
using SurgSim::Math::getSubVector;
using SurgSim::Math::setSubVector;
@@ -236,7 +285,7 @@ MockMassSpring::MockMassSpring(const std::string& name,
state->setNumDof(3, numNodes);
for (size_t i = 0; i < numNodes; i++)
{
- Vector3d p(static_cast<double>(i)/static_cast<double>(numNodes), 0, 0);
+ 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));
}
@@ -246,12 +295,12 @@ MockMassSpring::MockMassSpring(const std::string& name,
}
for (size_t i = 0; i < numNodes - 1; i++)
{
- std::shared_ptr<LinearSpring> spring = std::make_shared<LinearSpring>(i, i+1);
+ 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() );
+ const Vector3d& xj = getSubVector(state->getPositions(), i + 1, 3);
+ spring->setRestLength((xj - xi).norm());
addSpring(spring);
}
setInitialState(state);
@@ -269,77 +318,77 @@ const Vector3d& MockMassSpring::getGravityVector() const
return getGravity();
}
-
-MockFemElement::MockFemElement() : FemElement(), m_isInitialized(false)
+void MockMassSpring::clearFMDK()
{
- setNumDofPerNode(3);
+ m_f.setZero();
+ SurgSim::Math::clearMatrix(&m_M);
+ SurgSim::Math::clearMatrix(&m_D);
+ SurgSim::Math::clearMatrix(&m_K);
}
-void MockFemElement::addNode(size_t nodeId)
+MockFemElement::MockFemElement() : FemElement()
{
- this->m_nodeIds.push_back(nodeId);
+ initializeMembers();
}
-double MockFemElement::getVolume(const OdeState& state) const
+MockFemElement::MockFemElement(std::shared_ptr<FemElementStructs::FemElementParameter> elementData) : FemElement()
{
- return 1;
+ initializeMembers();
+ m_nodeIds.assign(elementData->nodeIds.begin(), elementData->nodeIds.end());
}
-void MockFemElement::addForce(const OdeState& state, Vector* F, double scale)
+void MockFemElement::addNode(size_t nodeId)
{
- SurgSim::Math::addSubVector(scale * m_F, m_nodeIds, 3, F);
+ this->m_nodeIds.push_back(nodeId);
}
-void MockFemElement::addMass(const OdeState& state, Matrix* M, double scale)
+double MockFemElement::getVolume(const OdeState& state) const
{
- SurgSim::Math::addSubMatrix(scale * m_M, m_nodeIds, 3, M);
+ return 1;
}
-void MockFemElement::addDamping(const OdeState& state, Matrix* D, double scale)
+Vector MockFemElement::computeCartesianCoordinate(const OdeState& state, const Vector& barycentricCoordinate) const
{
- SurgSim::Math::addSubMatrix(scale * m_D, m_nodeIds, 3, D);
-}
+ SURGSIM_ASSERT(getNumNodes() == static_cast<size_t>(barycentricCoordinate.size()));
-void MockFemElement::addStiffness(const OdeState& state, Matrix* K, double scale)
-{
- SurgSim::Math::addSubMatrix(scale * m_K, m_nodeIds, 3, K);
-}
+ Vector result = Vector::Zero(3);
+ for (size_t node = 0; node < getNumNodes(); node++)
+ {
+ result += state.getPosition(getNodeId(node)) * barycentricCoordinate[node];
+ }
-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);
+ return result;
}
-void MockFemElement::addMatVec(const OdeState& state, double alphaM, double alphaD, double alphaK,
- const Vector& x, Vector* F)
+Vector MockFemElement::computeNaturalCoordinate(const OdeState& state, const Vector& globalCoordinate) const
{
- 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);
+ return SurgSim::Math::Vector3d::Zero();
}
-Vector MockFemElement::computeCartesianCoordinate(const OdeState& state, const Vector &barycentricCoordinate) const
+void MockFemElement::initializeMembers()
{
- return SurgSim::Math::Vector3d::Zero();
+ m_isInitialized = false;
+ m_useDamping = true;
+ setNumDofPerNode(3);
}
-Vector MockFemElement::computeNaturalCoordinate(const OdeState& state, const Vector &globalCoordinate) const
+void MockFemElement::doUpdateFMDK(const Math::OdeState& state, int options)
{
- 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_f = Vector::LinSpaced(numDof, 1.0, static_cast<double>(numDof));
+ m_M.setIdentity();
+
+ m_D.setIdentity();
+ m_D *= 2.0;
+
+ m_K.setIdentity();
+ m_K *= 3.0;
+
m_isInitialized = true;
}
@@ -348,35 +397,43 @@ bool MockFemElement::isInitialized() const
return m_isInitialized;
}
-bool InvalidMockFemElement::update(const SurgSim::Math::OdeState& state)
+MockFemRepresentation::MockFemRepresentation(const std::string& name)
+ : FemRepresentation(name), m_setInitialStateCalled(false)
{
- return false;
+ this->m_numDofPerNode = 3;
}
+MockFemRepresentation::~MockFemRepresentation()
+{
+}
-MockFemRepresentation::MockFemRepresentation(const std::string& name) : FemRepresentation(name)
+void MockFemRepresentation::setInitialState(std::shared_ptr<SurgSim::Math::OdeState> initialState)
{
- this->m_numDofPerNode = 3;
+ FemRepresentation::setInitialState(initialState);
+ m_setInitialStateCalled = true;
}
-MockFemRepresentation::~MockFemRepresentation()
+void MockFemRepresentation::loadFem(const std::string &filename)
{
}
void MockFemRepresentation::addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
- SurgSim::Math::Vector& generalizedForce,
- const SurgSim::Math::Matrix& K,
- const SurgSim::Math::Matrix& D)
+ const 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);
+ std::shared_ptr<MockDeformableLocalization> loc =
+ std::dynamic_pointer_cast<MockDeformableLocalization>(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;
+ SurgSim::Math::addSubMatrix(K, static_cast<SparseMatrix::Index>(loc->getLocalNode()),
+ static_cast<SparseMatrix::Index>(loc->getLocalNode()),
+ &m_externalGeneralizedStiffness, true);
+ SurgSim::Math::addSubMatrix(D, static_cast<SparseMatrix::Index>(loc->getLocalNode()),
+ static_cast<SparseMatrix::Index>(loc->getLocalNode()),
+ &m_externalGeneralizedDamping, true);
+ m_hasExternalGeneralizedForce = true;
}
std::shared_ptr<FemPlyReaderDelegate> MockFemRepresentation::getDelegate()
@@ -384,59 +441,76 @@ std::shared_ptr<FemPlyReaderDelegate> MockFemRepresentation::getDelegate()
return nullptr;
}
-RepresentationType MockFemRepresentation::getType() const
+const std::vector<double>& MockFemRepresentation::getMassPerNode() const
{
- return REPRESENTATION_TYPE_INVALID;
+ return m_massPerNode;
}
-std::shared_ptr<OdeSolver> MockFemRepresentation::getOdeSolver() const
+void MockFemRepresentation::clearFMDK()
{
- return this->m_odeSolver;
+ m_f.setZero();
+ SurgSim::Math::clearMatrix(&m_M);
+ SurgSim::Math::clearMatrix(&m_D);
+ SurgSim::Math::clearMatrix(&m_K);
}
-const std::vector<double>& MockFemRepresentation::getMassPerNode() const
+void MockFemRepresentation::transformState(std::shared_ptr<OdeState> state, const RigidTransform3d& transform)
{
- return m_massPerNode;
}
-void MockFemRepresentation::transformState(std::shared_ptr<OdeState> state, const RigidTransform3d& transform)
+bool MockFemRepresentation::hasSetInitialStateBeenCalled()
{
+ return m_setInitialStateCalled;
+}
+
+SurgSim::Math::Matrix
+MockFemRepresentationValidComplianceWarping::getNodeTransformation(const SurgSim::Math::OdeState& state, size_t nodeId)
+{
+ return SurgSim::Math::Matrix::Identity(getNumDofPerNode(), getNumDofPerNode());
}
MockFem1DRepresentation::MockFem1DRepresentation(const std::string& name) : SurgSim::Physics::Fem1DRepresentation(name)
{
}
-const std::shared_ptr<OdeSolver> MockFem1DRepresentation::getOdeSolver() const
+bool MockFem1DRepresentation::doInitialize()
+{
+ return Fem1DRepresentation::doInitialize();
+}
+
+double MockFem1DRepresentation::getMassPerNode(size_t nodeId)
{
- return this->m_odeSolver;
+ return m_massPerNode[nodeId];
}
+MockFem2DRepresentation::MockFem2DRepresentation(const std::string& name) : SurgSim::Physics::Fem2DRepresentation(name)
+{
+}
-MockFixedConstraintBilateral3D::MockFixedConstraintBilateral3D() : ConstraintImplementation()
+double MockFem2DRepresentation::getMassPerNode(size_t nodeId)
{
+ return m_massPerNode[nodeId];
}
-MockFixedConstraintBilateral3D::~MockFixedConstraintBilateral3D()
+MockFixedConstraintFixedPoint::MockFixedConstraintFixedPoint() : ConstraintImplementation()
{
}
-SurgSim::Math::MlcpConstraintType MockFixedConstraintBilateral3D::getMlcpConstraintType() const
+MockFixedConstraintFixedPoint::~MockFixedConstraintFixedPoint()
{
- return SurgSim::Math::MLCP_BILATERAL_3D_CONSTRAINT;
}
-RepresentationType MockFixedConstraintBilateral3D::getRepresentationType() const
+SurgSim::Physics::ConstraintType MockFixedConstraintFixedPoint::getConstraintType() const
{
- return REPRESENTATION_TYPE_FIXED;
+ return SurgSim::Physics::FIXED_3DPOINT;
}
-size_t MockFixedConstraintBilateral3D::doGetNumDof() const
+size_t MockFixedConstraintFixedPoint::doGetNumDof() const
{
return 3;
}
-void MockFixedConstraintBilateral3D::doBuild(double dt,
+void MockFixedConstraintFixedPoint::doBuild(double dt,
const ConstraintData& data,
const std::shared_ptr<Localization>& localization,
MlcpPhysicsProblem* mlcp,
@@ -446,31 +520,25 @@ void MockFixedConstraintBilateral3D::doBuild(double dt,
{
}
-
-MockRigidConstraintBilateral3D::MockRigidConstraintBilateral3D() : ConstraintImplementation()
+MockRigidConstraintFixedPoint::MockRigidConstraintFixedPoint() : ConstraintImplementation()
{
}
-MockRigidConstraintBilateral3D::~MockRigidConstraintBilateral3D()
+MockRigidConstraintFixedPoint::~MockRigidConstraintFixedPoint()
{
}
-SurgSim::Math::MlcpConstraintType MockRigidConstraintBilateral3D::getMlcpConstraintType() const
+SurgSim::Physics::ConstraintType MockRigidConstraintFixedPoint::getConstraintType() const
{
- return SurgSim::Math::MLCP_BILATERAL_3D_CONSTRAINT;
+ return SurgSim::Physics::FIXED_3DPOINT;
}
-RepresentationType MockRigidConstraintBilateral3D::getRepresentationType() const
-{
- return REPRESENTATION_TYPE_RIGID;
-}
-
-size_t MockRigidConstraintBilateral3D::doGetNumDof() const
+size_t MockRigidConstraintFixedPoint::doGetNumDof() const
{
return 3;
}
-void MockRigidConstraintBilateral3D::doBuild(double dt,
+void MockRigidConstraintFixedPoint::doBuild(double dt,
const ConstraintData& data,
const std::shared_ptr<Localization>& localization,
MlcpPhysicsProblem* mlcp,
@@ -489,20 +557,19 @@ MockLocalization::MockLocalization(std::shared_ptr<Representation> representatio
{
}
-Vector3d MockLocalization::doCalculatePosition(double time)
+Vector3d MockLocalization::doCalculatePosition(double time) const
{
return SurgSim::Math::Vector3d::Zero();
}
-
-SurgSim::Math::MlcpConstraintType MockConstraintImplementation::getMlcpConstraintType() const
+Vector3d MockLocalization::doCalculateVelocity(double time) const
{
- return SurgSim::Math::MLCP_BILATERAL_3D_CONSTRAINT;
+ return SurgSim::Math::Vector3d::Zero();
}
-RepresentationType MockConstraintImplementation::getRepresentationType() const
+SurgSim::Physics::ConstraintType MockConstraintImplementation::getConstraintType() const
{
- return SurgSim::Physics::REPRESENTATION_TYPE_FIXED;
+ return SurgSim::Physics::FIXED_3DPOINT;
}
size_t MockConstraintImplementation::doGetNumDof() const
@@ -511,12 +578,12 @@ size_t MockConstraintImplementation::doGetNumDof() const
}
void MockConstraintImplementation::doBuild(double dt,
- const ConstraintData& data,
- const std::shared_ptr<Localization>& localization,
- MlcpPhysicsProblem* mlcp,
- size_t indexOfRepresentation,
- size_t indexOfConstraint,
- ConstraintSideSign sign)
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign)
{
}
@@ -571,7 +638,7 @@ void MockVirtualToolCoupler::setOptionalAngularDamping(const SurgSim::DataStruct
}
void MockVirtualToolCoupler::setOptionalAttachmentPoint(
- const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>& val)
+ const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>& val)
{
VirtualToolCoupler::setOptionalAttachmentPoint(val);
}
@@ -581,5 +648,39 @@ const SurgSim::DataStructures::DataGroup& MockVirtualToolCoupler::getOutputData(
return m_outputData;
}
+MockCollisionRepresentation::MockCollisionRepresentation(const std::string& name)
+ : SurgSim::Collision::Representation(name), m_numberOfTimesUpdateCalled(0)
+{}
+
+int MockCollisionRepresentation::getShapeType() const
+{
+ return -1;
+}
+
+const std::shared_ptr<SurgSim::Math::Shape> MockCollisionRepresentation::getShape() const
+{
+ return nullptr;
+}
+
+void MockCollisionRepresentation::update(const double& dt)
+{
+ ++m_numberOfTimesUpdateCalled;
+}
+
+/// \return The number of times update method has been invoked.
+int MockCollisionRepresentation::getNumberOfTimesUpdateCalled() const
+{
+ return m_numberOfTimesUpdateCalled;
+}
+
+MockComputation::MockComputation(bool doCopyState) : Computation(doCopyState)
+{
+}
+
+std::shared_ptr<PhysicsManagerState> MockComputation::doUpdate(const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ return state;
+}
}; // Physics
}; // SurgSim
diff --git a/SurgSim/Physics/UnitTests/MockObjects.h b/SurgSim/Physics/UnitTests/MockObjects.h
index 472c3d5..f565643 100644
--- a/SurgSim/Physics/UnitTests/MockObjects.h
+++ b/SurgSim/Physics/UnitTests/MockObjects.h
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -16,6 +16,7 @@
#ifndef SURGSIM_PHYSICS_UNITTESTS_MOCKOBJECTS_H
#define SURGSIM_PHYSICS_UNITTESTS_MOCKOBJECTS_H
+#include "SurgSim/Collision/Representation.h"
#include "SurgSim/Framework/ObjectFactory.h"
#include "SurgSim/Framework/Macros.h"
#include "SurgSim/Math/Matrix.h"
@@ -25,16 +26,19 @@
#include "SurgSim/Math/Vector.h"
#include "SurgSim/Physics/Constraint.h"
#include "SurgSim/Physics/ConstraintImplementation.h"
+#include "SurgSim/Physics/Computation.h"
#include "SurgSim/Physics/DeformableRepresentation.h"
#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/Fem2DRepresentation.h"
#include "SurgSim/Physics/Fem3DRepresentation.h"
#include "SurgSim/Physics/FemElement.h"
#include "SurgSim/Physics/FemRepresentation.h"
+#include "SurgSim/Physics/FixedRepresentation.h"
#include "SurgSim/Physics/LinearSpring.h"
#include "SurgSim/Physics/Localization.h"
#include "SurgSim/Physics/Mass.h"
+#include "SurgSim/Physics/MassSpringLocalization.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"
@@ -48,6 +52,7 @@ using SurgSim::Math::Matrix;
using SurgSim::Math::OdeSolver;
using SurgSim::Math::OdeState;
using SurgSim::Math::RigidTransform3d;
+using SurgSim::Math::SparseMatrix;
using SurgSim::Math::Vector;
using SurgSim::Math::Vector3d;
@@ -65,25 +70,25 @@ public:
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;
+ 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;
+ void update(double dt) override;
/// Postprocessing done after the update call
/// \param dt The time step (in seconds)
- virtual void afterUpdate(double dt) override;
+ void afterUpdate(double dt) override;
int getPreUpdateCount() const;
int getUpdateCount() const;
int getPostUpdateCount() const;
+
+ std::shared_ptr<Localization> createLocalization(const SurgSim::DataStructures::Location& location) override;
};
class MockRigidRepresentation : public RigidRepresentation
@@ -92,28 +97,45 @@ public:
MockRigidRepresentation();
// Non constant access to the states
- RigidRepresentationState& getInitialState();
- RigidRepresentationState& getCurrentState();
- RigidRepresentationState& getPreviousState();
+ RigidState& getInitialState();
+ RigidState& getCurrentState();
+ RigidState& getPreviousState();
};
-class MockDeformableRepresentationLocalization : public SurgSim::Physics::Localization
+class MockFixedRepresentation : public FixedRepresentation
{
public:
- MockDeformableRepresentationLocalization(){}
+ MockFixedRepresentation();
+
+ // Non constant access to the states
+ RigidState& getInitialState();
+ RigidState& getCurrentState();
+ RigidState& getPreviousState();
+};
- explicit MockDeformableRepresentationLocalization(std::shared_ptr<Representation> representation) : Localization()
+class MockDeformableLocalization : public SurgSim::Physics::Localization
+{
+public:
+ MockDeformableLocalization(){}
+
+ explicit MockDeformableLocalization(std::shared_ptr<Representation> representation) : Localization()
{
setRepresentation(representation);
}
- virtual ~MockDeformableRepresentationLocalization(){}
+ virtual ~MockDeformableLocalization(){}
- void setLocalNode(size_t nodeID){ m_nodeID = nodeID; }
+ void setLocalNode(size_t nodeID)
+ {
+ m_nodeID = nodeID;
+ }
- const size_t& getLocalNode() const { return m_nodeID; }
+ const size_t& getLocalNode() const
+ {
+ return m_nodeID;
+ }
- virtual bool isValidRepresentation(std::shared_ptr<Representation> representation) override
+ bool isValidRepresentation(std::shared_ptr<Representation> representation) override
{
std::shared_ptr<DeformableRepresentation> defRepresentation =
std::dynamic_pointer_cast<DeformableRepresentation>(representation);
@@ -123,13 +145,13 @@ public:
}
private:
- virtual SurgSim::Math::Vector3d doCalculatePosition(double time) override
+ SurgSim::Math::Vector3d doCalculatePosition(double time) const 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";
+ " 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);
@@ -138,6 +160,21 @@ private:
return SurgSim::Math::interpolate(previousPoint, currentPoint, time);
}
+ SurgSim::Math::Vector3d doCalculateVelocity(double time) const 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& currentVelocity = defRepresentation->getCurrentState()->getVelocity(m_nodeID);
+ const SurgSim::Math::Vector3d& previousVelocity = defRepresentation->getPreviousState()->getVelocity(m_nodeID);
+
+ return SurgSim::Math::interpolate(previousVelocity, currentVelocity, time);
+ }
+
size_t m_nodeID;
};
@@ -146,45 +183,21 @@ class MockDeformableRepresentation : public SurgSim::Physics::DeformableRepresen
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;
+ void addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ const SurgSim::Math::Vector& generalizedForce,
+ const SurgSim::Math::Matrix& K,
+ const SurgSim::Math::Matrix& D) 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;
+ void transformState(std::shared_ptr<SurgSim::Math::OdeState> state,
+ const SurgSim::Math::RigidTransform3d& transform) override;
+ void computeF(const OdeState& state) override;
+ void computeM(const OdeState& state) override;
+ void computeD(const OdeState& state) override;
+ void computeK(const OdeState& state) override;
+ void computeFMDK(const OdeState& state) override;
};
class MockSpring : public SurgSim::Physics::Spring
@@ -194,15 +207,17 @@ public:
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;
+ void addForce(const OdeState& state, Vector* F, double scale = 1.0) override;
+ void addDamping(const OdeState& state, SparseMatrix* D, double scale = 1.0) override;
+ void addStiffness(const SurgSim::Math::OdeState& state, SurgSim::Math::SparseMatrix* K,
+ double scale = 1.0) override;
+
+ void addFDK(const OdeState& state, Vector* f, SparseMatrix* D, SparseMatrix* K) override;
+ void addMatVec(const OdeState& state, double alphaD, double alphaK, const Vector& x, Vector* F) override;
private:
Vector m_F;
- Matrix m_D, m_K;
+ SparseMatrix m_D, m_K;
};
class MockMassSpring : public SurgSim::Physics::MassSpringRepresentation
@@ -213,58 +228,56 @@ public:
}
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);
+ 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; }
+ const SurgSim::Math::Vector& getExternalForce() const
+ {
+ return m_externalGeneralizedForce;
+ }
+ const SurgSim::Math::SparseMatrix& getExternalStiffness() const
+ {
+ return m_externalGeneralizedStiffness;
+ }
+ const SurgSim::Math::SparseMatrix& getExternalDamping() const
+ {
+ return m_externalGeneralizedDamping;
+ }
+
+ void clearFMDK();
};
class MockFemElement : public FemElement
{
public:
MockFemElement();
+ explicit MockFemElement(std::shared_ptr<FemElementStructs::FemElementParameter> elementData);
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;
+ double getVolume(const OdeState& state) const override;
+ Vector computeCartesianCoordinate(const OdeState& state, const Vector& barycentricCoordinate) const override;
+ Vector computeNaturalCoordinate(const SurgSim::Math::OdeState& state, const Vector& globalCoordinate) const
+ override;
+
+ void initialize(const SurgSim::Math::OdeState& state) override;
bool isInitialized() const;
private:
- Vector m_F;
- Matrix m_M, m_D, m_K;
+ void initializeMembers();
+ void doUpdateFMDK(const Math::OdeState& state, int options) override;
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
{
@@ -276,26 +289,40 @@ public:
/// 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;
+ void setInitialState(std::shared_ptr<SurgSim::Math::OdeState> initialState) override;
- virtual std::shared_ptr<FemPlyReaderDelegate> getDelegate() override;
+ void loadFem(const std::string& filename) override;
- /// Query the representation type
- /// \return the RepresentationType for this representation
- virtual RepresentationType getType() const override;
+ void addExternalGeneralizedForce(std::shared_ptr<Localization> localization,
+ const SurgSim::Math::Vector& generalizedForce, const SurgSim::Math::Matrix& K,
+ const SurgSim::Math::Matrix& D) override;
- std::shared_ptr<OdeSolver> getOdeSolver() const;
+ std::shared_ptr<FemPlyReaderDelegate> getDelegate();
const std::vector<double>& getMassPerNode() const;
+ void clearFMDK();
+
+ bool hasSetInitialStateBeenCalled();
+
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;
+ void transformState(std::shared_ptr<OdeState> state, const RigidTransform3d& transform) override;
+
+ // Flag to be set when setInitialState of this class is called.
+ bool m_setInitialStateCalled;
+};
+
+class MockFemRepresentationValidComplianceWarping : public MockFemRepresentation
+{
+public:
+ explicit MockFemRepresentationValidComplianceWarping(const std::string& name) : MockFemRepresentation(name)
+ {}
+
+protected:
+ SurgSim::Math::Matrix getNodeTransformation(const SurgSim::Math::OdeState& state, size_t nodeId) override;
};
class MockFem1DRepresentation : public SurgSim::Physics::Fem1DRepresentation
@@ -303,51 +330,57 @@ class MockFem1DRepresentation : public SurgSim::Physics::Fem1DRepresentation
public:
explicit MockFem1DRepresentation(const std::string& name);
- const std::shared_ptr<OdeSolver> getOdeSolver() const;
+ bool doInitialize() override;
+
+ double getMassPerNode(size_t nodeId);
};
-class MockFixedConstraintBilateral3D : public ConstraintImplementation
+class MockFem2DRepresentation : public SurgSim::Physics::Fem2DRepresentation
{
public:
- MockFixedConstraintBilateral3D();
- virtual ~MockFixedConstraintBilateral3D();
+ explicit MockFem2DRepresentation(const std::string& name);
- virtual SurgSim::Math::MlcpConstraintType getMlcpConstraintType() const override;
+ double getMassPerNode(size_t nodeId);
+};
- virtual RepresentationType getRepresentationType() const override;
+class MockFixedConstraintFixedPoint : public ConstraintImplementation
+{
+public:
+ MockFixedConstraintFixedPoint();
+ virtual ~MockFixedConstraintFixedPoint();
-private:
- virtual size_t doGetNumDof() const override;
+ SurgSim::Physics::ConstraintType getConstraintType() 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;
+private:
+ size_t doGetNumDof() const override;
+
+ 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
+class MockRigidConstraintFixedPoint : public ConstraintImplementation
{
public:
- MockRigidConstraintBilateral3D();
- virtual ~MockRigidConstraintBilateral3D();
-
- virtual SurgSim::Math::MlcpConstraintType getMlcpConstraintType() const override;
+ MockRigidConstraintFixedPoint();
+ virtual ~MockRigidConstraintFixedPoint();
- virtual RepresentationType getRepresentationType() const override;
+ SurgSim::Physics::ConstraintType getConstraintType() 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;
+ size_t doGetNumDof() const override;
+
+ 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>
@@ -355,7 +388,7 @@ class MockDescendent : public Base
{
public:
MockDescendent() : Base() {}
- explicit MockDescendent(const std::string &name) : Base(name) {}
+ explicit MockDescendent(const std::string& name) : Base(name) {}
};
class MockLocalization : public Localization
@@ -370,26 +403,26 @@ private:
/// \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;
+ SurgSim::Math::Vector3d doCalculatePosition(double time) const override;
+
+ SurgSim::Math::Vector3d doCalculateVelocity(double time) const override;
};
class MockConstraintImplementation : public ConstraintImplementation
{
public:
- virtual SurgSim::Math::MlcpConstraintType getMlcpConstraintType() const override;
-
- virtual RepresentationType getRepresentationType() const override;
+ SurgSim::Physics::ConstraintType getConstraintType() const override;
private:
- virtual size_t doGetNumDof() const override;
+ 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);
+ const ConstraintData& data,
+ const std::shared_ptr<Localization>& localization,
+ MlcpPhysicsProblem* mlcp,
+ size_t indexOfRepresentation,
+ size_t indexOfConstraint,
+ ConstraintSideSign sign);
};
class MockVirtualToolCoupler : public VirtualToolCoupler
@@ -413,15 +446,54 @@ public:
};
inline std::shared_ptr<Constraint> makeMockConstraint(std::shared_ptr<MockRepresentation> firstRepresentation,
- std::shared_ptr<MockRepresentation> secondRepresentation)
+ 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));
+ using SurgSim::DataStructures::Location;
+
+ static auto type = MockConstraintImplementation().getConstraintType();
+ if (firstRepresentation->getConstraintImplementation(type) == nullptr)
+ {
+ ConstraintImplementation::getFactory().addImplementation(typeid(MockRepresentation),
+ std::make_shared<MockConstraintImplementation>());
+ }
+
+ return std::make_shared<Constraint>(type, std::make_shared<ConstraintData>(),
+ firstRepresentation, Location(),
+ secondRepresentation, Location());
}
+/// Class to represent a mock collision representation to test if update gets called from the Computation.
+class MockCollisionRepresentation : public SurgSim::Collision::Representation
+{
+public:
+ /// Default constructor
+ /// \param name The name of the collision representation.
+ explicit MockCollisionRepresentation(const std::string& name);
+
+ int getShapeType() const override;
+ const std::shared_ptr<SurgSim::Math::Shape> getShape() const override;
+ void update(const double& dt) override;
+
+ /// \return The number of times update method has been invoked.
+ int getNumberOfTimesUpdateCalled() const;
+
+private:
+ /// Number of times update method has been invoked.
+ int m_numberOfTimesUpdateCalled;
+};
+
+class MockComputation : public Computation
+{
+public:
+ explicit MockComputation(bool doCopyState = false);
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::MockComputation);
+
+protected:
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state) override;
+};
+
}; // Physics
}; // SurgSim
diff --git a/SurgSim/Physics/UnitTests/ParticleCollisionResponseTests.cpp b/SurgSim/Physics/UnitTests/ParticleCollisionResponseTests.cpp
new file mode 100644
index 0000000..26ec8a9
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/ParticleCollisionResponseTests.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 <gtest/gtest.h>
+#include <memory>
+
+#include "SurgSim/Particles/ParticlesCollisionRepresentation.h"
+#include "SurgSim/Particles/Representation.h"
+#include "SurgSim/Physics/ParticleCollisionResponse.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/Representation.h"
+
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+class MockParticleSystem : public SurgSim::Particles::Representation
+{
+public:
+ explicit MockParticleSystem(const std::string& name) :
+ SurgSim::Particles::Representation(name),
+ handleCollisionsCount(0)
+ {
+ }
+
+ int handleCollisionsCount;
+
+private:
+ bool doUpdate(double dt) override
+ {
+ return true;
+ }
+
+ bool doHandleCollisions(double dt, const SurgSim::Collision::ContactMapType& collisions) override
+ {
+ handleCollisionsCount++;
+ return true;
+ }
+};
+
+
+TEST(ParticleCollisionResponseTest, UpdateCallTest)
+{
+ auto mockRepresentation = std::make_shared<MockParticleSystem>("particles");
+ auto physicsManagerState = std::make_shared<PhysicsManagerState>();
+ std::vector<std::shared_ptr<Particles::Representation>> allRepresentations;
+ allRepresentations.push_back(mockRepresentation);
+ physicsManagerState->setParticleRepresentations(allRepresentations);
+
+ double dt = 1e-3;
+ auto computation = std::make_shared<ParticleCollisionResponse>();
+ EXPECT_EQ(0, mockRepresentation->handleCollisionsCount);
+ computation->update(dt, physicsManagerState);
+ EXPECT_EQ(0, mockRepresentation->handleCollisionsCount)
+ << "Particle Representation without a Collision Representation should not handle collisions.";
+
+ auto collisionRepresentation = std::make_shared<Particles::ParticlesCollisionRepresentation>("collision");
+ mockRepresentation->setCollisionRepresentation(collisionRepresentation);
+ computation->update(dt, physicsManagerState);
+ EXPECT_EQ(1, mockRepresentation->handleCollisionsCount);
+ computation->update(dt, physicsManagerState);
+ EXPECT_EQ(2, mockRepresentation->handleCollisionsCount);
+ computation->update(dt, physicsManagerState);
+ EXPECT_EQ(3, mockRepresentation->handleCollisionsCount);
+}
+
+};
+};
+
diff --git a/SurgSim/Physics/UnitTests/PhysicsManagerStateTests.cpp b/SurgSim/Physics/UnitTests/PhysicsManagerStateTests.cpp
index 2ed270b..c03a890 100644
--- a/SurgSim/Physics/UnitTests/PhysicsManagerStateTests.cpp
+++ b/SurgSim/Physics/UnitTests/PhysicsManagerStateTests.cpp
@@ -30,9 +30,8 @@
#include "SurgSim/Physics/MlcpMapping.h"
#include "SurgSim/Physics/PhysicsManagerState.h"
#include "SurgSim/Physics/RigidCollisionRepresentation.h"
+#include "SurgSim/Physics/RigidConstraintFrictionlessContact.h"
#include "SurgSim/Physics/RigidRepresentation.h"
-#include "SurgSim/Physics/RigidRepresentationContact.h"
-#include "SurgSim/Physics/RigidRepresentationLocalization.h"
using SurgSim::Physics::Constraint;
using SurgSim::Physics::ContactConstraintData;
@@ -41,9 +40,9 @@ using SurgSim::Physics::MlcpMapping;
using SurgSim::Physics::PhysicsManagerState;
using SurgSim::Physics::Representation;
using SurgSim::Physics::RigidCollisionRepresentation;
+using SurgSim::Physics::RigidConstraintFrictionlessContact;
+using SurgSim::Physics::RigidLocalization;
using SurgSim::Physics::RigidRepresentation;
-using SurgSim::Physics::RigidRepresentationContact;
-using SurgSim::Physics::RigidRepresentationLocalization;
TEST(PhysicsManagerStateTest, SetGetRigidRepresentations)
{
@@ -159,6 +158,32 @@ TEST(PhysicsManagerStateTest, SetGetCollisionRepresentations)
EXPECT_EQ(collision2, actualRepresentations.back());
}
+TEST(PhysicsManagerStateTest, SetGetActiveCollisionRepresentations)
+{
+ 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->setActiveCollisionRepresentations(expectedRepresentations);
+ actualRepresentations = physicsState->getActiveCollisionRepresentations();
+ 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->setActiveCollisionRepresentations(expectedRepresentations);
+ actualRepresentations = physicsState->getActiveCollisionRepresentations();
+ ASSERT_EQ(2, actualRepresentations.size());
+ EXPECT_EQ(collision2, actualRepresentations.back());
+}
+
TEST(PhysicsManagerStateTest, SetGetCollisionPairs)
{
auto physicsState = std::make_shared<PhysicsManagerState>();
@@ -188,29 +213,20 @@ TEST(PhysicsManagerStateTest, SetGetConstraintGroup)
std::vector<std::shared_ptr<Constraint>> expectedConstraints;
std::vector<std::shared_ptr<Constraint>> actualConstraints;
+ auto constraintType = SurgSim::Physics::FRICTIONLESS_3DCONTACT;
+
// We need a populated constraint to check the constraintsIndexMapping.
- // Create first side of a constraint.
+ // Create the representations for the 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);
+ auto constraint1 = std::make_shared<Constraint>(constraintType, data,
+ rigid1, SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()),
+ rigid2, SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()));
// Check the constraintGroup.
expectedConstraints.push_back(constraint1);
@@ -220,8 +236,9 @@ TEST(PhysicsManagerStateTest, SetGetConstraintGroup)
EXPECT_EQ(constraint1, actualConstraints.back());
// Create a second constraint.
- auto constraint2 = std::make_shared<Constraint>(data, rigid1Contact, rigid1Localization,
- rigid2Contact, rigid2Localization);
+ auto constraint2 = std::make_shared<Constraint>(constraintType, data,
+ rigid1, SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()),
+ rigid2, SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()));
// Check the constraintGroup.
expectedConstraints.push_back(constraint2);
@@ -237,29 +254,20 @@ TEST(PhysicsManagerStateTest, SetGetConstraintsMapping)
MlcpMapping<Constraint> expectedConstraintsIndexMapping;
MlcpMapping<Constraint> actualConstraintsIndexMapping;
+ auto constraintType = SurgSim::Physics::FRICTIONLESS_3DCONTACT;
+
// We need a populated constraint to check the constraintsIndexMapping.
- // Create first side of a constraint.
+ // Create the representations for the 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);
+ auto constraint1 = std::make_shared<Constraint>(constraintType, data,
+ rigid1, SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()),
+ rigid2, SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()));
// Check the constraintGroup.
expectedConstraintsIndexMapping.setValue(constraint1.get(), 5);
@@ -275,28 +283,20 @@ TEST(PhysicsManagerStateTest, SetGetActiveConstraints)
std::vector<std::shared_ptr<Constraint>> expectedConstraints;
std::vector<std::shared_ptr<Constraint>> actualConstraints;
+ auto constraintType = SurgSim::Physics::FRICTIONLESS_3DCONTACT;
+
// Create first side of a constraint.
+ // Create the representations for the 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);
+ auto constraint1 = std::make_shared<Constraint>(constraintType, data,
+ rigid1, SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()),
+ rigid2, SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()));
// Check the active constraints.
expectedConstraints.push_back(constraint1);
@@ -306,8 +306,9 @@ TEST(PhysicsManagerStateTest, SetGetActiveConstraints)
EXPECT_EQ(constraint1, actualConstraints.back());
// Create a second constraint.
- auto constraint2 = std::make_shared<Constraint>(data, rigid1Contact, rigid1Localization,
- rigid2Contact, rigid2Localization);
+ auto constraint2 = std::make_shared<Constraint>(constraintType, data,
+ rigid1, SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()),
+ rigid2, SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()));
// Check the active constraints.
expectedConstraints.push_back(constraint2);
diff --git a/SurgSim/Physics/UnitTests/PhysicsManagerTests.cpp b/SurgSim/Physics/UnitTests/PhysicsManagerTests.cpp
index 4d3520f..33bc1e6 100644
--- a/SurgSim/Physics/UnitTests/PhysicsManagerTests.cpp
+++ b/SurgSim/Physics/UnitTests/PhysicsManagerTests.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -19,12 +19,15 @@
/// 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 <gmock/gmock.h>
#include <string>
#include <memory>
+#include "SurgSim/Collision/ShapeCollisionRepresentation.h"
#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Particles/SphRepresentation.h"
#include "SurgSim/Physics/ConstraintComponent.h"
#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
#include "SurgSim/Physics/PhysicsManager.h"
@@ -39,8 +42,25 @@ using SurgSim::Physics::FixedRepresentation;
using SurgSim::Physics::PhysicsManager;
using SurgSim::Math::Vector3d;
+
namespace SurgSim
{
+
+namespace Collision
+{
+class MockContactFilter : public Collision::ContactFilter
+{
+public:
+ explicit MockContactFilter(const std::string& name) : ContactFilter(name) {}
+ MOCK_METHOD0(doWakeUp, bool());
+ MOCK_METHOD0(doInitialize, bool());
+ MOCK_METHOD1(doUpdate, void(double));
+ MOCK_METHOD2(doFilterContacts, void(const std::shared_ptr<Physics::PhysicsManagerState>& state,
+ const std::shared_ptr<CollisionPair>& pairs));
+
+};
+}
+
namespace Physics
{
@@ -67,12 +87,6 @@ public:
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;
};
@@ -98,6 +112,35 @@ TEST_F(PhysicsManagerTest, AddRemoveRepresentation)
EXPECT_TRUE(testDoRemoveComponent(representation2));
}
+TEST_F(PhysicsManagerTest, AddRemoveCollisionRepresentation)
+{
+ auto representation1 = std::make_shared<Collision::ShapeCollisionRepresentation>("Rep1");
+ auto representation2 = std::make_shared<Collision::ShapeCollisionRepresentation>("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, AddRemoveContactFilter)
+{
+ auto filter1 = std::make_shared<Collision::MockContactFilter>("filter1");
+ auto filter2 = std::make_shared<Collision::MockContactFilter>("filter2");
+
+ EXPECT_TRUE(testDoAddComponent(filter1));
+ EXPECT_TRUE(testDoAddComponent(filter2));
+ EXPECT_FALSE(testDoAddComponent(filter1));
+
+ EXPECT_TRUE(testDoRemoveComponent(filter1));
+ EXPECT_FALSE(testDoRemoveComponent(filter1));
+ EXPECT_TRUE(testDoRemoveComponent(filter2));
+
+}
+
TEST_F(PhysicsManagerTest, AddRemoveConstraintComponent)
{
auto constraintComponent1 = std::make_shared<ConstraintComponent>("component1");
@@ -117,67 +160,48 @@ TEST_F(PhysicsManagerTest, AddRemoveConstraintComponent)
EXPECT_TRUE(testDoRemoveComponent(constraintComponent2));
}
-TEST_F(PhysicsManagerTest, AddRemoveExcludedCollisionPair)
+TEST_F(PhysicsManagerTest, AddRemoveParticleRepresentation)
{
- 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 representation1 = std::make_shared<Particles::SphRepresentation>("Rep1");
+ auto representation2 = std::make_shared<Particles::SphRepresentation>("Rep2");
- 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());
- }
+ EXPECT_TRUE(testDoAddComponent(representation1));
+ EXPECT_TRUE(testDoAddComponent(representation2));
+ EXPECT_FALSE(testDoAddComponent(representation1));
- {
- 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());
- }
+ EXPECT_TRUE(testDoRemoveComponent(representation1));
+ EXPECT_FALSE(testDoRemoveComponent(representation1));
+ EXPECT_TRUE(testDoRemoveComponent(representation2));
+}
- {
- 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());
- }
+TEST_F(PhysicsManagerTest, SetComputations)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ runtime->addManager(physicsManager);
+ EXPECT_NO_THROW(physicsManager->setComputations(createDcdPipeline()));
+ EXPECT_NO_THROW(runtime->start());
+ EXPECT_ANY_THROW(physicsManager->setComputations(createDcdPipeline()));
+}
- {
- SCOPED_TRACE("Add and remove with swapped representations.");
- EXPECT_NO_THROW(physicsManager->addExcludedCollisionPair(rep1, rep2));
- EXPECT_EQ(1u, collisionPairs->size());
+TEST_F(PhysicsManagerTest, RunCcd)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ runtime->addManager(physicsManager);
+ EXPECT_NO_THROW(physicsManager->setComputations(createCcdPipeline()));
+ EXPECT_NO_THROW(runtime->start());
+ EXPECT_NO_THROW(runtime->stop());
+}
- EXPECT_NO_THROW(physicsManager->removeExcludedCollisionPair(rep2, rep1));
- EXPECT_EQ(0u, collisionPairs->size());
- }
+TEST_F(PhysicsManagerTest, RunDcd)
+{
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ runtime->addManager(physicsManager);
+ EXPECT_NO_THROW(physicsManager->setComputations(createDcdPipeline()));
+ EXPECT_NO_THROW(runtime->start());
+ EXPECT_NO_THROW(runtime->stop());
}
+
}; // namespace Physics
}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/PrepareCollisionPairsTests.cpp b/SurgSim/Physics/UnitTests/PrepareCollisionPairsTests.cpp
new file mode 100644
index 0000000..2bc1b1f
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/PrepareCollisionPairsTests.cpp
@@ -0,0 +1,217 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2015, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file PrepareCollisionPairsTests.cpp
+/// Tests for the PrepareCollisionPairs Class
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <vector>
+
+#include "SurgSim/Blocks/SphereElement.h"
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/Scene.h"
+#include "SurgSim/Framework/SceneElement.h"
+#include "SurgSim/Math/Quaternion.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/PrepareCollisionPairs.h"
+
+using SurgSim::Math::Vector3d;
+
+class PrepareCollisionPairsTest : public ::testing::Test
+{
+public:
+ virtual void SetUp()
+ {
+ sphere1 = std::make_shared<SurgSim::Blocks::SphereElement>("Sphere1");
+ sphere2 = std::make_shared<SurgSim::Blocks::SphereElement>("Sphere2");
+
+ runtime = std::make_shared<SurgSim::Framework::Runtime>();
+ auto scene = runtime->getScene();
+ scene->addSceneElement(sphere1);
+ scene->addSceneElement(sphere2);
+
+ physicsRepresentations.push_back(sphere1->getComponents<SurgSim::Physics::Representation>()[0]);
+ physicsRepresentations.push_back(sphere2->getComponents<SurgSim::Physics::Representation>()[0]);
+
+ sphere1Collision = sphere1->getComponents<SurgSim::Collision::Representation>()[0];
+ sphere2Collision = sphere2->getComponents<SurgSim::Collision::Representation>()[0];
+ sphere1Collision->update(0.0);
+ sphere2Collision->update(0.0);
+ collisionRepresentations.push_back(sphere1Collision);
+ collisionRepresentations.push_back(sphere2Collision);
+
+ computation = std::make_shared<SurgSim::Physics::PrepareCollisionPairs>(false);
+ }
+
+ void prepareState()
+ {
+ auto stateTmp = std::make_shared<SurgSim::Physics::PhysicsManagerState>();
+ stateTmp->setRepresentations(physicsRepresentations);
+ stateTmp->setCollisionRepresentations(collisionRepresentations);
+ state = computation->update(0.0, stateTmp); // This step prepares the collision pairs
+ }
+
+ virtual void TearDown()
+ {
+ }
+
+ std::vector<std::shared_ptr<SurgSim::Physics::Representation>> physicsRepresentations;
+ std::vector<std::shared_ptr<SurgSim::Collision::Representation>> collisionRepresentations;
+
+ std::shared_ptr<SurgSim::Framework::SceneElement> sphere1;
+ std::shared_ptr<SurgSim::Collision::Representation> sphere1Collision;
+
+ std::shared_ptr<SurgSim::Framework::SceneElement> sphere2;
+ std::shared_ptr<SurgSim::Collision::Representation> sphere2Collision;
+
+ std::shared_ptr<SurgSim::Physics::PhysicsManagerState> state;
+ std::shared_ptr<SurgSim::Physics::PrepareCollisionPairs> computation;
+
+ std::shared_ptr<SurgSim::Framework::Runtime> runtime;
+};
+
+namespace SurgSim
+{
+namespace Physics
+{
+
+TEST_F(PrepareCollisionPairsTest, RigidRigidCollisionTest)
+{
+ sphere2->setPose(Math::makeRigidTransform(Math::Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.5)));
+
+ prepareState();
+
+ std::shared_ptr<PhysicsManagerState> newState = computation->update(1.0, state);
+ ASSERT_EQ(1u, newState->getCollisionPairs().size());
+}
+
+TEST_F(PrepareCollisionPairsTest, InactiveTest1)
+{
+ sphere1->setActive(false);
+ sphere2->setPose(Math::makeRigidTransform(Math::Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.5)));
+
+ prepareState();
+
+ std::shared_ptr<PhysicsManagerState> newState = computation->update(1.0, state);
+ ASSERT_EQ(0u, newState->getCollisionPairs().size());
+}
+
+TEST_F(PrepareCollisionPairsTest, InactiveTest2)
+{
+ sphere2->setActive(false);
+ sphere2->setPose(Math::makeRigidTransform(Math::Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.5)));
+
+ prepareState();
+
+ std::shared_ptr<PhysicsManagerState> newState = computation->update(1.0, state);
+ ASSERT_EQ(0u, newState->getCollisionPairs().size());
+}
+
+TEST_F(PrepareCollisionPairsTest, InactiveTest3)
+{
+ sphere1->setActive(false);
+ sphere2->setActive(false);
+ sphere2->setPose(Math::makeRigidTransform(Math::Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.5)));
+
+ prepareState();
+
+ std::shared_ptr<PhysicsManagerState> newState = computation->update(1.0, state);
+ ASSERT_EQ(0u, newState->getCollisionPairs().size());
+}
+
+TEST_F(PrepareCollisionPairsTest, ExlcudeCollisionsTest1)
+{
+ sphere1Collision->ignore("Sphere2/Sphere Collision Representation");
+ sphere2->setPose(Math::makeRigidTransform(Math::Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.5)));
+
+ prepareState();
+
+ std::shared_ptr<PhysicsManagerState> newState = computation->update(1.0, state);
+ ASSERT_EQ(0u, newState->getCollisionPairs().size());
+}
+
+TEST_F(PrepareCollisionPairsTest, ExlcudeCollisionsTest2)
+{
+ sphere2Collision->ignore("Sphere1/Sphere Collision Representation");
+ sphere2->setPose(Math::makeRigidTransform(Math::Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.5)));
+
+ prepareState();
+
+ std::shared_ptr<PhysicsManagerState> newState = computation->update(1.0, state);
+ ASSERT_EQ(0u, newState->getCollisionPairs().size());
+}
+
+TEST_F(PrepareCollisionPairsTest, ExlcudeCollisionsTest3)
+{
+ sphere1Collision->ignore("Sphere2/Sphere Collision Representation");
+ sphere2Collision->ignore("Sphere1/Sphere Collision Representation");
+ sphere2->setPose(Math::makeRigidTransform(Math::Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.5)));
+
+ prepareState();
+
+ std::shared_ptr<PhysicsManagerState> newState = computation->update(1.0, state);
+ ASSERT_EQ(0u, newState->getCollisionPairs().size());
+}
+
+TEST_F(PrepareCollisionPairsTest, IgnoreContinuousTypeCollisions)
+{
+ sphere1Collision->setCollisionDetectionType(Collision::COLLISION_DETECTION_TYPE_CONTINUOUS);
+ sphere2Collision->setCollisionDetectionType(Collision::COLLISION_DETECTION_TYPE_CONTINUOUS);
+ sphere2->setPose(Math::makeRigidTransform(Math::Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.5)));
+
+ prepareState();
+
+ std::shared_ptr<PhysicsManagerState> newState = computation->update(1.0, state);
+ ASSERT_EQ(1u, newState->getCollisionPairs().size());
+}
+
+TEST_F(PrepareCollisionPairsTest, IgnoreNoneTypeCollisions)
+{
+ sphere2Collision->setCollisionDetectionType(Collision::COLLISION_DETECTION_TYPE_NONE);
+ sphere2->setPose(Math::makeRigidTransform(Math::Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.5)));
+
+ prepareState();
+
+ std::shared_ptr<PhysicsManagerState> newState = computation->update(1.0, state);
+ ASSERT_EQ(0u, newState->getCollisionPairs().size());
+}
+
+TEST_F(PrepareCollisionPairsTest, SelfCollisionPair)
+{
+ sphere2->setPose(Math::makeRigidTransform(Math::Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.5)));
+ sphere2Collision->setSelfCollisionDetectionType(Collision::COLLISION_DETECTION_TYPE_DISCRETE);
+
+ prepareState();
+ std::shared_ptr<PhysicsManagerState> newState = computation->update(1.0, state);
+ ASSERT_EQ(2u, newState->getCollisionPairs().size());
+}
+
+// Check for a bug fix in PrepareCollisionPairs where Scenes with 1 representation did not check for self collision
+TEST_F(PrepareCollisionPairsTest, SingleSelfCollisionPair)
+{
+ physicsRepresentations.pop_back();
+ collisionRepresentations.pop_back();
+ sphere1->setPose(Math::makeRigidTransform(Math::Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.5)));
+ sphere1Collision->setSelfCollisionDetectionType(Collision::COLLISION_DETECTION_TYPE_DISCRETE);
+
+ prepareState();
+ std::shared_ptr<PhysicsManagerState> newState = computation->update(1.0, state);
+ ASSERT_EQ(1u, newState->getCollisionPairs().size());
+}
+
+};
+};
diff --git a/SurgSim/Physics/UnitTests/PushResultsTests.cpp b/SurgSim/Physics/UnitTests/PushResultsTests.cpp
index 081e57e..ebe6301 100644
--- a/SurgSim/Physics/UnitTests/PushResultsTests.cpp
+++ b/SurgSim/Physics/UnitTests/PushResultsTests.cpp
@@ -89,38 +89,21 @@ TEST_F(PushResultsTests, OneRepresentationOneConstraintTest)
m_usedRepresentations.push_back(m_fixedWorldRepresentation);
// Set the representation list in the Physics Manager State
m_physicsManagerState->setRepresentations(m_usedRepresentations);
+ // The type of constraint.
+ auto constraintType = SurgSim::Physics::FRICTIONLESS_3DCONTACT;
// 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);
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(constraintType,
+ data, m_usedRepresentations[0],
+ SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()),
+ m_fixedWorldRepresentation,
+ SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()));
// Register the constraint in the list of used constraints for this test
m_usedConstraints.push_back(constraint);
@@ -185,72 +168,36 @@ TEST_F(PushResultsTests, OneRepresentationTwoConstraintsTest)
m_usedRepresentations.push_back(m_fixedWorldRepresentation);
// Set the representation list in the Physics Manager State
m_physicsManagerState->setRepresentations(m_usedRepresentations);
+ // The type of constraint.
+ auto constraintType = SurgSim::Physics::FRICTIONLESS_3DCONTACT;
// 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);
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(constraintType,
+ data, m_usedRepresentations[0],
+ SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()),
+ m_fixedWorldRepresentation,
+ SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Zero()));
// 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);
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(constraintType,
+ data, m_usedRepresentations[0],
+ SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Ones()),
+ m_fixedWorldRepresentation,
+ SurgSim::DataStructures::Location(SurgSim::Math::Vector3d::Ones()));
// Register the constraint in the list of used constraints for this test
m_usedConstraints.push_back(constraint);
@@ -327,72 +274,36 @@ TEST_F(PushResultsTests, TwoRepresentationsTwoConstraintsTest)
m_usedRepresentations.push_back(m_allRepresentations[1]);
// Set the representation list in the Physics Manager State
m_physicsManagerState->setRepresentations(m_usedRepresentations);
+ // The type of constraint.
+ auto constraintType = SurgSim::Physics::FRICTIONLESS_3DCONTACT;
// 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);
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(constraintType,
+ data, m_usedRepresentations[0],
+ SurgSim::DataStructures::Location(pointOrigin),
+ m_usedRepresentations[1],
+ SurgSim::DataStructures::Location(pointOrigin));
// 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);
+ std::shared_ptr<Constraint> constraint = std::make_shared<Constraint>(constraintType,
+ data, m_usedRepresentations[0],
+ SurgSim::DataStructures::Location(pointOrigin),
+ m_usedRepresentations[1],
+ SurgSim::DataStructures::Location(pointOne));
// Register the constraint in the list of used constraints for this test
m_usedConstraints.push_back(constraint);
diff --git a/SurgSim/Physics/UnitTests/RepresentationTest.cpp b/SurgSim/Physics/UnitTests/RepresentationTest.cpp
index 38949dd..af5de14 100644
--- a/SurgSim/Physics/UnitTests/RepresentationTest.cpp
+++ b/SurgSim/Physics/UnitTests/RepresentationTest.cpp
@@ -184,3 +184,38 @@ TEST(RepresentationTest, SerializationTest)
EXPECT_FALSE(newRepresentation4->getValue<bool>("IsDrivingSceneElementPose"));
}
}
+
+// Local class to test constraint registration.
+class MockRigidRepresentation : public SurgSim::Physics::RigidRepresentation
+{
+public:
+ explicit MockRigidRepresentation(const std::string& name) : RigidRepresentation(name)
+ {}
+
+ std::shared_ptr<SurgSim::Physics::ConstraintImplementation>
+ getConstraintImplementation(SurgSim::Physics::ConstraintType type)
+ {
+ return RigidRepresentation::getConstraintImplementation(type);
+ }
+};
+
+TEST(RepresentationTest, ConstraintTest)
+{
+ using SurgSim::Physics::ConstraintImplementation;
+ using SurgSim::Physics::MockConstraintImplementation;
+
+ auto implementation = std::make_shared<MockConstraintImplementation>();
+ auto representation = std::make_shared<MockRigidRepresentation>("rep");
+
+ // Test the getConstraintImplementation
+ EXPECT_TRUE(representation->getConstraintImplementation(implementation->getConstraintType())
+ == nullptr);
+
+ // Add the constraint implementation.
+ ConstraintImplementation::getFactory().addImplementation(typeid(MockRigidRepresentation),
+ implementation);
+
+ // Test the getConstraintImplementation
+ EXPECT_TRUE(representation->getConstraintImplementation(implementation->getConstraintType())
+ != nullptr);
+}
diff --git a/SurgSim/Physics/UnitTests/RigidCollisionRepresentationTest.cpp b/SurgSim/Physics/UnitTests/RigidCollisionRepresentationTest.cpp
index c7a36f2..609f41c 100644
--- a/SurgSim/Physics/UnitTests/RigidCollisionRepresentationTest.cpp
+++ b/SurgSim/Physics/UnitTests/RigidCollisionRepresentationTest.cpp
@@ -1,5 +1,5 @@
// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
+// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -18,6 +18,7 @@
#include <gtest/gtest.h>
#include "SurgSim/Framework/ApplicationData.h"
+#include "SurgSim/Framework/BasicSceneElement.h"
#include "SurgSim/Framework/FrameworkConvert.h"
#include "SurgSim/Math/MeshShape.h"
#include "SurgSim/Math/BoxShape.h"
@@ -27,6 +28,7 @@
#include "SurgSim/Math/SphereShape.h"
#include "SurgSim/Physics/RigidCollisionRepresentation.h"
#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Framework/Runtime.h"
using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Vector3d;
@@ -61,8 +63,8 @@ TEST_F(RigidCollisionRepresentationTest, InitTest)
EXPECT_NO_THROW(RigidCollisionRepresentation("TestRigidCollisionRepresentation"));
EXPECT_NO_THROW(m_rigidCollisionRepresentation =
- std::make_shared<RigidCollisionRepresentation>("RigidCollisionRepresentation")
- );
+ std::make_shared<RigidCollisionRepresentation>("RigidCollisionRepresentation")
+ );
}
TEST_F(RigidCollisionRepresentationTest, SetGetRigidRepresentationTest)
@@ -124,8 +126,6 @@ TEST_F(RigidCollisionRepresentationTest, SerializationTest)
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 =
@@ -135,7 +135,7 @@ TEST_F(RigidCollisionRepresentationTest, SerializationTest)
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"));
+ newRepresentation->getValue<std::shared_ptr<SurgSim::Math::Shape>>("Shape"));
ASSERT_NE(nullptr, newBoxShape);
EXPECT_EQ(boxShape->getType(), newRepresentation->getShapeType());
@@ -151,8 +151,6 @@ TEST_F(RigidCollisionRepresentationTest, SerializationTest)
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 =
@@ -163,7 +161,7 @@ TEST_F(RigidCollisionRepresentationTest, SerializationTest)
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"));
+ newRepresentation->getValue<std::shared_ptr<SurgSim::Math::Shape>>("Shape"));
ASSERT_NE(nullptr, newShpereShape);
EXPECT_TRUE(m_sphereShape->getCenter().isApprox(newShpereShape->getCenter()));
@@ -173,40 +171,38 @@ TEST_F(RigidCollisionRepresentationTest, SerializationTest)
}
}
-TEST_F(RigidCollisionRepresentationTest, MeshUpdateTest)
+TEST_F(RigidCollisionRepresentationTest, GetPosedShape)
{
- SurgSim::Framework::ApplicationData applicationData("config.txt");
- const std::string fileName = "MeshShapeData/staple_collision.ply";
+ auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
- auto meshShape = std::make_shared<SurgSim::Math::MeshShape>();
- EXPECT_NO_THROW(meshShape->load(fileName, applicationData));
+ const std::string fileName = "Geometry/staple_collision.ply";
+ auto mesh = std::make_shared<SurgSim::DataStructures::TriangleMeshPlain>();
+ EXPECT_NO_THROW(mesh->load(fileName));
+
+ auto originalMesh = std::make_shared<SurgSim::Math::MeshShape>(*mesh);
+ auto expectedMesh = std::make_shared<SurgSim::Math::MeshShape>(*mesh);
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->setShape(originalMesh);
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();
-
+ auto actualMesh = std::static_pointer_cast<SurgSim::Math::MeshShape>(collisionRepresentation->getPosedShape());
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);
+ Vector3d(-1.5, 7.5, -2.5),
+ Vector3d(8.7, -4.7, -3.1));
+ collisionRepresentation->setLocalPose(transform);
+ collisionRepresentation->update(dt);
+ actualMesh = std::static_pointer_cast<SurgSim::Math::MeshShape>(collisionRepresentation->getPosedShape());
+ expectedMesh->transform(transform);
+ expectedMesh->update();
EXPECT_EQ(expectedMesh->getVertices(), actualMesh->getVertices());
EXPECT_EQ(expectedMesh->getTriangles(), actualMesh->getTriangles());
}
diff --git a/SurgSim/Physics/UnitTests/RigidConstraintFixedPointTests.cpp b/SurgSim/Physics/UnitTests/RigidConstraintFixedPointTests.cpp
new file mode 100644
index 0000000..44ec576
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/RigidConstraintFixedPointTests.cpp
@@ -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.
+
+#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/RigidConstraintFixedPoint.h"
+#include "SurgSim/Physics/RigidRepresentation.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(RigidConstraintFixedPointTests, Constructor)
+{
+ ASSERT_NO_THROW(
+ { RigidConstraintFixedPoint constraint; });
+}
+
+TEST(RigidConstraintFixedPointTests, Constants)
+{
+ RigidConstraintFixedPoint constraint;
+
+ EXPECT_EQ(SurgSim::Physics::FIXED_3DPOINT, constraint.getConstraintType());
+ EXPECT_EQ(3u, constraint.getNumDof());
+}
+
+TEST(RigidConstraintFixedPointTests, 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.
+ RigidConstraintFixedPoint 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 RigidConstraintFixedPoint::build
+ auto representation = std::make_shared<MockRigidRepresentation>();
+ representation->setShape(std::make_shared<SurgSim::Math::SphereShape>(1.0));
+ auto localization = std::make_shared<RigidLocalization>(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(RigidConstraintFixedPointTests, 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.
+ RigidConstraintFixedPoint 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 RigidConstraintFixedPoint::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<RigidLocalization>(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/RigidConstraintFixedRotationVectorTests.cpp b/SurgSim/Physics/UnitTests/RigidConstraintFixedRotationVectorTests.cpp
new file mode 100644
index 0000000..3cbe34f
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/RigidConstraintFixedRotationVectorTests.cpp
@@ -0,0 +1,118 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/RotationVectorConstraintData.h"
+#include "SurgSim/Physics/Representation.h"
+#include "SurgSim/Physics/RigidConstraintFixedRotationVector.h"
+#include "SurgSim/Physics/RigidRepresentation.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(RigidConstraintFixedRotationVectorTests, Constructor)
+{
+ ASSERT_NO_THROW(
+ { RigidConstraintFixedRotationVector constraint; });
+}
+
+TEST(RigidConstraintFixedRotationVectorTests, Constants)
+{
+ RigidConstraintFixedRotationVector constraint;
+
+ EXPECT_EQ(SurgSim::Physics::FIXED_3DROTATION_VECTOR, constraint.getConstraintType());
+ EXPECT_EQ(3u, constraint.getNumDof());
+}
+
+TEST(RigidConstraintFixedRotationVectorTests, BuildMlcp)
+{
+ using SurgSim::Framework::Runtime;
+
+ RigidConstraintFixedRotationVector constraint;
+
+ // Prepare the fem1d representation for this constraint type
+ auto fem1d = std::make_shared<Fem1DRepresentation>("fem1d");
+ auto initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(6, 2);
+ initialState->getPositions().setZero();
+ initialState->getPositions().segment<3>(6) = SurgSim::Math::Vector3d(1.0, 0.0, 0.0);
+ fem1d->setInitialState(initialState);
+ std::array<size_t, 2> nodeIds = { { 0, 1 } };
+ auto beam = std::make_shared<Fem1DElementBeam>(nodeIds);
+ beam->setMassDensity(900);
+ beam->setPoissonRatio(0.4);
+ beam->setRadius(0.1);
+ beam->setShearingEnabled(false);
+ beam->setYoungModulus(1e6);
+ fem1d->addFemElement(beam);
+ fem1d->initialize(std::make_shared<Runtime>()); // Initializes the beams initial rotation matrix
+
+ // Prepare the rigid representation for this constraint type
+ auto representation = std::make_shared<MockRigidRepresentation>();
+ representation->setShape(std::make_shared<SurgSim::Math::SphereShape>(1.0));
+ 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);
+ representation->getCurrentState().setPose(objectPose);
+ auto localization = std::make_shared<RigidLocalization>(representation);
+ localization->setLocalPosition(objectPose.inverse() * Vector3d(8.0, 6.4, 3.5));
+
+ // Prepare the specific constraint data
+ RotationVectorRigidFem1DConstraintData constraintData;
+ constraintData.setFem1DRotation(fem1d, 0);
+ constraintData.setRigidOrFixedRotation(representation, representation->getPose().linear());
+
+ MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(6, 3, 1);
+
+ ASSERT_NO_THROW(constraint.build(
+ dt, constraintData, localization, &mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE));
+
+ // Compare results
+ Eigen::AngleAxisd aa(objectRotation);
+ Eigen::Matrix<double, 3, 1> violation = aa.axis() * aa.angle();
+ EXPECT_NEAR_EIGEN(violation, mlcpPhysicsProblem.b, epsilon);
+
+ Eigen::Matrix<double, 3, 6> H = Eigen::Matrix<double, 3, 6>::Zero();
+ EXPECT_NEAR_EIGEN(H, mlcpPhysicsProblem.H, epsilon);
+
+ EXPECT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
+}
+
+}; // namespace Physics
+}; // namespace SurgSim
diff --git a/SurgSim/Physics/UnitTests/RigidConstraintFrictionlessContactTests.cpp b/SurgSim/Physics/UnitTests/RigidConstraintFrictionlessContactTests.cpp
new file mode 100644
index 0000000..5ad0c4c
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/RigidConstraintFrictionlessContactTests.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 <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/RigidConstraintFrictionlessContact.h"
+#include "SurgSim/Physics/RigidRepresentation.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 (RigidConstraintFrictionlessContactTests, 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));
+
+ auto loc = std::make_shared<RigidLocalization>(rigid);
+ loc->setLocalPosition(contactPosition);
+ std::shared_ptr<RigidConstraintFrictionlessContact> implementation =
+ std::make_shared<RigidConstraintFrictionlessContact>();
+
+ EXPECT_EQ(SurgSim::Physics::FRICTIONLESS_3DCONTACT, implementation->getConstraintType());
+ 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));
+ SurgSim::Math::Matrix h = mlcpPhysicsProblem.H;
+ EXPECT_NEAR(dt * n[0] , h(0, 0), epsilon);
+ EXPECT_NEAR(dt * n[1] , h(0, 1), epsilon);
+ EXPECT_NEAR(dt * n[2] , h(0, 2), epsilon);
+ EXPECT_NEAR(dt * n_GP[0], h(0, 3), epsilon);
+ EXPECT_NEAR(dt * n_GP[1], h(0, 4), epsilon);
+ EXPECT_NEAR(dt * n_GP[2], 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/RigidLocalizationTest.cpp b/SurgSim/Physics/UnitTests/RigidLocalizationTest.cpp
new file mode 100644
index 0000000..e6faccd
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/RigidLocalizationTest.cpp
@@ -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.
+
+#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/FixedRepresentation.h"
+#include "SurgSim/Physics/RigidLocalization.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+
+using SurgSim::Physics::MockFixedRepresentation;
+using SurgSim::Physics::MockRigidRepresentation;
+using SurgSim::Physics::RigidRepresentation;
+using SurgSim::Physics::RigidLocalization;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+class RigidLocalizationTest : 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(RigidLocalizationTest, ConstructorTest)
+{
+ ASSERT_NO_THROW( {RigidLocalization rigidRepresentationLoc;});
+
+ ASSERT_NO_THROW(
+ {
+ std::shared_ptr<RigidRepresentation> rigid = std::make_shared<RigidRepresentation>("RigidRepresentation");
+ RigidLocalization rigidRepresentationLoc(rigid);
+ });
+}
+
+TEST_F(RigidLocalizationTest, SetGetRepresentation)
+{
+ RigidLocalization 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(RigidLocalizationTest, CalculatePosition)
+{
+ // 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);
+
+ RigidLocalization localization = RigidLocalization(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));
+
+ // Out-Of-Range assertions
+ EXPECT_THROW(localization.calculatePosition(-0.01), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(localization.calculatePosition(1.01), SurgSim::Framework::AssertionFailure);
+}
+
+TEST_F(RigidLocalizationTest, CalculateVelocity)
+{
+ // 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);
+
+ RigidLocalization localization = RigidLocalization(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));
+
+ // Out-Of-Range assertions
+ EXPECT_THROW(localization.calculateVelocity(-0.01), SurgSim::Framework::AssertionFailure);
+ EXPECT_THROW(localization.calculateVelocity(1.01), SurgSim::Framework::AssertionFailure);
+}
+
+TEST_F(RigidLocalizationTest, FixedRepresentation)
+{
+ // Create the rigid body
+ auto fixedRepresentation = std::make_shared<MockFixedRepresentation>();
+
+ // Activate the rigid body and setup its initial pose
+ fixedRepresentation->setLocalActive(true);
+ fixedRepresentation->getCurrentState().setPose(m_initialTransformation);
+
+ RigidLocalization localization = RigidLocalization(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/RigidRepresentationBilateral3DTests.cpp b/SurgSim/Physics/UnitTests/RigidRepresentationBilateral3DTests.cpp
deleted file mode 100644
index 0dece56..0000000
--- a/SurgSim/Physics/UnitTests/RigidRepresentationBilateral3DTests.cpp
+++ /dev/null
@@ -1,163 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 73c65c6..0000000
--- a/SurgSim/Physics/UnitTests/RigidRepresentationContactTests.cpp
+++ /dev/null
@@ -1,99 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 624a1ad..0000000
--- a/SurgSim/Physics/UnitTests/RigidRepresentationLocalizationTest.cpp
+++ /dev/null
@@ -1,123 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
deleted file mode 100644
index 8921900..0000000
--- a/SurgSim/Physics/UnitTests/RigidRepresentationStateTest.cpp
+++ /dev/null
@@ -1,134 +0,0 @@
-// This file is a part of the OpenSurgSim project.
-// Copyright 2013, SimQuest Solutions Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES 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
index 957b3d1..eb9f4e9 100644
--- a/SurgSim/Physics/UnitTests/RigidRepresentationTest.cpp
+++ b/SurgSim/Physics/UnitTests/RigidRepresentationTest.cpp
@@ -1,23 +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.
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/DataStructures/BufferedValue.h"
#include "SurgSim/Framework/Runtime.h"
#include "SurgSim/Framework/FrameworkConvert.h"
#include "SurgSim/Math/Matrix.h"
@@ -33,7 +34,6 @@
#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;
@@ -120,10 +120,10 @@ public:
std::shared_ptr<SphereShape> m_sphere;
// Rigid representation state
- RigidRepresentationState m_state;
+ RigidState m_state;
// Rigid representation default state
- RigidRepresentationState m_defaultState;
+ RigidState m_defaultState;
// Max number of simulation step for testing
int m_maxNumSimulationStepTest;
@@ -239,7 +239,7 @@ TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceOnMassCenterTest)
Matrix66d D = -0.6 * Matrix66d::Ones() + 4.1 * Matrix66d::Identity();
rigidBody->addExternalGeneralizedForce(F, K, D);
- EXPECT_TRUE(rigidBody->getExternalGeneralizedForce().isApprox(F));
+ EXPECT_TRUE(rigidBody->getExternalGeneralizedForce().unsafeGet().isApprox(F));
EXPECT_TRUE(rigidBody->getExternalGeneralizedStiffness().isApprox(K));
EXPECT_TRUE(rigidBody->getExternalGeneralizedDamping().isApprox(D));
}
@@ -356,7 +356,7 @@ TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceExtraTermsTest)
std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
rigidBody->setShape(m_sphere);
- SurgSim::Physics::RigidRepresentationState initialState;
+ SurgSim::Physics::RigidState initialState;
initialState.setPose(transform);
rigidBody->setInitialState(initialState);
SurgSim::DataStructures::Location location(anchorLocalPoint);
@@ -368,7 +368,7 @@ TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceExtraTermsTest)
Vector6d F = inputForce;
Matrix66d K = Matrix66d::Zero(), D = Matrix66d::Zero();
rigidBody->addExternalGeneralizedForce(location, F, K, D);
- F = rigidBody->getExternalGeneralizedForce();
+ F = rigidBody->getExternalGeneralizedForce().unsafeGet();
K = rigidBody->getExternalGeneralizedStiffness();
D = rigidBody->getExternalGeneralizedDamping();
@@ -396,7 +396,7 @@ TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceExtraTermsTest)
Vector6d F = inputForce;
Matrix66d K = Matrix66d::Zero(), D = Matrix66d::Zero();
rigidBody->addExternalGeneralizedForce(location, F, K, D);
- F = rigidBody->getExternalGeneralizedForce();
+ F = rigidBody->getExternalGeneralizedForce().unsafeGet();
K = rigidBody->getExternalGeneralizedStiffness();
D = rigidBody->getExternalGeneralizedDamping();
@@ -422,7 +422,7 @@ TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceExtraTermsTest)
std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
rigidBody->setShape(m_sphere);
- SurgSim::Physics::RigidRepresentationState initialState;
+ SurgSim::Physics::RigidState initialState;
initialState.setPose(transform);
rigidBody->setInitialState(initialState);
SurgSim::DataStructures::Location location(anchorLocalPoint);
@@ -430,23 +430,22 @@ TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceExtraTermsTest)
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();
+ F = rigidBody->getExternalGeneralizedForce().unsafeGet();
K = rigidBody->getExternalGeneralizedStiffness();
D = rigidBody->getExternalGeneralizedDamping();
EXPECT_LE((F - Fnumeric).cwiseAbs().maxCoeff(), 2e-7);
- EXPECT_LE((K - Knumeric).cwiseAbs().maxCoeff(), 2e-7);
+ EXPECT_LE((K - Knumeric).cwiseAbs().maxCoeff(), 2.2e-7); // Epsilon set by trial and error
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());
+ 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);
@@ -460,7 +459,7 @@ TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceExtraTermsTest)
std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
rigidBody->setShape(m_sphere);
- SurgSim::Physics::RigidRepresentationState initialState;
+ SurgSim::Physics::RigidState initialState;
initialState.setPose(transform);
rigidBody->setInitialState(initialState);
SurgSim::DataStructures::Location location(anchorLocalPoint);
@@ -472,7 +471,7 @@ TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceExtraTermsTest)
Vector6d F = inputForce;
Matrix66d K = Matrix66d::Zero(), D = Matrix66d::Zero();
rigidBody->addExternalGeneralizedForce(location, F, K, D);
- F = rigidBody->getExternalGeneralizedForce();
+ F = rigidBody->getExternalGeneralizedForce().unsafeGet();
K = rigidBody->getExternalGeneralizedStiffness();
D = rigidBody->getExternalGeneralizedDamping();
@@ -523,11 +522,11 @@ Vector6d computeSpringForce(double linearStiffness, double linearDamping,
}
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())
+ 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;
@@ -537,26 +536,30 @@ Matrix66d computeSpringStiffness(double linearStiffness, double linearDamping,
Vector6d dofX = dofPosition;
dofX[column] += 2.0 * epsilon;
Vector6d fXPlus2H = computeSpringForce(linearStiffness, linearDamping, angularStiffness, angularDamping,
- targetPosition, targetLinearVelocity, targetRotationVector, targetAngularVelocity, dofX, dofVelocity,
- doComputeExtraTorque, localApplicationPoint);
+ 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);
+ 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);
+ 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);
+ targetPosition, targetLinearVelocity, targetRotationVector,
+ targetAngularVelocity, dofX, dofVelocity,
+ doComputeExtraTorque, localApplicationPoint);
K.col(column) = - (-fXPlus2H + 8.0 * fXPlusH - 8.0 * fXMinusH + fXMinus2H) / (12.0 * epsilon);
}
@@ -579,26 +582,30 @@ Matrix66d computeSpringDamping(double linearStiffness, double linearDamping,
Vector6d dofV = dofVelocity;
dofV[column] += 2.0 * epsilon;
Vector6d fXPlus2H = computeSpringForce(linearStiffness, linearDamping, angularStiffness, angularDamping,
- targetPosition, targetLinearVelocity, targetRotationVector, targetAngularVelocity, dofPosition, dofV,
- doComputeExtraTorque, localApplicationPoint);
+ 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);
+ 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);
+ 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);
+ targetPosition, targetLinearVelocity, targetRotationVector,
+ targetAngularVelocity, dofPosition, dofV,
+ doComputeExtraTorque, localApplicationPoint);
D.col(column) = - (-fXPlus2H + 8.0 * fXPlusH - 8.0 * fXMinusH + fXMinus2H) / (12.0 * epsilon);
}
@@ -635,28 +642,28 @@ TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceTest)
Vector6d dofV = Vector6d::Zero();
Vector6d FWithoutExtraTorque = computeSpringForce(linearStiffness, linearDamping,
- angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
- targetRotationVector, targetAngularVelocity, dofX, dofV);
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV);
Matrix66d KWithoutExtraTorque = computeSpringStiffness(linearStiffness, linearDamping,
- angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
- targetRotationVector, targetAngularVelocity, dofX, dofV);
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV);
Matrix66d DWithoutExtraTorque = computeSpringDamping(linearStiffness, linearDamping,
- angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
- targetRotationVector, targetAngularVelocity, dofX, dofV);
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV);
Vector6d FWithExtraTorque = computeSpringForce(linearStiffness, linearDamping,
- angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
- targetRotationVector, targetAngularVelocity, dofX, dofV, true, localAnchorPoint);
+ 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);
+ 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);
+ 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;
+ SurgSim::Physics::RigidState initialState;
initialState.setPose(transform);
rigidBody->setInitialState(initialState);
SurgSim::DataStructures::Location location(localAnchorPoint);
@@ -664,7 +671,7 @@ TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceTest)
Vector6d F = FWithoutExtraTorque;
Matrix66d K = KWithoutExtraTorque, D = DWithoutExtraTorque;
rigidBody->addExternalGeneralizedForce(location, F, K, D);
- F = rigidBody->getExternalGeneralizedForce();
+ F = rigidBody->getExternalGeneralizedForce().unsafeGet();
K = rigidBody->getExternalGeneralizedStiffness();
D = rigidBody->getExternalGeneralizedDamping();
@@ -687,22 +694,22 @@ TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceTest)
Vector6d dofV = Vector6d::Zero();
Vector6d FWithoutExtraTorque = computeSpringForce(linearStiffness, linearDamping,
- angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
- targetRotationVector, targetAngularVelocity, dofX, dofV);
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV);
Matrix66d KWithoutExtraTorque = computeSpringStiffness(linearStiffness, linearDamping,
- angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
- targetRotationVector, targetAngularVelocity, dofX, dofV);
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV);
Vector6d FWithExtraTorque = computeSpringForce(linearStiffness, linearDamping,
- angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
- targetRotationVector, targetAngularVelocity, dofX, dofV, true, localAnchorPoint);
+ 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);
+ 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;
+ SurgSim::Physics::RigidState initialState;
initialState.setPose(transform);
rigidBody->setInitialState(initialState);
SurgSim::DataStructures::Location location(localAnchorPoint);
@@ -710,7 +717,7 @@ TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceTest)
Vector6d F = FWithoutExtraTorque;
Matrix66d K = KWithoutExtraTorque;
rigidBody->addExternalGeneralizedForce(location, F, K);
- F = rigidBody->getExternalGeneralizedForce();
+ F = rigidBody->getExternalGeneralizedForce().unsafeGet();
K = rigidBody->getExternalGeneralizedStiffness();
EXPECT_LE((F - FWithExtraTorque).cwiseAbs().maxCoeff(), 1e-7);
@@ -731,23 +738,23 @@ TEST_F(RigidRepresentationTest, AddExternalGeneralizedForceTest)
Vector6d dofV = Vector6d::Zero();
Vector6d FWithoutExtraTorque = computeSpringForce(linearStiffness, linearDamping,
- angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
- targetRotationVector, targetAngularVelocity, dofX, dofV);
+ angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
+ targetRotationVector, targetAngularVelocity, dofX, dofV);
Vector6d FWithExtraTorque = computeSpringForce(linearStiffness, linearDamping,
- angularStiffness, angularDamping, targetPosition, targetLinearVelocity,
- targetRotationVector, targetAngularVelocity, dofX, dofV, true, localAnchorPoint);
+ 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;
+ SurgSim::Physics::RigidState initialState;
initialState.setPose(transform);
rigidBody->setInitialState(initialState);
SurgSim::DataStructures::Location location(localAnchorPoint);
Vector6d F = FWithoutExtraTorque;
rigidBody->addExternalGeneralizedForce(location, F);
- F = rigidBody->getExternalGeneralizedForce();
+ F = rigidBody->getExternalGeneralizedForce().unsafeGet();
EXPECT_LE((F - FWithExtraTorque).cwiseAbs().maxCoeff(), 1e-7);
}
@@ -849,7 +856,7 @@ TEST_F(RigidRepresentationTest, PreviousStateDifferentFromCurrentTest)
}
void disableWhenDivergeTest(std::shared_ptr<RigidRepresentation> rigidBody,
- const RigidRepresentationState& state, double dt)
+ const RigidState& state, double dt)
{
// Setup phase
rigidBody->setLocalActive(true);
@@ -978,7 +985,7 @@ TEST_F(RigidRepresentationTest, LocalizationCreation)
Location loc(Vector3d(3.0, 2.0, 1.0));
std::shared_ptr<Localization> localization = rigidBody->createLocalization(loc);
- localization->setRepresentation(rigidBody);
+ EXPECT_TRUE(localization->getRepresentation() == rigidBody);
Vector3d globalPos = rigidBody->getCurrentState().getPose() * loc.rigidLocalPosition.getValue();
@@ -1006,7 +1013,7 @@ 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");
+ shape->load("Geometry/staple_collision.ply");
EXPECT_TRUE(shape->isValid());
std::shared_ptr<RigidRepresentation> rigidBody = std::make_shared<RigidRepresentation>("Rigid");
@@ -1072,12 +1079,12 @@ 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));
+ 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>>()));
+ EXPECT_NO_THROW(newRepresentation = std::dynamic_pointer_cast<RigidRepresentation>
+ (node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
}
{
diff --git a/SurgSim/Physics/UnitTests/RigidStateTest.cpp b/SurgSim/Physics/UnitTests/RigidStateTest.cpp
new file mode 100644
index 0000000..0414746
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/RigidStateTest.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/RigidState.h"
+
+struct RigidStateTest : 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(RigidStateTest, ConstructorTest)
+{
+ ASSERT_NO_THROW(SurgSim::Physics::RigidState rigidRepresentationState);
+}
+
+TEST_F(RigidStateTest, DefaultValueTest)
+{
+ // Create the base rigid representation state
+ std::shared_ptr<SurgSim::Physics::RigidState> rigidRepresentationState;
+ rigidRepresentationState = std::make_shared<SurgSim::Physics::RigidState>();
+
+ // 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(RigidStateTest, ResetTest)
+{
+ // Create the base rigid representation state
+ std::shared_ptr<SurgSim::Physics::RigidState> rigidRepresentationState;
+ rigidRepresentationState = std::make_shared<SurgSim::Physics::RigidState>();
+
+ 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(RigidStateTest, SetGetTest)
+{
+ // Create the base rigid representation state
+ std::shared_ptr<SurgSim::Physics::RigidState> rigidRepresentationState;
+ rigidRepresentationState = std::make_shared<SurgSim::Physics::RigidState>();
+
+ // 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(RigidStateTest, SerializationTest)
+{
+ SurgSim::Physics::RigidState rigidRepresentationState;
+
+ rigidRepresentationState.setValue("Pose", pose);
+ rigidRepresentationState.setValue("LinearVelocity", linearVelocity);
+ rigidRepresentationState.setValue("AngularVelocity", angularVelocity);
+
+ YAML::Node node;
+ ASSERT_NO_THROW(node = YAML::convert<SurgSim::Physics::RigidState>::encode(rigidRepresentationState));
+ EXPECT_EQ(1u, node.size());
+
+ SurgSim::Physics::RigidState newRigidRepresentationState;
+ ASSERT_NO_THROW(newRigidRepresentationState = node.as<SurgSim::Physics::RigidState>());
+
+ 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/RotationVectorConstraintDataTests.cpp b/SurgSim/Physics/UnitTests/RotationVectorConstraintDataTests.cpp
new file mode 100644
index 0000000..d752424
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/RotationVectorConstraintDataTests.cpp
@@ -0,0 +1,70 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/RotationVectorConstraintData.h"
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Physics/Fem1DElementBeam.h"
+#include "SurgSim/Physics/Fem1DRepresentation.h"
+#include "SurgSim/Physics/RigidRepresentation.h"
+
+using SurgSim::Physics::Fem1DElementBeam;
+using SurgSim::Physics::Fem1DRepresentation;
+using SurgSim::Physics::RigidRepresentation;
+using SurgSim::Physics::RotationVectorRigidFem1DConstraintData;
+
+TEST (RotationVectorConstraintDataTests, TestSetGet)
+{
+ using SurgSim::Framework::AssertionFailure;
+
+ RotationVectorRigidFem1DConstraintData rotationVectorConstraintData;
+ auto Id = SurgSim::Math::Matrix33d::Identity();
+
+ EXPECT_THROW(rotationVectorConstraintData.getCurrentRotationVector(), AssertionFailure);
+
+ auto rigid = std::make_shared<SurgSim::Physics::RigidRepresentation>("rigid");
+ auto rigidRAtGrasp = rigid->getPose().linear();
+ EXPECT_THROW(rotationVectorConstraintData.setRigidOrFixedRotation(nullptr, Id), AssertionFailure);
+ rotationVectorConstraintData.setRigidOrFixedRotation(rigid, rigidRAtGrasp);
+
+ EXPECT_THROW(rotationVectorConstraintData.getCurrentRotationVector(), AssertionFailure);
+
+ EXPECT_THROW(rotationVectorConstraintData.setFem1DRotation(nullptr, 0), AssertionFailure);
+ auto fem1d = std::make_shared<SurgSim::Physics::Fem1DRepresentation>("fem1d");
+ EXPECT_THROW(rotationVectorConstraintData.setFem1DRotation(fem1d, 0), AssertionFailure);
+
+ auto initialState = std::make_shared<SurgSim::Math::OdeState>();
+ initialState->setNumDof(6, 2);
+ initialState->getPositions().setZero();
+ initialState->getPositions().segment<3>(6) = SurgSim::Math::Vector3d(1.0, 0.0, 0.0);
+ fem1d->setInitialState(initialState);
+ auto elementData = std::make_shared<SurgSim::Physics::FemElementStructs::FemElement1DParameter>();
+ elementData->enableShear = false;
+ elementData->massDensity = 950;
+ elementData->nodeIds.push_back(0);
+ elementData->nodeIds.push_back(1);
+ elementData->poissonRatio = 0.35;
+ elementData->radius = 0.01;
+ elementData->youngModulus = 1e6;
+ fem1d->addFemElement(std::make_shared<SurgSim::Physics::Fem1DElementBeam>(elementData));
+ fem1d->initialize(std::make_shared<SurgSim::Framework::Runtime>());
+ EXPECT_NO_THROW(rotationVectorConstraintData.setFem1DRotation(fem1d, 0));
+
+ EXPECT_NO_THROW(rotationVectorConstraintData.getCurrentRotationVector());
+}
diff --git a/SurgSim/Physics/UnitTests/SlidingConstraintDataTests.cpp b/SurgSim/Physics/UnitTests/SlidingConstraintDataTests.cpp
new file mode 100644
index 0000000..2e4d142
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/SlidingConstraintDataTests.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 <gtest/gtest.h>
+#include "SurgSim/Physics/Constraint.h"
+#include "SurgSim/Physics/ConstraintData.h"
+#include "SurgSim/Physics/SlidingConstraintData.h"
+using SurgSim::Physics::Constraint;
+using SurgSim::Physics::ConstraintData;
+using SurgSim::Physics::SlidingConstraintData;
+
+#include "SurgSim/Math/Vector.h"
+using SurgSim::Math::Vector3d;
+
+namespace
+{
+ const double epsilon = 1e-10;
+};
+
+TEST(SlidingConstraintDataTests, TestSetGet)
+{
+ using SurgSim::DataStructures::Location;
+
+ Vector3d point(1.2, 3.4, 5.6), direction(7.8, 9.8, 7.6);
+ direction.normalize();
+
+ SlidingConstraintData slidingConstraintData;
+ slidingConstraintData.setSlidingDirection(point, direction);
+
+ const auto normals = slidingConstraintData.getNormals();
+
+ EXPECT_TRUE(point.isApprox(slidingConstraintData.getPose().translation()));
+ auto pose = slidingConstraintData.getPose().inverse().rotation();
+ EXPECT_TRUE((pose * direction).isApprox(Vector3d(1.0, 0.0, 0.0)));
+ EXPECT_TRUE((pose * normals[0]).isApprox(Vector3d(0.0, 1.0, 0.0)));
+ EXPECT_TRUE((pose * normals[1]).isApprox(Vector3d(0.0, 0.0, 1.0)));
+
+ EXPECT_NEAR(0.0, direction.dot(normals[0]), epsilon);
+ EXPECT_NEAR(0.0, direction.dot(normals[1]), epsilon);
+
+ const auto tangent = slidingConstraintData.getTangent();
+ const auto distanceTangent = slidingConstraintData.getDistanceTangent();
+
+ EXPECT_NEAR(0.0, point.dot(tangent) + distanceTangent, epsilon);
+ EXPECT_TRUE(direction.isApprox(tangent));
+
+ // Friction coefficient
+ EXPECT_DOUBLE_EQ(0.5, slidingConstraintData.getFrictionCoefficient());
+ EXPECT_NO_THROW(slidingConstraintData.setFrictionCoefficient(0.1));
+ EXPECT_DOUBLE_EQ(0.1, slidingConstraintData.getFrictionCoefficient());
+}
diff --git a/SurgSim/Physics/UnitTests/SolveMlcpTests.cpp b/SurgSim/Physics/UnitTests/SolveMlcpTests.cpp
index d6d43ce..0a42007 100644
--- a/SurgSim/Physics/UnitTests/SolveMlcpTests.cpp
+++ b/SurgSim/Physics/UnitTests/SolveMlcpTests.cpp
@@ -40,7 +40,7 @@ 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)
+static void testMlcp(const std::string& filename, double contactTolerance, double solverPrecision, size_t maxIteration)
{
std::shared_ptr<MlcpTestData> data = loadTestData(filename);
ASSERT_NE(nullptr, data) << "Could not load data file 'mlcpOriginalTest.txt'";
@@ -51,8 +51,11 @@ static void testMlcp(std::string filename, double contactTolerance, double solve
double dt = 1e-3;
solveMlcpComputation->setContactTolerance(contactTolerance);
- solveMlcpComputation->setSolverPrecision(solverPrecision);
+ EXPECT_NEAR(contactTolerance, solveMlcpComputation->getContactTolerance(), 1e-10);
+ solveMlcpComputation->setPrecision(solverPrecision);
+ EXPECT_NEAR(solverPrecision, solveMlcpComputation->getPrecision(), 1e-10);
solveMlcpComputation->setMaxIterations(maxIteration);
+ EXPECT_EQ(maxIteration, solveMlcpComputation->getMaxIterations());
// Copy the MlcpProblem data over into the input state
state->getMlcpProblem().A = data->problem.A;
diff --git a/SurgSim/Physics/UnitTests/UpdateCollisionRepresentationsTest.cpp b/SurgSim/Physics/UnitTests/UpdateCollisionRepresentationsTest.cpp
new file mode 100644
index 0000000..46e74d6
--- /dev/null
+++ b/SurgSim/Physics/UnitTests/UpdateCollisionRepresentationsTest.cpp
@@ -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.
+
+/// \file UpdateCollisionRepresentationsTest.cpp
+/// Tests for the UpdateCollisionRepresentations Class
+
+#include <memory>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "SurgSim/Collision/Representation.h"
+#include "SurgSim/Math/Shape.h"
+#include "SurgSim/Physics/UnitTests/MockObjects.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Physics/UpdateCollisionRepresentations.h"
+
+using SurgSim::Physics::MockCollisionRepresentation;
+using SurgSim::Physics::PhysicsManagerState;
+using SurgSim::Physics::UpdateCollisionRepresentations;
+
+TEST(UpdateCollisionRepresentationsTest, Construction)
+{
+ EXPECT_NO_THROW(UpdateCollisionRepresentations computation(true););
+ EXPECT_NO_THROW(UpdateCollisionRepresentations computation(false););
+}
+
+TEST(UpdateCollisionRepresentationsTest, Update)
+{
+ std::shared_ptr<PhysicsManagerState> state = std::make_shared<PhysicsManagerState>();
+
+ // Create two collision representations.
+ auto collision1 = std::make_shared<MockCollisionRepresentation>("Collision1");
+ auto collision2 = std::make_shared<MockCollisionRepresentation>("Collision2");
+
+ // Setup the state.
+ std::vector<std::shared_ptr<SurgSim::Collision::Representation>> collisions;
+ collisions.push_back(collision1);
+ collisions.push_back(collision2);
+ state->setCollisionRepresentations(collisions);
+
+ // Test the m_numberOfTimesUpdateCalled before calling update().
+ EXPECT_EQ(0, collision1->getNumberOfTimesUpdateCalled());
+ EXPECT_EQ(0, collision2->getNumberOfTimesUpdateCalled());
+
+ // Set the local active flags.
+ collision1->setLocalActive(true);
+ collision2->setLocalActive(false);
+
+ // Test compuation.update()
+ SurgSim::Physics::UpdateCollisionRepresentations computation(false);
+ std::shared_ptr<PhysicsManagerState> newState = computation.update(1.0, state);
+
+ // Test the m_numberOfTimesUpdateCalled before calling update().
+ EXPECT_EQ(1, collision1->getNumberOfTimesUpdateCalled());
+ EXPECT_EQ(0, collision2->getNumberOfTimesUpdateCalled());
+}
\ No newline at end of file
diff --git a/SurgSim/Physics/UnitTests/VirtualToolCouplerTest.cpp b/SurgSim/Physics/UnitTests/VirtualToolCouplerTest.cpp
index 2d99ab3..771276f 100644
--- a/SurgSim/Physics/UnitTests/VirtualToolCouplerTest.cpp
+++ b/SurgSim/Physics/UnitTests/VirtualToolCouplerTest.cpp
@@ -1,25 +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.
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/Collision/CollisionPair.h"
+#include "SurgSim/DataStructures/Location.h"
#include "SurgSim/Devices/IdentityPoseDevice/IdentityPoseDevice.h"
#include "SurgSim/Input/InputComponent.h"
+#include "SurgSim/Input/OutputComponent.h"
#include "SurgSim/Framework/FrameworkConvert.h"
#include "SurgSim/Framework/Runtime.h"
#include "SurgSim/Math/Matrix.h"
@@ -27,14 +30,17 @@
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/SphereShape.h"
#include "SurgSim/Math/Vector.h"
+#include "SurgSim/Physics/RigidCollisionRepresentation.h"
#include "SurgSim/Physics/RigidRepresentation.h"
-#include "SurgSim/Physics/RigidRepresentationState.h"
+#include "SurgSim/Physics/RigidState.h"
#include "SurgSim/Physics/UnitTests/MockObjects.h"
#include "SurgSim/Physics/VirtualToolCoupler.h"
+#include "SurgSim/Testing/MockInputComponent.h"
-using SurgSim::Device::IdentityPoseDevice;
+using SurgSim::Devices::IdentityPoseDevice;
using SurgSim::Input::InputComponent;
using SurgSim::Framework::Runtime;
+using SurgSim::Math::makeRigidTranslation;
using SurgSim::Math::Matrix33d;
using SurgSim::Math::Quaterniond;
using SurgSim::Math::RigidTransform3d;
@@ -61,7 +67,7 @@ struct VirtualToolCouplerTest : public ::testing::Test
rigidBody->setLinearDamping(0.0);
rigidBody->setShape(std::make_shared<SphereShape>(0.1));
- RigidRepresentationState state;
+ RigidState state;
state.setAngularVelocity(Vector3d::Zero());
state.setLinearVelocity(Vector3d::Zero());
state.setPose(RigidTransform3d::Identity());
@@ -70,7 +76,7 @@ struct VirtualToolCouplerTest : public ::testing::Test
input = std::make_shared<InputComponent>("Input");
input->setDeviceName(inputDeviceName);
device = std::make_shared<IdentityPoseDevice>(inputDeviceName);
- input->connectDevice(device);
+ device->addInputConsumer(input);
virtualToolCoupler = std::make_shared<MockVirtualToolCoupler>();
virtualToolCoupler->setInput(input);
@@ -89,7 +95,7 @@ struct VirtualToolCouplerTest : public ::testing::Test
protected:
void runSystem(const int numSteps, const double dt = 0.001)
{
- for(int step=0; step<numSteps; step++)
+ for (int step = 0; step < numSteps; step++)
{
virtualToolCoupler->update(dt);
rigidBody->beforeUpdate(dt);
@@ -124,7 +130,7 @@ protected:
double dt = expectedSettlingTime / NUM_STEPS;
Vector3d previousPosition = initialPose.translation();
Vector3d currentPosition;
- for(int step=0; step<NUM_STEPS; step++)
+ for (int step = 0; step < NUM_STEPS; step++)
{
virtualToolCoupler->update(dt);
rigidBody->beforeUpdate(dt);
@@ -158,7 +164,7 @@ protected:
// 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 inertia = rigidBody->getLocalInertia()(1, 1);
double stiffness = virtualToolCoupler->getAngularStiffness();
double naturalFrequency = sqrt(stiffness / inertia);
double expectedSettlingTime = 4.744 / naturalFrequency;
@@ -166,7 +172,7 @@ protected:
double dt = expectedSettlingTime / NUM_STEPS;
double previousAngle = initialAngle;
double currentAngle;
- for(int step=0; step<NUM_STEPS; step++)
+ for (int step = 0; step < NUM_STEPS; step++)
{
virtualToolCoupler->update(dt);
rigidBody->beforeUpdate(dt);
@@ -216,7 +222,7 @@ TEST_F(VirtualToolCouplerTest, LinearDisplacement)
runSystem(2000);
EXPECT_TRUE(rigidBody->isActive());
- RigidRepresentationState state = rigidBody->getCurrentState();
+ RigidState state = rigidBody->getCurrentState();
EXPECT_TRUE(state.getLinearVelocity().isZero(epsilon));
EXPECT_TRUE(state.getAngularVelocity().isZero(epsilon));
EXPECT_TRUE(state.getPose().translation().isZero(epsilon));
@@ -225,6 +231,41 @@ TEST_F(VirtualToolCouplerTest, LinearDisplacement)
EXPECT_NEAR(0.0, angleAxis.angle(), epsilon);
}
+TEST_F(VirtualToolCouplerTest, LinearDisplacementWithOffset)
+{
+ const double mass = rigidBody->getMass();
+ virtualToolCoupler->overrideAngularDamping(mass * 1.0);
+ virtualToolCoupler->overrideAngularStiffness(mass * 200);
+ virtualToolCoupler->overrideLinearDamping(mass * 50);
+ virtualToolCoupler->overrideLinearStiffness(mass * 200);
+ rigidBody->setIsGravityEnabled(false);
+
+ auto device = std::make_shared<IdentityPoseDevice>("IdentityPoseDevice");
+ auto inputComponent = std::make_shared<InputComponent>("InputComponent");
+ device->addInputConsumer(inputComponent);
+ virtualToolCoupler->setInput(inputComponent);
+
+ RigidTransform3d inputOffset = makeRigidTranslation(Vector3d(0.1, 0.0, 0.0));
+ inputComponent->setLocalPose(inputOffset);
+
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ virtualToolCoupler->initialize(runtime);
+ rigidBody->initialize(runtime);
+ virtualToolCoupler->wakeUp();
+ rigidBody->wakeUp();
+
+ runSystem(2500);
+ EXPECT_TRUE(rigidBody->isActive());
+
+ RigidState state = rigidBody->getCurrentState();
+ EXPECT_TRUE(state.getLinearVelocity().isZero(epsilon));
+ EXPECT_TRUE(state.getAngularVelocity().isZero(epsilon));
+ EXPECT_TRUE(state.getPose().translation().isApprox(inputOffset.translation(), epsilon))
+ << "Rigid Body Position is incorrect" << std::endl
+ << " Actual: " << state.getPose().translation().transpose() << std::endl
+ << "Expected: " << inputOffset.translation().transpose() << std::endl;
+}
+
TEST_F(VirtualToolCouplerTest, LinearDisplacementWithInertialTorques)
{
const double mass = rigidBody->getMass();
@@ -250,7 +291,7 @@ TEST_F(VirtualToolCouplerTest, LinearDisplacementWithInertialTorques)
virtualToolCoupler->wakeUp();
rigidBody->wakeUp();
- RigidRepresentationState state = rigidBody->getCurrentState();
+ RigidState state = rigidBody->getCurrentState();
EXPECT_TRUE(state.getAngularVelocity().isZero(epsilon));
Eigen::AngleAxisd angleAxis = Eigen::AngleAxisd(state.getPose().linear());
EXPECT_NEAR(0.0, angleAxis.angle(), epsilon);
@@ -278,7 +319,7 @@ TEST_F(VirtualToolCouplerTest, AngularDisplacement)
virtualToolCoupler->overrideLinearStiffness(mass * 200);
RigidTransform3d initialPose = RigidTransform3d::Identity();
- initialPose.linear() = Matrix33d(Eigen::AngleAxisd(M_PI/4.0, Vector3d::UnitY()));
+ initialPose.linear() = Matrix33d(Eigen::AngleAxisd(M_PI / 4.0, Vector3d::UnitY()));
rigidBody->setLocalPose(initialPose);
rigidBody->setIsGravityEnabled(false);
@@ -299,7 +340,7 @@ TEST_F(VirtualToolCouplerTest, AngularDisplacement)
runSystem(2000);
EXPECT_TRUE(rigidBody->isActive());
- RigidRepresentationState state = rigidBody->getCurrentState();
+ RigidState state = rigidBody->getCurrentState();
EXPECT_TRUE(state.getLinearVelocity().isZero(epsilon));
EXPECT_TRUE(state.getAngularVelocity().isZero(epsilon));
EXPECT_TRUE(state.getPose().translation().isZero(epsilon));
@@ -311,7 +352,7 @@ TEST_F(VirtualToolCouplerTest, AngularDisplacement)
TEST_F(VirtualToolCouplerTest, WithGravity)
{
const double mass = rigidBody->getMass();
- virtualToolCoupler->overrideAngularDamping(mass * 1.0 );
+ virtualToolCoupler->overrideAngularDamping(mass * 1.0);
virtualToolCoupler->overrideAngularStiffness(mass * 200);
virtualToolCoupler->overrideLinearDamping(mass * 50);
virtualToolCoupler->overrideLinearStiffness(mass * 200);
@@ -336,7 +377,7 @@ TEST_F(VirtualToolCouplerTest, WithGravity)
runSystem(2000);
EXPECT_TRUE(rigidBody->isActive());
- RigidRepresentationState state = rigidBody->getCurrentState();
+ RigidState state = rigidBody->getCurrentState();
EXPECT_TRUE(state.getLinearVelocity().isZero(epsilon));
EXPECT_TRUE(state.getAngularVelocity().isZero(epsilon));
@@ -388,6 +429,105 @@ TEST_F(VirtualToolCouplerTest, SetAngularDamping)
checkAngularIsCriticallyDamped();
}
+TEST_F(VirtualToolCouplerTest, SetHapticOutputOnlyWhenColliding)
+{
+ RigidTransform3d initialPose =
+ Math::makeRigidTransform(Math::makeRotationQuaternion(10.0, Vector3d::UnitX().eval()), Vector3d(0.1, 0.0, 0.0));
+ rigidBody->setLocalPose(initialPose);
+ rigidBody->setIsGravityEnabled(false);
+ auto input = std::make_shared<Testing::MockInputComponent>("input");
+ DataStructures::DataGroupBuilder builder;
+ builder.addPose(DataStructures::Names::POSE);
+ // The VTC only provides the Jacobians if it gets input velocities.
+ builder.addVector(DataStructures::Names::LINEAR_VELOCITY);
+ builder.addVector(DataStructures::Names::ANGULAR_VELOCITY);
+ DataStructures::DataGroup inputData = builder.createData();
+ inputData.poses().set(DataStructures::Names::POSE, RigidTransform3d::Identity());
+ inputData.vectors().set(DataStructures::Names::LINEAR_VELOCITY, Vector3d::Identity());
+ inputData.vectors().set(DataStructures::Names::ANGULAR_VELOCITY, Vector3d::Identity());
+ input->setData(inputData);
+ virtualToolCoupler->setInput(input);
+ auto output = std::make_shared<Input::OutputComponent>("output");
+ std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
+ virtualToolCoupler->initialize(runtime);
+ rigidBody->initialize(runtime);
+ input->initialize(runtime);
+ output->initialize(runtime);
+ virtualToolCoupler->wakeUp();
+ rigidBody->wakeUp();
+ input->wakeUp();
+ output->wakeUp();
+
+ EXPECT_FALSE(virtualToolCoupler->isHapticOutputOnlyWhenColliding());
+ virtualToolCoupler->setHapticOutputOnlyWhenColliding(true);
+ EXPECT_TRUE(virtualToolCoupler->isHapticOutputOnlyWhenColliding());
+
+ // no OutputComponent
+ virtualToolCoupler->update(0.1);
+ DataStructures::DataGroup data = virtualToolCoupler->getOutputData();
+ ASSERT_TRUE(data.vectors().hasEntry(DataStructures::Names::FORCE));
+ EXPECT_FALSE(data.vectors().hasData(DataStructures::Names::FORCE));
+ ASSERT_TRUE(data.vectors().hasEntry(DataStructures::Names::TORQUE));
+ EXPECT_FALSE(data.vectors().hasData(DataStructures::Names::TORQUE));
+ ASSERT_TRUE(data.matrices().hasEntry(DataStructures::Names::DAMPER_JACOBIAN));
+ EXPECT_FALSE(data.matrices().hasData(DataStructures::Names::DAMPER_JACOBIAN));
+ ASSERT_TRUE(data.matrices().hasEntry(DataStructures::Names::SPRING_JACOBIAN));
+ EXPECT_FALSE(data.matrices().hasData(DataStructures::Names::SPRING_JACOBIAN));
+
+ // no collision representation
+ virtualToolCoupler->setOutput(output);
+ virtualToolCoupler->update(0.1);
+ data = virtualToolCoupler->getOutputData();
+ Vector3d force;
+ Vector3d torque;
+ ASSERT_TRUE(data.vectors().get(DataStructures::Names::FORCE, &force));
+ EXPECT_GT(force.norm(), 0.0);
+ ASSERT_TRUE(data.vectors().get(DataStructures::Names::TORQUE, &torque));
+ EXPECT_GT(torque.norm(), 0.0);
+ EXPECT_TRUE(data.matrices().hasData(SurgSim::DataStructures::Names::DAMPER_JACOBIAN));
+ EXPECT_TRUE(data.matrices().hasData(SurgSim::DataStructures::Names::SPRING_JACOBIAN));
+
+ // forces disabled in free motion
+ auto collision = std::make_shared<RigidCollisionRepresentation>("collision");
+ rigidBody->setCollisionRepresentation(collision);
+ virtualToolCoupler->update(0.1);
+ data = virtualToolCoupler->getOutputData();
+ ASSERT_TRUE(data.vectors().get(DataStructures::Names::FORCE, &force));
+ EXPECT_EQ(force, Vector3d::Zero());
+ ASSERT_TRUE(data.vectors().get(DataStructures::Names::TORQUE, &torque));
+ EXPECT_EQ(torque, Vector3d::Zero());
+ EXPECT_FALSE(data.matrices().hasData(DataStructures::Names::DAMPER_JACOBIAN));
+ EXPECT_FALSE(data.matrices().hasData(DataStructures::Names::SPRING_JACOBIAN));
+
+ // no flag
+ virtualToolCoupler->setHapticOutputOnlyWhenColliding(false);
+ virtualToolCoupler->update(0.1);
+ data = virtualToolCoupler->getOutputData();
+ ASSERT_TRUE(data.vectors().get(DataStructures::Names::FORCE, &force));
+ EXPECT_GT(force.norm(), 0.0);
+ ASSERT_TRUE(data.vectors().get(DataStructures::Names::TORQUE, &torque));
+ EXPECT_GT(torque.norm(), 0.0);
+ EXPECT_TRUE(data.matrices().hasData(DataStructures::Names::DAMPER_JACOBIAN));
+ EXPECT_TRUE(data.matrices().hasData(DataStructures::Names::SPRING_JACOBIAN));
+
+ // forces enabled if any collisions
+ virtualToolCoupler->setHapticOutputOnlyWhenColliding(true);
+ auto& collisions = collision->getCollisions().unsafeGet();
+ collisions[std::make_shared<RigidCollisionRepresentation>("collision2")].push_back(
+ std::make_shared<Collision::Contact>(
+ Collision::COLLISION_DETECTION_TYPE_DISCRETE, 0.1, 1.0,
+ Vector3d::UnitX().eval(), Vector3d::UnitY().eval(),
+ std::make_pair(DataStructures::Location(), DataStructures::Location())));
+ virtualToolCoupler->update(0.1);
+ data = virtualToolCoupler->getOutputData();
+ ASSERT_TRUE(data.vectors().get(DataStructures::Names::FORCE, &force));
+ EXPECT_GT(force.norm(), 0.0);
+ ASSERT_TRUE(data.vectors().get(DataStructures::Names::TORQUE, &torque));
+ EXPECT_GT(torque.norm(), 0.0);
+ EXPECT_TRUE(data.matrices().hasData(DataStructures::Names::DAMPER_JACOBIAN));
+ EXPECT_TRUE(data.matrices().hasData(DataStructures::Names::SPRING_JACOBIAN));
+}
+
TEST_F(VirtualToolCouplerTest, GetInput)
{
EXPECT_EQ(input, virtualToolCoupler->getInput());
@@ -502,6 +642,8 @@ TEST_F(VirtualToolCouplerTest, Serialization)
virtualToolCoupler->setOptionalAttachmentPoint(optionalVec);
virtualToolCoupler->setCalculateInertialTorques(true);
+ virtualToolCoupler->setHapticOutputOnlyWhenColliding(true);
+
// Encode
YAML::Node node;
EXPECT_NO_THROW(node = YAML::convert<SurgSim::Framework::Component>::encode(*virtualToolCoupler););
@@ -509,7 +651,7 @@ TEST_F(VirtualToolCouplerTest, Serialization)
// 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>>()));
+ node.as<std::shared_ptr<SurgSim::Framework::Component>>()));
// Verify
newVirtualToolCoupler->initialize(std::make_shared<SurgSim::Framework::Runtime>());
@@ -521,6 +663,7 @@ TEST_F(VirtualToolCouplerTest, Serialization)
EXPECT_EQ(num, newVirtualToolCoupler->getAngularDamping());
EXPECT_TRUE(vec.isApprox(newVirtualToolCoupler->getAttachmentPoint()));
EXPECT_TRUE(newVirtualToolCoupler->getCalculateInertialTorques());
+ EXPECT_TRUE(virtualToolCoupler->isHapticOutputOnlyWhenColliding());
EXPECT_NE(nullptr, newVirtualToolCoupler->getInput());
EXPECT_NE(nullptr, newVirtualToolCoupler->getRepresentation());
@@ -533,7 +676,8 @@ TEST_F(VirtualToolCouplerTest, Serialization)
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>());
+ node[virtualToolCoupler->getClassName()]
+ [input->getName()][input->getClassName()]["Id"].as<std::string>());
}
TEST_F(VirtualToolCouplerTest, OutputDataEntries)
diff --git a/SurgSim/Physics/UnitTests/config.txt.in b/SurgSim/Physics/UnitTests/config.txt.in
index 22fbf60..aad44f8 100644
--- a/SurgSim/Physics/UnitTests/config.txt.in
+++ b/SurgSim/Physics/UnitTests/config.txt.in
@@ -1,2 +1,2 @@
-${CMAKE_CURRENT_BINARY_DIR}/Data
-${PROJECT_BINARY_DIR}/SurgSim/Testing/Data
\ No newline at end of file
+${CMAKE_CURRENT_BINARY_DIR}/Data/
+${PROJECT_BINARY_DIR}/SurgSim/Testing/Data/
diff --git a/SurgSim/Physics/UpdateCcdData.cpp b/SurgSim/Physics/UpdateCcdData.cpp
new file mode 100644
index 0000000..86608a0
--- /dev/null
+++ b/SurgSim/Physics/UpdateCcdData.cpp
@@ -0,0 +1,81 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/UpdateCcdData.h"
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/ThreadPool.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Collision/Representation.h"
+
+#include <unordered_set>
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+UpdateCcdData::UpdateCcdData(bool copyState):
+ Computation(copyState)
+{
+
+}
+
+UpdateCcdData::~UpdateCcdData()
+{
+
+}
+
+
+std::shared_ptr<PhysicsManagerState> UpdateCcdData::doUpdate(
+ const double& interval,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ std::shared_ptr<PhysicsManagerState> result = state;
+
+ auto threadPool = Framework::Runtime::getThreadPool();
+ std::vector<std::future<void>> tasks;
+
+ const auto& pairs = result->getCollisionPairs();
+ std::unordered_set<Collision::Representation*> representations;
+
+ for (const auto& pair : pairs)
+ {
+ if (pair->getType() == SurgSim::Collision::COLLISION_DETECTION_TYPE_CONTINUOUS)
+ {
+ representations.insert(pair->getFirst().get());
+ representations.insert(pair->getSecond().get());
+ }
+ }
+
+ for (auto& representation : representations)
+ {
+ tasks.push_back(threadPool->enqueue<void>([interval, &representation]()
+ {
+ representation->updateCcdData(interval);
+ }));
+ }
+
+ for (auto& task : tasks)
+ {
+ task.get();
+ }
+
+ return result;
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Physics/UpdateCcdData.h b/SurgSim/Physics/UpdateCcdData.h
new file mode 100644
index 0000000..49bd4ec
--- /dev/null
+++ b/SurgSim/Physics/UpdateCcdData.h
@@ -0,0 +1,55 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_UPDATECCDDATA_H
+#define SURGSIM_PHYSICS_UPDATECCDDATA_H
+
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Physics/Computation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+class UpdateCcdData : public Computation
+{
+public:
+ /// Constructor
+ explicit UpdateCcdData(bool copyState);
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::UpdateCcdData);
+
+ /// Destructor
+ ~UpdateCcdData();
+
+ /// Change of behavior, the value transported is not the complete 'interval' but is the percentage
+ /// of the last interval where the previous impact was found, this is to trigger the
+ /// correct interpolation of the state from the previous state to the interpolated state. This
+ /// value is relative to the last interval to match the behavior of the rest of the system
+ /// \param interval Parameter to be used to interpolate the previousPhysicsState
+ /// \param state normal PhysicsManagerState
+ std::shared_ptr<PhysicsManagerState>
+ doUpdate(const double& interval, const std::shared_ptr<PhysicsManagerState>& state) override;
+
+private:
+
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Physics/UpdateCollisionData.cpp b/SurgSim/Physics/UpdateCollisionData.cpp
new file mode 100644
index 0000000..2a2e6a5
--- /dev/null
+++ b/SurgSim/Physics/UpdateCollisionData.cpp
@@ -0,0 +1,66 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/UpdateCollisionData.h"
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/ThreadPool.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Collision/Representation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+UpdateCollisionData::UpdateCollisionData(bool copyState) :
+ Computation(copyState)
+{
+
+}
+
+UpdateCollisionData::~UpdateCollisionData()
+{
+
+}
+
+
+std::shared_ptr<PhysicsManagerState> UpdateCollisionData::doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ std::shared_ptr<PhysicsManagerState> result = state;
+
+ auto threadPool = Framework::Runtime::getThreadPool();
+ std::vector<std::future<void>> tasks;
+ auto& representations = result->getActiveCollisionRepresentations();
+ for (auto& representation : representations)
+ {
+ tasks.push_back(threadPool->enqueue<void>([dt, &representation]()
+ {
+ representation->updateShapeData();
+ }));
+ }
+ for (auto& task : tasks)
+ {
+ task.get();
+ }
+
+ return result;
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Physics/UpdateCollisionData.h b/SurgSim/Physics/UpdateCollisionData.h
new file mode 100644
index 0000000..115bca2
--- /dev/null
+++ b/SurgSim/Physics/UpdateCollisionData.h
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_UPDATECOLLISIONDATA_H
+#define SURGSIM_PHYSICS_UPDATECOLLISIONDATA_H
+
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Physics/Computation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+class UpdateCollisionData : public Computation
+{
+public:
+ /// Constructor
+ explicit UpdateCollisionData(bool copyState);
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::UpdateCollisionData);
+
+ /// Destructor
+ ~UpdateCollisionData();
+
+ std::shared_ptr<PhysicsManagerState>
+ doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state) override;
+
+private:
+
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Physics/UpdateCollisionRepresentations.cpp b/SurgSim/Physics/UpdateCollisionRepresentations.cpp
index f215e22..975a28d 100644
--- a/SurgSim/Physics/UpdateCollisionRepresentations.cpp
+++ b/SurgSim/Physics/UpdateCollisionRepresentations.cpp
@@ -13,6 +13,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/ThreadPool.h"
#include "SurgSim/Physics/UpdateCollisionRepresentations.h"
#include "SurgSim/Physics/PhysicsManagerState.h"
#include "SurgSim/Collision/Representation.h"
@@ -36,14 +38,18 @@ std::shared_ptr<PhysicsManagerState> SurgSim::Physics::UpdateCollisionRepresenta
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)
+ auto threadPool = Framework::Runtime::getThreadPool();
+ std::vector<std::future<void>> tasks;
+ auto& representations = result->getActiveCollisionRepresentations();
+ for (auto& representation : representations)
{
- representation->update(dt);
- });
+ tasks.push_back(threadPool->enqueue<void>([dt, &representation]() { representation->update(dt); }));
+ }
+ for (auto& task : tasks)
+ {
+ task.get();
+ }
return result;
}
diff --git a/SurgSim/Physics/UpdateCollisionRepresentations.h b/SurgSim/Physics/UpdateCollisionRepresentations.h
index ac56444..be405bf 100644
--- a/SurgSim/Physics/UpdateCollisionRepresentations.h
+++ b/SurgSim/Physics/UpdateCollisionRepresentations.h
@@ -16,6 +16,7 @@
#ifndef SURGSIM_PHYSICS_UPDATECOLLISIONREPRESENTATIONS_H
#define SURGSIM_PHYSICS_UPDATECOLLISIONREPRESENTATIONS_H
+#include "SurgSim/Framework/Macros.h"
#include "SurgSim/Physics/Computation.h"
namespace SurgSim
@@ -31,11 +32,13 @@ public:
/// \param doCopyState whether to copy the PhysicsManagerState on update
explicit UpdateCollisionRepresentations(bool doCopyState);
+ SURGSIM_CLASSNAME(SurgSim::Physics::UpdateCollisionRepresentations);
+
/// Destructor
virtual ~UpdateCollisionRepresentations();
- virtual std::shared_ptr<PhysicsManagerState> doUpdate(
- const double& dt, const std::shared_ptr<PhysicsManagerState>& state) override;
+ std::shared_ptr<PhysicsManagerState> doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
+ override;
};
diff --git a/SurgSim/Physics/UpdateDcdData.cpp b/SurgSim/Physics/UpdateDcdData.cpp
new file mode 100644
index 0000000..2b53a32
--- /dev/null
+++ b/SurgSim/Physics/UpdateDcdData.cpp
@@ -0,0 +1,81 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/UpdateDcdData.h"
+
+#include "SurgSim/Framework/Runtime.h"
+#include "SurgSim/Framework/ThreadPool.h"
+#include "SurgSim/Physics/PhysicsManagerState.h"
+#include "SurgSim/Collision/Representation.h"
+
+#include <unordered_set>
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+UpdateDcdData::UpdateDcdData(bool copyState):
+ Computation(copyState)
+{
+
+}
+
+UpdateDcdData::~UpdateDcdData()
+{
+
+}
+
+
+std::shared_ptr<PhysicsManagerState> UpdateDcdData::doUpdate(
+ const double& dt,
+ const std::shared_ptr<PhysicsManagerState>& state)
+{
+ std::shared_ptr<PhysicsManagerState> result = state;
+
+ auto threadPool = Framework::Runtime::getThreadPool();
+ std::vector<std::future<void>> tasks;
+
+ const auto& pairs = result->getCollisionPairs();
+ std::unordered_set<Collision::Representation*> representations;
+
+ for (const auto& pair : pairs)
+ {
+ if (pair->getType() == SurgSim::Collision::COLLISION_DETECTION_TYPE_DISCRETE)
+ {
+ representations.insert(pair->getFirst().get());
+ representations.insert(pair->getSecond().get());
+ }
+ }
+
+ for (auto representation : representations)
+ {
+ tasks.push_back(threadPool->enqueue<void>([dt, representation]()
+ {
+ representation->updateDcdData();
+ }));
+ }
+
+ for (auto& task : tasks)
+ {
+ task.get();
+ }
+
+ return result;
+}
+
+}
+}
\ No newline at end of file
diff --git a/SurgSim/Physics/UpdateDcdData.h b/SurgSim/Physics/UpdateDcdData.h
new file mode 100644
index 0000000..53e8061
--- /dev/null
+++ b/SurgSim/Physics/UpdateDcdData.h
@@ -0,0 +1,49 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR 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_UPDATEDCDDATA_H
+#define SURGSIM_PHYSICS_UPDATEDCDDATA_H
+
+#include "SurgSim/Framework/Macros.h"
+#include "SurgSim/Physics/Computation.h"
+
+namespace SurgSim
+{
+
+namespace Physics
+{
+
+class UpdateDcdData : public Computation
+{
+public:
+ /// Constructor
+ explicit UpdateDcdData(bool copyState);
+
+ SURGSIM_CLASSNAME(SurgSim::Physics::UpdateDcdData);
+
+ /// Destructor
+ ~UpdateDcdData();
+
+ std::shared_ptr<PhysicsManagerState>
+ doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state) override;
+
+private:
+
+};
+
+}
+}
+
+#endif
diff --git a/SurgSim/Physics/VirtualToolCoupler.cpp b/SurgSim/Physics/VirtualToolCoupler.cpp
index bf77005..024bd34 100644
--- a/SurgSim/Physics/VirtualToolCoupler.cpp
+++ b/SurgSim/Physics/VirtualToolCoupler.cpp
@@ -15,10 +15,11 @@
#include <Eigen/Eigenvalues>
+#include "SurgSim/Collision/CollisionPair.h"
+#include "SurgSim/Collision/Representation.h"
#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"
@@ -27,7 +28,6 @@
#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;
@@ -37,6 +37,7 @@ using SurgSim::Math::Matrix33d;
using SurgSim::Math::Matrix66d;
using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Quaterniond;
+using SurgSim::Framework::checkAndConvert;
namespace SurgSim
{
@@ -53,8 +54,11 @@ VirtualToolCoupler::VirtualToolCoupler(const std::string& name) :
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())),
+ std::numeric_limits<double>::quiet_NaN())),
m_calculateInertialTorques(false),
+ m_logger(SurgSim::Framework::Logger::getLogger("Physics/VirtualToolCoupler")),
+ m_hapticOutputOnlyWhenColliding(false),
+ m_previousInputPose(Math::RigidTransform3d::Identity()),
m_poseIndex(-1),
m_linearVelocityIndex(-1),
m_angularVelocityIndex(-1),
@@ -67,25 +71,27 @@ VirtualToolCoupler::VirtualToolCoupler(const std::string& name) :
m_damperJacobianIndex(-1)
{
SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, SurgSim::DataStructures::OptionalValue<double>,
- LinearStiffness, getOptionalLinearStiffness, setOptionalLinearStiffness);
+ LinearStiffness, getOptionalLinearStiffness, setOptionalLinearStiffness);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, SurgSim::DataStructures::OptionalValue<double>,
- LinearDamping, getOptionalLinearDamping, setOptionalLinearDamping);
+ LinearDamping, getOptionalLinearDamping, setOptionalLinearDamping);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, SurgSim::DataStructures::OptionalValue<double>,
- AngularStiffness, getOptionalAngularStiffness, setOptionalAngularStiffness);
+ AngularStiffness, getOptionalAngularStiffness, setOptionalAngularStiffness);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, SurgSim::DataStructures::OptionalValue<double>,
- AngularDamping, getOptionalAngularDamping, setOptionalAngularDamping);
+ AngularDamping, getOptionalAngularDamping, setOptionalAngularDamping);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler,
- SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>, AttachmentPoint,
- getOptionalAttachmentPoint, setOptionalAttachmentPoint);
+ SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>, AttachmentPoint,
+ getOptionalAttachmentPoint, setOptionalAttachmentPoint);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, bool, CalculateInertialTorques,
- getCalculateInertialTorques, setCalculateInertialTorques);
+ getCalculateInertialTorques, setCalculateInertialTorques);
+ SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, bool, HapticOutputOnlyWhenColliding,
+ isHapticOutputOnlyWhenColliding, setHapticOutputOnlyWhenColliding);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, std::shared_ptr<SurgSim::Framework::Component>,
- Input, getInput, setInput);
+ Input, getInput, setInput);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, std::shared_ptr<SurgSim::Framework::Component>,
- Output, getOutput, setOutput);
+ Output, getOutput, setOutput);
SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, std::shared_ptr<SurgSim::Framework::Component>,
- Representation, getRepresentation, setRepresentation);
+ Representation, getRepresentation, setRepresentation);
}
VirtualToolCoupler::~VirtualToolCoupler()
@@ -99,7 +105,7 @@ const std::shared_ptr<SurgSim::Framework::Component> VirtualToolCoupler::getInpu
void VirtualToolCoupler::setInput(const std::shared_ptr<SurgSim::Framework::Component> input)
{
- m_input = std::dynamic_pointer_cast<SurgSim::Input::InputComponent>(input);
+ m_input = checkAndConvert<SurgSim::Input::InputComponent>(input, "SurgSim::Input::InputComponent");
}
const std::shared_ptr<SurgSim::Framework::Component> VirtualToolCoupler::getOutput()
@@ -109,7 +115,7 @@ const std::shared_ptr<SurgSim::Framework::Component> VirtualToolCoupler::getOutp
void VirtualToolCoupler::setOutput(const std::shared_ptr<SurgSim::Framework::Component> output)
{
- m_output = std::dynamic_pointer_cast<SurgSim::Input::OutputComponent>(output);
+ m_output = checkAndConvert<SurgSim::Input::OutputComponent>(output, "SurgSim::Input::OutputComponent");
}
const std::shared_ptr<SurgSim::Framework::Component> VirtualToolCoupler::getRepresentation()
@@ -134,23 +140,49 @@ void VirtualToolCoupler::setPoseName(const std::string& poseName)
void VirtualToolCoupler::update(double dt)
{
- SurgSim::DataStructures::DataGroup inputData;
+ DataStructures::DataGroup inputData;
m_input->getData(&inputData);
- RigidTransform3d inputPose;
inputData.poses().cacheIndex(m_poseName, &m_poseIndex);
+ inputData.vectors().cacheIndex(DataStructures::Names::LINEAR_VELOCITY, &m_linearVelocityIndex);
+ if (m_linearVelocityIndex < 0)
+ {
+ SURGSIM_LOG_ONCE(m_logger, WARNING) << getFullName() << " is receiving an input DataGroup that does not " <<
+ "contain a linear velocity vector named " << DataStructures::Names::LINEAR_VELOCITY <<
+ ", so it will be estimated.";
+ }
+ inputData.vectors().cacheIndex(DataStructures::Names::ANGULAR_VELOCITY, &m_angularVelocityIndex);
+ if (m_angularVelocityIndex < 0)
+ {
+ SURGSIM_LOG_ONCE(m_logger, WARNING) << getFullName() << " is receiving an input DataGroup that does not " <<
+ "contain an angular velocity vector named " << DataStructures::Names::ANGULAR_VELOCITY <<
+ ") so it will be estimated.";
+ }
+
+ RigidTransform3d inputPose;
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());
+ Vector3d inputLinearVelocity(Vector3d::Zero());
+ const bool gotInputLinearVelocity = inputData.vectors().get(m_linearVelocityIndex, &inputLinearVelocity);
+ if (!gotInputLinearVelocity)
+ {
+ inputLinearVelocity = (inputPose.translation() - m_previousInputPose.translation()) / dt;
+ }
+
+ Vector3d inputAngularVelocity(Vector3d::Zero());
+ const bool gotInputAngularVelocity = inputData.vectors().get(m_angularVelocityIndex, &inputAngularVelocity);
+ if (!gotInputAngularVelocity)
+ {
+ Math::computeRotationVector(inputPose, m_previousInputPose, &inputAngularVelocity);
+ inputAngularVelocity /= dt;
+ }
+ m_previousInputPose = inputPose;
+
+ RigidTransform3d inputAlignment = m_input->getLocalPose();
+ inputPose = inputAlignment * inputPose;
+ inputLinearVelocity = inputAlignment.linear() * inputLinearVelocity;
+ inputAngularVelocity = inputAlignment.rotation() * inputAngularVelocity;
+
+ RigidState objectState(m_rigid->getCurrentState());
RigidTransform3d objectPose(objectState.getPose());
Vector3d objectPosition = objectPose * m_rigid->getMassCenter();
Vector3d attachmentPoint = objectPose * m_localAttachmentPoint;
@@ -166,20 +198,17 @@ void VirtualToolCoupler::update(double dt)
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;
+
+ const Matrix33d identity3x3 = Matrix33d::Identity();
+ const Matrix33d zero3x3 = Matrix33d::Zero();
+ Matrix66d generalizedStiffness;
+ generalizedStiffness << m_linearStiffness * identity3x3, zero3x3,
+ zero3x3 , m_angularStiffness * identity3x3;
+ Matrix66d generalizedDamping;
+ generalizedDamping << m_linearDamping * identity3x3, zero3x3,
+ zero3x3 , m_angularDamping * identity3x3;
if (m_calculateInertialTorques)
{
@@ -194,16 +223,57 @@ void VirtualToolCoupler::update(double dt)
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);
-
+ RigidTransform3d outputAlignment = m_output->getLocalPose().inverse();
+ Matrix33d outputAlignmentUnScaled = outputAlignment.rotation();
+
+ bool output = true;
+ if (m_hapticOutputOnlyWhenColliding)
+ {
+ auto collision = m_rigid->getCollisionRepresentation();
+ output = (collision == nullptr) || (collision->getCollisions().unsafeGet().size() > 0);
+ }
+
+ if (output)
+ {
+ m_outputData.vectors().set(m_forceIndex, outputAlignmentUnScaled * (-force));
+ m_outputData.vectors().set(m_torqueIndex, outputAlignmentUnScaled * (-torque));
+ if (gotInputLinearVelocity && gotInputAngularVelocity)
+ {
+ m_outputData.matrices().set(m_springJacobianIndex, -generalizedStiffness);
+ m_outputData.matrices().set(m_damperJacobianIndex, -generalizedDamping);
+ }
+ else
+ {
+ m_outputData.matrices().reset(m_springJacobianIndex);
+ m_outputData.matrices().reset(m_damperJacobianIndex);
+ }
+ }
+ else
+ {
+ m_outputData.vectors().set(m_forceIndex, Vector3d::Zero());
+ m_outputData.vectors().set(m_torqueIndex, Vector3d::Zero());
+ m_outputData.matrices().reset(m_springJacobianIndex);
+ m_outputData.matrices().reset(m_damperJacobianIndex);
+ }
+
+ if (gotInputLinearVelocity)
+ {
+ m_outputData.vectors().set(m_inputLinearVelocityIndex, outputAlignment.linear() * inputLinearVelocity);
+ }
+ else
+ {
+ m_outputData.vectors().reset(m_inputLinearVelocityIndex);
+ }
+
+ if (gotInputAngularVelocity)
+ {
+ m_outputData.vectors().set(m_inputAngularVelocityIndex, outputAlignmentUnScaled * inputAngularVelocity);
+ }
+ else
+ {
+ m_outputData.vectors().reset(m_inputAngularVelocityIndex);
+ }
+ m_outputData.poses().set(m_inputPoseIndex, outputAlignment * inputPose);
m_output->setData(m_outputData);
}
}
@@ -242,14 +312,13 @@ bool VirtualToolCoupler::doWakeUp()
{
if (m_input == nullptr)
{
- SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << "VirtualToolCoupler named " <<
- getName() << " does not have an Input Component.";
+ SURGSIM_LOG_SEVERE(m_logger) <<
+ "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.";
+ SURGSIM_LOG_SEVERE(m_logger) << "VirtualToolCoupler named " << getName() << " does not have a Representation.";
return false;
}
@@ -324,7 +393,10 @@ bool VirtualToolCoupler::doWakeUp()
}
}
-
+ SURGSIM_LOG_INFO(m_logger) <<
+ "VirtualToolCoupler named '" << getName() << "' has linear stiffness " << m_linearStiffness <<
+ ", linear damping " << m_linearDamping << ", angular stiffness " << m_angularStiffness <<
+ ", and angular damping " << m_angularDamping << ". Its Physics Representation has mass " << m_rigid->getMass();
return true;
}
@@ -344,7 +416,7 @@ void VirtualToolCoupler::overrideLinearStiffness(double linearStiffness)
double VirtualToolCoupler::getLinearStiffness()
{
SURGSIM_ASSERT(isAwake() || m_optionalLinearStiffness.hasValue())
- << "Vtc parameter has not been initialized";
+ << "Vtc parameter has not been initialized";
return m_linearStiffness;
}
@@ -360,7 +432,7 @@ void VirtualToolCoupler::overrideLinearDamping(double linearDamping)
double VirtualToolCoupler::getLinearDamping()
{
SURGSIM_ASSERT(isAwake() || m_optionalLinearDamping.hasValue())
- << "Vtc parameter has not been initialized";
+ << "Vtc parameter has not been initialized";
return m_linearDamping;
}
@@ -376,7 +448,7 @@ void VirtualToolCoupler::overrideAngularStiffness(double angularStiffness)
double VirtualToolCoupler::getAngularStiffness()
{
SURGSIM_ASSERT(isAwake() || m_optionalAngularStiffness.hasValue())
- << "Vtc parameter has not been initialized";
+ << "Vtc parameter has not been initialized";
return m_angularStiffness;
}
@@ -392,7 +464,7 @@ void VirtualToolCoupler::overrideAngularDamping(double angularDamping)
double VirtualToolCoupler::getAngularDamping()
{
SURGSIM_ASSERT(isAwake() || m_optionalAngularDamping.hasValue())
- << "Vtc parameter has not been initialized";
+ << "Vtc parameter has not been initialized";
return m_angularDamping;
}
@@ -408,7 +480,7 @@ void VirtualToolCoupler::overrideAttachmentPoint(const SurgSim::Math::Vector3d&
const SurgSim::Math::Vector3d& VirtualToolCoupler::getAttachmentPoint()
{
SURGSIM_ASSERT(isAwake() || m_optionalAttachmentPoint.hasValue())
- << "Vtc parameter has not been initialized";
+ << "Vtc parameter has not been initialized";
return m_localAttachmentPoint;
}
@@ -477,5 +549,27 @@ bool VirtualToolCoupler::getCalculateInertialTorques() const
return m_calculateInertialTorques;
}
+void VirtualToolCoupler::doRetire()
+{
+ m_outputData.resetAll();
+ Behavior::doRetire();
+}
+
+void VirtualToolCoupler::setLocalActive(bool val)
+{
+ m_outputData.resetAll();
+ Behavior::setLocalActive(val);
+}
+
+bool VirtualToolCoupler::isHapticOutputOnlyWhenColliding() const
+{
+ return m_hapticOutputOnlyWhenColliding;
+}
+
+void VirtualToolCoupler::setHapticOutputOnlyWhenColliding(bool hapticOutputOnlyWhenColliding)
+{
+ m_hapticOutputOnlyWhenColliding = hapticOutputOnlyWhenColliding;
+}
+
}; /// Physics
}; /// SurgSim
diff --git a/SurgSim/Physics/VirtualToolCoupler.h b/SurgSim/Physics/VirtualToolCoupler.h
index 79de9d9..8d4b1a0 100644
--- a/SurgSim/Physics/VirtualToolCoupler.h
+++ b/SurgSim/Physics/VirtualToolCoupler.h
@@ -22,6 +22,7 @@
#include "SurgSim/DataStructures/DataGroupBuilder.h"
#include "SurgSim/DataStructures/OptionalValue.h"
#include "SurgSim/Framework/Behavior.h"
+#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/ObjectFactory.h"
namespace SurgSim
@@ -40,10 +41,21 @@ 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
+/// The VirtualToolCoupler couples a rigid object to an input/output device through a spring and damper.
+/// The object will follow the pose provided by the device. If an Output is connected, it is provided
+/// forces and torques that will push the device towards matching the object's pose and velocity.
+/// This "virtual coupling" or "god-object" paradigm is common for haptic applications utilizing a device that
+/// may update significantly faster than the physics computation thread.
+///
+/// For an overview of haptics see:
+/// Salisbury, Kenneth, Francois Conti, and Federico Barbagli. "Haptic rendering: introductory concepts."
+/// Computer Graphics and Applications, IEEE 24.2 (2004): 24-32.
+///
+/// For an introduction to virtual coupling see:
+/// Colgate, J. Edward, Michael C. Stanley, and J. Michael Brown. "Issues in the haptic display of tool use."
+/// Intelligent Robots and Systems 95.'Human Robot Interaction and Cooperative Robots',
+/// Proceedings. 1995 IEEE/RSJ International Conference on. Vol. 3. IEEE, 1995.
+class VirtualToolCoupler : public Framework::Behavior
{
public:
/// Constructor
@@ -55,34 +67,47 @@ public:
SURGSIM_CLASSNAME(SurgSim::Physics::VirtualToolCoupler);
/// \return Input Component to get the pose from
- const std::shared_ptr<SurgSim::Framework::Component> getInput();
+ const std::shared_ptr<Framework::Component> getInput();
- /// Set the Input Component
+ /// Set the Input Component.
+ /// The force calculations rely upon pose and velocity. If the input DataGroup does not contain a pose, no forces
+ /// will be calculated. If the input DataGroup does not contain linear velocity or angular velocity, they will
+ /// be estimated.
/// \param input Input Component to get the pose from
- void setInput(const std::shared_ptr<SurgSim::Framework::Component> input);
+ void setInput(const std::shared_ptr<Framework::Component> input);
/// \return Output Component to send forces and torques
- const std::shared_ptr<SurgSim::Framework::Component> getOutput();
+ const std::shared_ptr<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);
+ void setOutput(const std::shared_ptr<Framework::Component> output);
/// \return Rigid Representation that provides state and receives external forces and torques
- const std::shared_ptr<SurgSim::Framework::Component> getRepresentation();
+ const std::shared_ptr<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);
+ void setRepresentation(const std::shared_ptr<Framework::Component> rigid);
+
+ /// Get whether or not the haptic forces should be provided only during collisions.
+ /// \return false if the VTC forces and torques are sent to the output device (if any) at all times. true if
+ /// zeros are sent for the forces and torques unless the tool is colliding.
+ bool isHapticOutputOnlyWhenColliding() const;
+
+ /// Set whether or not the haptic forces should be provided only during collisions.
+ /// \param haptic false to send the VTC forces and torques to the output device (if any) at all times. true to
+ /// send zeros for the forces and torques unless the tool is colliding.
+ void setHapticOutputOnlyWhenColliding(bool haptic);
/// \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);
+ void setPoseName(const std::string& poseName = DataStructures::Names::POSE);
- virtual void update(double dt) override;
+ 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
@@ -124,11 +149,11 @@ public:
/// 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);
+ void overrideAttachmentPoint(const 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();
+ const Math::Vector3d& getAttachmentPoint();
/// Enable/disable torques that simulate inertia. This setting only has an effect if the attachment point is not
/// the mass center.
@@ -142,84 +167,87 @@ public:
/// \return true if inertia is being simulated.
bool getCalculateInertialTorques() const;
+ void doRetire() override;
+
+ void setLocalActive(bool val) override;
+
protected:
- virtual bool doInitialize() override;
- virtual bool doWakeUp() override;
- virtual int getTargetManagerType() const override;
+ bool doInitialize() override;
+ bool doWakeUp() override;
+ int getTargetManagerType() const override;
/// \return The DataGroup to be sent to the device via the OutputComponent.
- virtual SurgSim::DataStructures::DataGroup buildOutputData();
+ virtual 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);
+ void setOptionalLinearStiffness(const 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;
+ const 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);
+ void setOptionalLinearDamping(const 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;
+ const 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);
+ void setOptionalAngularStiffness(const 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;
+ const 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);
+ void setOptionalAngularDamping(const 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;
+ const 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);
+ void setOptionalAttachmentPoint(const DataStructures::OptionalValue<Math::Vector3d>& attachmentPoint);
/// Used for Serialization.
/// \return The OptionalValue object containing the attachment point.
- const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>& getOptionalAttachmentPoint() const;
+ const DataStructures::OptionalValue<Math::Vector3d>& getOptionalAttachmentPoint() const;
/// User supplied Vtc stiffness parameter in linear mode (in N·m-1)
- SurgSim::DataStructures::OptionalValue<double> m_optionalLinearStiffness;
+ 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;
+ DataStructures::OptionalValue<double> m_optionalLinearDamping;
/// User supplied Vtc stiffness parameter in angular mode (in N·m rad-1)
- SurgSim::DataStructures::OptionalValue<double> m_optionalAngularStiffness;
+ 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;
+ DataStructures::OptionalValue<double> m_optionalAngularDamping;
/// User supplied attachment point
- SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d> m_optionalAttachmentPoint;
+ DataStructures::OptionalValue<Math::Vector3d> m_optionalAttachmentPoint;
/// The DataGroup to output
- SurgSim::DataStructures::DataGroup m_outputData;
+ DataStructures::DataGroup m_outputData;
/// The input component.
- std::shared_ptr<SurgSim::Input::InputComponent> m_input;
+ std::shared_ptr<Input::InputComponent> m_input;
/// The output component.
- std::shared_ptr<SurgSim::Input::OutputComponent> m_output;
+ std::shared_ptr<Input::OutputComponent> m_output;
private:
- std::shared_ptr<SurgSim::Physics::RigidRepresentation> m_rigid;
+ std::shared_ptr<RigidRepresentation> m_rigid;
std::string m_poseName;
/// Used Vtc stiffness parameter in linear mode (in N·m-1)
@@ -241,12 +269,21 @@ private:
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;
+ 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;
+ /// The logger.
+ std::shared_ptr<Framework::Logger> m_logger;
+
+ /// Whether or not the VTC sends forces and torques to the output device (if any) only when the tool is colliding.
+ bool m_hapticOutputOnlyWhenColliding;
+
+ /// The previous input pose.
+ Math::RigidTransform3d m_previousInputPose;
+
///@{
/// Cached DataGroup indices.
int m_poseIndex;
diff --git a/SurgSim/Serialize/CMakeLists.txt b/SurgSim/Serialize/CMakeLists.txt
index 2868c11..bb063e3 100644
--- a/SurgSim/Serialize/CMakeLists.txt
+++ b/SurgSim/Serialize/CMakeLists.txt
@@ -15,7 +15,6 @@
include_directories(
- ${OSG_INCLUDE_DIR}
)
set(SURGSIM_SERIALIZE_SOURCES
@@ -28,12 +27,12 @@ set(SURGSIM_SERIALIZE_HEADERS
ShapesFactory.h
ShapesFactory-inl.h
)
+surgsim_create_library_header(Serialize.h "${SURGSIM_SERIALIZE_HEADERS}")
surgsim_add_library(
SurgSimSerialize
"${SURGSIM_SERIALIZE_SOURCES}"
"${SURGSIM_SERIALIZE_HEADERS}"
- "SurgSim/Serialize"
)
set(LIBS
diff --git a/SurgSim/Serialize/GraphicsConvert.h b/SurgSim/Serialize/GraphicsConvert.h
index b4e1a3c..5ea7e8f 100644
--- a/SurgSim/Serialize/GraphicsConvert.h
+++ b/SurgSim/Serialize/GraphicsConvert.h
@@ -25,22 +25,24 @@
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);
- };
+
+/// 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
+#endif // SURGSIM_SERIALIZE_GRAPHICSCONVERT_H
diff --git a/SurgSim/Testing/CMakeLists.txt b/SurgSim/Testing/CMakeLists.txt
index 06c54e9..585bd87 100644
--- a/SurgSim/Testing/CMakeLists.txt
+++ b/SurgSim/Testing/CMakeLists.txt
@@ -1,5 +1,5 @@
# This file is a part of the OpenSurgSim project.
-# Copyright 2013, SimQuest Solutions Inc.
+# Copyright 2013-2015, SimQuest Solutions Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -20,6 +20,7 @@ include_directories(
set(SURGSIM_TESTING_SOURCES
MathUtilities.cpp
+ MockInputComponent.cpp
MockInputOutput.cpp
SerializationMockComponent.cpp
TestCube.cpp
@@ -28,19 +29,20 @@ set(SURGSIM_TESTING_SOURCES
set(SURGSIM_TESTING_HEADERS
MathUtilities.h
+ MockInputComponent.h
MockInputOutput.h
MockPhysicsManager.h
SerializationMockComponent.h
TestCube.h
+ Utilities.h
)
+surgsim_create_library_header(Testing.h "${SURGSIM_TESTING_HEADERS}")
-surgsim_add_library(
+add_library(
SurgSimTesting
- "${SURGSIM_TESTING_SOURCES}"
- "${SURGSIM_TESTING_HEADERS}"
- "SurgSim/Testing"
+ ${SURGSIM_TESTING_SOURCES}
+ ${SURGSIM_TESTING_HEADERS}
)
-add_dependencies(SurgSimTesting yaml-cpp)
set(LIBS
SurgSimDataStructures
@@ -57,6 +59,7 @@ if(GLUT_FOUND)
add_subdirectory(VisualTestCommon)
endif(GLUT_FOUND)
-file(COPY Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
+# Copy all shared data which can be used by other component's unit tests
+file(COPY Data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
set_target_properties(SurgSimTesting PROPERTIES FOLDER "Testing")
diff --git a/SurgSim/DataStructures/UnitTests/Data/PlyReaderTests/Cube.ply b/SurgSim/Testing/Data/Geometry/Cube.ply
similarity index 100%
rename from SurgSim/DataStructures/UnitTests/Data/PlyReaderTests/Cube.ply
rename to SurgSim/Testing/Data/Geometry/Cube.ply
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgMeshRepresentationTests/Cube.ply b/SurgSim/Testing/Data/Geometry/Cube_with_texture.ply
similarity index 100%
rename from SurgSim/Graphics/UnitTests/Data/OsgMeshRepresentationTests/Cube.ply
rename to SurgSim/Testing/Data/Geometry/Cube_with_texture.ply
diff --git a/SurgSim/Testing/MeshShapeData/InvalidMesh.ply b/SurgSim/Testing/Data/Geometry/InvalidMesh.ply
similarity index 100%
rename from SurgSim/Testing/MeshShapeData/InvalidMesh.ply
rename to SurgSim/Testing/Data/Geometry/InvalidMesh.ply
diff --git a/SurgSim/Testing/OsgSceneryRepresentationTests/Torus.mtl b/SurgSim/Testing/Data/Geometry/Torus.mtl
similarity index 100%
rename from SurgSim/Testing/OsgSceneryRepresentationTests/Torus.mtl
rename to SurgSim/Testing/Data/Geometry/Torus.mtl
diff --git a/SurgSim/Testing/OsgSceneryRepresentationTests/Torus.obj b/SurgSim/Testing/Data/Geometry/Torus.obj
similarity index 100%
rename from SurgSim/Testing/OsgSceneryRepresentationTests/Torus.obj
rename to SurgSim/Testing/Data/Geometry/Torus.obj
diff --git a/SurgSim/Testing/OsgSceneryRepresentationTests/Torus.osgb b/SurgSim/Testing/Data/Geometry/Torus.osgb
similarity index 100%
rename from SurgSim/Testing/OsgSceneryRepresentationTests/Torus.osgb
rename to SurgSim/Testing/Data/Geometry/Torus.osgb
diff --git a/SurgSim/Physics/RenderTests/Data/box.ply b/SurgSim/Testing/Data/Geometry/box.ply
similarity index 100%
rename from SurgSim/Physics/RenderTests/Data/box.ply
rename to SurgSim/Testing/Data/Geometry/box.ply
diff --git a/SurgSim/Testing/Data/Geometry/cube.osgt b/SurgSim/Testing/Data/Geometry/cube.osgt
new file mode 100644
index 0000000..38c2803
--- /dev/null
+++ b/SurgSim/Testing/Data/Geometry/cube.osgt
@@ -0,0 +1,162 @@
+#Ascii Scene
+#Version 100
+#Generator OpenSceneGraph 3.2.1
+
+osg::Group {
+ UniqueID 1
+ Name "cube.obj"
+ Children 1 {
+ osg::Geode {
+ UniqueID 2
+ Name "Cube"
+ Drawables 1 {
+ osg::Geometry {
+ UniqueID 3
+ DataVariance STATIC
+ StateSet TRUE {
+ osg::StateSet {
+ UniqueID 4
+ DataVariance STATIC
+ AttributeList 1 {
+ osg::Material {
+ UniqueID 5
+ Name "Material"
+ Ambient TRUE Front 0 0 0 1 Back 0 0 0 1
+ Diffuse TRUE Front 0.64 0.64 0.64 1 Back 0.64 0.64 0.64 1
+ Specular TRUE Front 0.5 0.5 0.5 1 Back 0.5 0.5 0.5 1
+ Emission TRUE Front 0 0 0 1 Back 0 0 0 1
+ Shininess TRUE Front 12.288 Back 12.288
+ }
+ Value OFF
+ }
+ }
+ }
+ PrimitiveSetList 6 {
+ DrawElementsUShort GL_TRIANGLE_STRIP 0 4 {
+ 1 2 0 3
+ }
+ DrawElementsUShort GL_TRIANGLE_STRIP 0 4 {
+ 7 4 6 5
+ }
+ DrawElementsUShort GL_TRIANGLE_STRIP 0 4 {
+ 23 20 22 21
+ }
+ DrawElementsUShort GL_TRIANGLE_STRIP 0 4 {
+ 19 16 18 17
+ }
+ DrawElementsUShort GL_TRIANGLE_STRIP 0 4 {
+ 13 14 12 15
+ }
+ DrawElementsUShort GL_TRIANGLE_STRIP 0 4 {
+ 9 10 8 11
+ }
+
+ }
+ VertexData {
+ Array TRUE ArrayID 1 Vec3fArray 24 {
+ 0.1 0.1 -0.1
+ 0.1 -0.1 -0.1
+ -0.1 -0.1 -0.1
+ -0.1 0.1 -0.1
+ 0.1 0.0999999 0.1
+ -0.1 0.1 0.1
+ -0.1 -0.1 0.1
+ 0.0999999 -0.1 0.1
+ 0.1 0.1 -0.1
+ 0.1 0.0999999 0.1
+ 0.0999999 -0.1 0.1
+ 0.1 -0.1 -0.1
+ 0.1 -0.1 -0.1
+ 0.0999999 -0.1 0.1
+ -0.1 -0.1 0.1
+ -0.1 -0.1 -0.1
+ -0.1 -0.1 -0.1
+ -0.1 -0.1 0.1
+ -0.1 0.1 0.1
+ -0.1 0.1 -0.1
+ 0.1 0.0999999 0.1
+ 0.1 0.1 -0.1
+ -0.1 0.1 -0.1
+ -0.1 0.1 0.1
+ }
+ Indices FALSE
+ Binding BIND_PER_VERTEX
+ Normalize 0
+ }
+ NormalData {
+ Array TRUE ArrayID 2 Vec3fArray 24 {
+ 0 -0 -1
+ 0 -0 -1
+ 0 -0 -1
+ 0 -0 -1
+ 0 -0 1
+ 0 -0 1
+ 0 -0 1
+ 0 -0 1
+ 1 -0 0
+ 1 -0 0
+ 1 -0 0
+ 1 -0 0
+ -0 -1 -0
+ -0 -1 -0
+ -0 -1 -0
+ -0 -1 -0
+ -1 0 -0
+ -1 0 -0
+ -1 0 -0
+ -1 0 -0
+ 0 1 0
+ 0 1 0
+ 0 1 0
+ 0 1 0
+ }
+ Indices FALSE
+ Binding BIND_PER_VERTEX
+ Normalize 0
+ }
+ ColorData {
+ Array TRUE ArrayID 3 Vec4fArray 1 {
+ 1 1 1 1
+ }
+ Indices FALSE
+ Binding BIND_OVERALL
+ Normalize 0
+ }
+ TexCoordData 1 {
+ Data {
+ Array TRUE ArrayID 4 Vec2fArray 24 {
+ 0.9999 0.9999
+ 0.0001 0.9999
+ 0.0001 0.0001
+ 0.9999 0.0001
+ 0.9999 0.9999
+ 0.0001 0.9999
+ 0.0001 0.0001
+ 0.9999 0.0001
+ 0.9999 0.0001
+ 0.9999 0.9999
+ 0.0001 0.9999
+ 0.0001 0.0001
+ 0.9999 0.0001
+ 0.9999 0.9999
+ 0.0001 0.9999
+ 0.0001 0.0001
+ 0.0001 0.9999
+ 0.0001 0.0001
+ 0.9999 0.0001
+ 0.9999 0.9999
+ 0.9999 0.0001
+ 0.9999 0.9999
+ 0.0001 0.9999
+ 0.0001 0.0001
+ }
+ Indices FALSE
+ Binding BIND_PER_VERTEX
+ Normalize 0
+ }
+ }
+ }
+ }
+ }
+ }
+}
diff --git a/SurgSim/Testing/OctreeShapeData/invalid-staple.vox b/SurgSim/Testing/Data/Geometry/invalid-staple.ply
similarity index 100%
rename from SurgSim/Testing/OctreeShapeData/invalid-staple.vox
rename to SurgSim/Testing/Data/Geometry/invalid-staple.ply
diff --git a/SurgSim/Testing/Data/Geometry/plane.ply b/SurgSim/Testing/Data/Geometry/plane.ply
new file mode 100644
index 0000000..6e8de92
--- /dev/null
+++ b/SurgSim/Testing/Data/Geometry/plane.ply
@@ -0,0 +1,70 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.71 (sub 0) - www.blender.org, source file: ''
+element vertex 25
+property float x
+property float y
+property float z
+property float nx
+property float ny
+property float nz
+element face 32
+property list uchar uint vertex_indices
+end_header
+1.000000 0.000000 0.000000 -0.000000 0.000000 1.000000
+1.000000 0.500000 0.000000 -0.000000 0.000000 1.000000
+0.500000 0.500000 0.000000 -0.000000 0.000000 1.000000
+0.000000 0.000000 0.000000 -0.000000 0.000000 1.000000
+0.000000 0.500000 0.000000 -0.000000 0.000000 1.000000
+-0.500000 0.500000 0.000000 -0.000000 0.000000 1.000000
+0.000000 -1.000000 0.000000 -0.000000 0.000000 1.000000
+0.000000 -0.500000 0.000000 -0.000000 0.000000 1.000000
+-0.500000 -0.500000 0.000000 -0.000000 0.000000 1.000000
+1.000000 -1.000000 0.000000 -0.000000 0.000000 1.000000
+1.000000 -0.500000 0.000000 -0.000000 0.000000 1.000000
+0.500000 -0.500000 0.000000 -0.000000 0.000000 1.000000
+0.500000 0.000000 0.000000 0.000000 0.000000 1.000000
+-1.000000 0.000000 0.000000 0.000000 0.000000 1.000000
+-0.500000 0.000000 0.000000 0.000000 0.000000 1.000000
+-1.000000 0.500000 0.000000 0.000000 0.000000 1.000000
+-1.000000 -1.000000 0.000000 0.000000 0.000000 1.000000
+-0.500000 -1.000000 0.000000 0.000000 0.000000 1.000000
+-1.000000 -0.500000 0.000000 0.000000 0.000000 1.000000
+0.500000 -1.000000 0.000000 0.000000 0.000000 1.000000
+0.500000 1.000000 0.000000 -0.000000 0.000000 1.000000
+0.000000 1.000000 0.000000 -0.000000 0.000000 1.000000
+1.000000 1.000000 0.000000 -0.000000 0.000000 1.000000
+-0.500000 1.000000 0.000000 -0.000000 0.000000 1.000000
+-1.000000 1.000000 0.000000 -0.000000 0.000000 1.000000
+3 0 1 2
+3 3 4 5
+3 6 7 8
+3 9 10 11
+3 3 12 4
+3 13 14 15
+3 16 17 18
+3 6 19 7
+3 2 20 21
+3 2 1 20
+3 1 22 20
+3 5 23 24
+3 5 4 23
+3 4 21 23
+3 8 14 13
+3 8 7 14
+3 7 3 14
+3 11 12 3
+3 11 10 12
+3 10 0 12
+3 4 2 21
+3 4 12 2
+3 12 0 2
+3 15 5 24
+3 15 14 5
+3 14 3 5
+3 18 8 13
+3 18 17 8
+3 17 6 8
+3 7 11 3
+3 7 19 11
+3 19 9 11
diff --git a/SurgSim/Physics/RenderTests/Data/sphere.ply b/SurgSim/Testing/Data/Geometry/sphere.ply
similarity index 100%
rename from SurgSim/Physics/RenderTests/Data/sphere.ply
rename to SurgSim/Testing/Data/Geometry/sphere.ply
diff --git a/SurgSim/Testing/Data/Geometry/sphere0_025.ply b/SurgSim/Testing/Data/Geometry/sphere0_025.ply
new file mode 100644
index 0000000..2bdbf14
--- /dev/null
+++ b/SurgSim/Testing/Data/Geometry/sphere0_025.ply
@@ -0,0 +1,492 @@
+ply
+format ascii 1.0
+comment Created by Blender 2.76 (sub 0) - www.blender.org, source file: ''
+element vertex 162
+property float x
+property float y
+property float z
+element face 320
+property list uchar uint vertex_indices
+end_header
+0.000000 0.000000 -0.025000
+0.005080 -0.003690 -0.024199
+-0.001940 -0.005971 -0.024199
+0.018090 -0.013143 -0.011180
+0.015239 -0.011071 -0.016438
+0.020318 -0.007381 -0.012558
+-0.006279 0.000000 -0.024199
+-0.001940 0.005971 -0.024199
+0.005080 0.003690 -0.024199
+0.021517 -0.011071 -0.006279
+-0.006910 -0.021266 -0.011180
+-0.000741 -0.021605 -0.012558
+-0.003880 -0.023886 -0.006279
+-0.022361 0.000000 -0.011180
+-0.020776 -0.005971 -0.012557
+-0.023916 -0.003690 -0.006279
+-0.006910 0.021266 -0.011180
+-0.012099 0.017914 -0.012558
+-0.010900 0.021605 -0.006279
+0.018090 0.013143 -0.011180
+0.013299 0.017043 -0.012558
+0.017179 0.017043 -0.006279
+0.017179 -0.017043 -0.006279
+-0.010900 -0.021605 -0.006279
+-0.023916 0.003690 -0.006279
+-0.003880 0.023886 -0.006279
+0.021517 0.011071 -0.006279
+0.006910 -0.021266 0.011180
+0.012099 -0.017914 0.012558
+0.005821 -0.017914 0.016438
+-0.018090 -0.013143 0.011180
+-0.013299 -0.017043 0.012558
+-0.015239 -0.011071 0.016438
+-0.018090 0.013143 0.011180
+-0.020318 0.007381 0.012558
+-0.015239 0.011071 0.016438
+0.006910 0.021266 0.011180
+0.000741 0.021605 0.012558
+0.005821 0.017914 0.016438
+0.022361 0.000000 0.011180
+0.020776 0.005971 0.012557
+0.018836 0.000000 0.016438
+0.006279 0.000000 0.024199
+0.001940 0.005971 0.024199
+0.000000 0.000000 0.025000
+0.013143 0.000000 0.021266
+0.009045 0.006572 0.022361
+0.015955 0.006572 0.018090
+0.004061 0.012500 0.021266
+0.011180 0.013143 0.018090
+0.017205 0.012500 0.013143
+0.012099 0.017914 0.012558
+-0.005080 0.003690 0.024199
+-0.003455 0.010633 0.022361
+-0.001320 0.017205 0.018090
+-0.010633 0.007725 0.021266
+-0.009045 0.014694 0.018090
+-0.006572 0.020225 0.013143
+-0.013299 0.017043 0.012558
+-0.005080 -0.003690 0.024199
+-0.011180 0.000000 0.022361
+-0.016770 0.004061 0.018090
+-0.010633 -0.007725 0.021266
+-0.016770 -0.004061 0.018090
+-0.021266 0.000000 0.013143
+-0.020318 -0.007381 0.012558
+0.001940 -0.005971 0.024199
+-0.003455 -0.010633 0.022361
+-0.009045 -0.014694 0.018090
+0.004061 -0.012500 0.021266
+-0.001320 -0.017205 0.018090
+-0.006572 -0.020225 0.013143
+0.000741 -0.021605 0.012558
+0.009045 -0.006572 0.022361
+0.011180 -0.013143 0.018090
+0.015955 -0.006572 0.018090
+0.017205 -0.012500 0.013143
+0.020776 -0.005971 0.012557
+0.023916 0.003690 0.006279
+0.023776 0.007725 0.000000
+0.021545 0.010633 0.006910
+0.020225 0.014695 -0.000000
+0.016771 0.017205 0.006910
+0.014695 0.020225 -0.000000
+0.010900 0.021605 0.006279
+0.003880 0.023886 0.006279
+0.000000 0.025000 0.000000
+-0.003455 0.023776 0.006910
+-0.007725 0.023776 -0.000000
+-0.011180 0.021266 0.006910
+-0.014695 0.020225 -0.000000
+-0.017179 0.017043 0.006279
+-0.021517 0.011071 0.006279
+-0.023776 0.007725 0.000000
+-0.023680 0.004061 0.006910
+-0.025000 -0.000000 0.000000
+-0.023680 -0.004061 0.006910
+-0.023776 -0.007725 -0.000000
+-0.021517 -0.011071 0.006279
+-0.017179 -0.017043 0.006279
+-0.014695 -0.020225 0.000000
+-0.011180 -0.021266 0.006910
+-0.007725 -0.023776 -0.000000
+-0.003455 -0.023776 0.006910
+0.000000 -0.025000 -0.000000
+0.003880 -0.023886 0.006279
+0.010900 -0.021605 0.006279
+0.014695 -0.020225 0.000000
+0.016771 -0.017205 0.006910
+0.020225 -0.014695 -0.000000
+0.021545 -0.010633 0.006910
+0.023776 -0.007725 -0.000000
+0.023916 -0.003690 0.006279
+0.007725 0.023776 -0.000000
+0.011180 0.021266 -0.006910
+0.003455 0.023776 -0.006910
+0.006572 0.020225 -0.013143
+-0.000741 0.021605 -0.012558
+-0.020225 0.014695 -0.000000
+-0.016770 0.017205 -0.006910
+-0.021545 0.010633 -0.006910
+-0.017205 0.012500 -0.013143
+-0.020776 0.005971 -0.012557
+-0.020225 -0.014695 0.000000
+-0.021545 -0.010633 -0.006910
+-0.016770 -0.017205 -0.006910
+-0.017205 -0.012500 -0.013143
+-0.012099 -0.017914 -0.012558
+0.007725 -0.023776 0.000000
+0.003455 -0.023776 -0.006910
+0.011180 -0.021266 -0.006910
+0.006572 -0.020225 -0.013143
+0.013299 -0.017043 -0.012558
+0.025000 -0.000000 0.000000
+0.023680 -0.004061 -0.006910
+0.023680 0.004061 -0.006910
+0.021266 -0.000000 -0.013143
+0.020318 0.007381 -0.012558
+0.015239 0.011071 -0.016438
+0.010633 0.007725 -0.021266
+0.009045 0.014694 -0.018090
+0.003455 0.010633 -0.022361
+0.001320 0.017205 -0.018090
+-0.004061 0.012500 -0.021266
+-0.005821 0.017914 -0.016438
+-0.011180 0.013143 -0.018090
+-0.009045 0.006572 -0.022361
+-0.015955 0.006572 -0.018090
+-0.013143 0.000000 -0.021266
+-0.018836 0.000000 -0.016438
+-0.015955 -0.006572 -0.018090
+-0.009045 -0.006572 -0.022361
+-0.011180 -0.013143 -0.018090
+-0.004061 -0.012500 -0.021266
+-0.005821 -0.017914 -0.016438
+0.016770 0.004061 -0.018090
+0.016770 -0.004061 -0.018090
+0.011180 -0.000000 -0.022361
+0.010633 -0.007725 -0.021266
+0.001320 -0.017205 -0.018090
+0.003455 -0.010633 -0.022361
+0.009045 -0.014694 -0.018090
+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 43 44
+3 45 46 42
+3 41 47 45
+3 42 46 43
+3 46 48 43
+3 45 47 46
+3 47 49 46
+3 46 49 48
+3 49 38 48
+3 41 40 47
+3 40 50 47
+3 47 50 49
+3 50 51 49
+3 49 51 38
+3 51 36 38
+3 43 52 44
+3 48 53 43
+3 38 54 48
+3 43 53 52
+3 53 55 52
+3 48 54 53
+3 54 56 53
+3 53 56 55
+3 56 35 55
+3 38 37 54
+3 37 57 54
+3 54 57 56
+3 57 58 56
+3 56 58 35
+3 58 33 35
+3 52 59 44
+3 55 60 52
+3 35 61 55
+3 52 60 59
+3 60 62 59
+3 55 61 60
+3 61 63 60
+3 60 63 62
+3 63 32 62
+3 35 34 61
+3 34 64 61
+3 61 64 63
+3 64 65 63
+3 63 65 32
+3 65 30 32
+3 59 66 44
+3 62 67 59
+3 32 68 62
+3 59 67 66
+3 67 69 66
+3 62 68 67
+3 68 70 67
+3 67 70 69
+3 70 29 69
+3 32 31 68
+3 31 71 68
+3 68 71 70
+3 71 72 70
+3 70 72 29
+3 72 27 29
+3 66 42 44
+3 69 73 66
+3 29 74 69
+3 66 73 42
+3 73 45 42
+3 69 74 73
+3 74 75 73
+3 73 75 45
+3 75 41 45
+3 29 28 74
+3 28 76 74
+3 74 76 75
+3 76 77 75
+3 75 77 41
+3 77 39 41
+3 78 40 39
+3 79 80 78
+3 26 81 79
+3 78 80 40
+3 80 50 40
+3 79 81 80
+3 81 82 80
+3 80 82 50
+3 82 51 50
+3 26 21 81
+3 21 83 81
+3 81 83 82
+3 83 84 82
+3 82 84 51
+3 84 36 51
+3 85 37 36
+3 86 87 85
+3 25 88 86
+3 85 87 37
+3 87 57 37
+3 86 88 87
+3 88 89 87
+3 87 89 57
+3 89 58 57
+3 25 18 88
+3 18 90 88
+3 88 90 89
+3 90 91 89
+3 89 91 58
+3 91 33 58
+3 92 34 33
+3 93 94 92
+3 24 95 93
+3 92 94 34
+3 94 64 34
+3 93 95 94
+3 95 96 94
+3 94 96 64
+3 96 65 64
+3 24 15 95
+3 15 97 95
+3 95 97 96
+3 97 98 96
+3 96 98 65
+3 98 30 65
+3 99 31 30
+3 100 101 99
+3 23 102 100
+3 99 101 31
+3 101 71 31
+3 100 102 101
+3 102 103 101
+3 101 103 71
+3 103 72 71
+3 23 12 102
+3 12 104 102
+3 102 104 103
+3 104 105 103
+3 103 105 72
+3 105 27 72
+3 106 28 27
+3 107 108 106
+3 22 109 107
+3 106 108 28
+3 108 76 28
+3 107 109 108
+3 109 110 108
+3 108 110 76
+3 110 77 76
+3 22 9 109
+3 9 111 109
+3 109 111 110
+3 111 112 110
+3 110 112 77
+3 112 39 77
+3 84 85 36
+3 83 113 84
+3 21 114 83
+3 84 113 85
+3 113 86 85
+3 83 114 113
+3 114 115 113
+3 113 115 86
+3 115 25 86
+3 21 20 114
+3 20 116 114
+3 114 116 115
+3 116 117 115
+3 115 117 25
+3 117 16 25
+3 91 92 33
+3 90 118 91
+3 18 119 90
+3 91 118 92
+3 118 93 92
+3 90 119 118
+3 119 120 118
+3 118 120 93
+3 120 24 93
+3 18 17 119
+3 17 121 119
+3 119 121 120
+3 121 122 120
+3 120 122 24
+3 122 13 24
+3 98 99 30
+3 97 123 98
+3 15 124 97
+3 98 123 99
+3 123 100 99
+3 97 124 123
+3 124 125 123
+3 123 125 100
+3 125 23 100
+3 15 14 124
+3 14 126 124
+3 124 126 125
+3 126 127 125
+3 125 127 23
+3 127 10 23
+3 105 106 27
+3 104 128 105
+3 12 129 104
+3 105 128 106
+3 128 107 106
+3 104 129 128
+3 129 130 128
+3 128 130 107
+3 130 22 107
+3 12 11 129
+3 11 131 129
+3 129 131 130
+3 131 132 130
+3 130 132 22
+3 132 3 22
+3 112 78 39
+3 111 133 112
+3 9 134 111
+3 112 133 78
+3 133 79 78
+3 111 134 133
+3 134 135 133
+3 133 135 79
+3 135 26 79
+3 9 5 134
+3 5 136 134
+3 134 136 135
+3 136 137 135
+3 135 137 26
+3 137 19 26
+3 138 20 19
+3 139 140 138
+3 8 141 139
+3 138 140 20
+3 140 116 20
+3 139 141 140
+3 141 142 140
+3 140 142 116
+3 142 117 116
+3 8 7 141
+3 7 143 141
+3 141 143 142
+3 143 144 142
+3 142 144 117
+3 144 16 117
+3 144 17 16
+3 143 145 144
+3 7 146 143
+3 144 145 17
+3 145 121 17
+3 143 146 145
+3 146 147 145
+3 145 147 121
+3 147 122 121
+3 7 6 146
+3 6 148 146
+3 146 148 147
+3 148 149 147
+3 147 149 122
+3 149 13 122
+3 149 14 13
+3 148 150 149
+3 6 151 148
+3 149 150 14
+3 150 126 14
+3 148 151 150
+3 151 152 150
+3 150 152 126
+3 152 127 126
+3 6 2 151
+3 2 153 151
+3 151 153 152
+3 153 154 152
+3 152 154 127
+3 154 10 127
+3 137 138 19
+3 136 155 137
+3 5 156 136
+3 137 155 138
+3 155 139 138
+3 136 156 155
+3 156 157 155
+3 155 157 139
+3 157 8 139
+3 5 4 156
+3 4 158 156
+3 156 158 157
+3 158 1 157
+3 157 1 8
+3 1 0 8
+3 154 11 10
+3 153 159 154
+3 2 160 153
+3 154 159 11
+3 159 131 11
+3 153 160 159
+3 160 161 159
+3 159 161 131
+3 161 132 131
+3 2 1 160
+3 1 158 160
+3 160 158 161
+3 158 4 161
+3 161 4 132
+3 4 3 132
diff --git a/SurgSim/Testing/Data/Geometry/sphere0_5.mtl b/SurgSim/Testing/Data/Geometry/sphere0_5.mtl
new file mode 100644
index 0000000..70d3ba1
--- /dev/null
+++ b/SurgSim/Testing/Data/Geometry/sphere0_5.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/SurgSim/Testing/Data/Geometry/sphere0_5.obj b/SurgSim/Testing/Data/Geometry/sphere0_5.obj
new file mode 100644
index 0000000..cfaaa00
--- /dev/null
+++ b/SurgSim/Testing/Data/Geometry/sphere0_5.obj
@@ -0,0 +1,1604 @@
+# Blender v2.73 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib sphere0_5.mtl
+o Sphere
+v -0.114861 0.492527 -0.001202
+v -0.208658 0.464074 -0.001202
+v -0.295101 0.417869 -0.001202
+v -0.370869 0.355688 -0.001202
+v -0.433051 0.279919 -0.001202
+v -0.479256 0.193476 -0.001202
+v -0.507709 0.099680 -0.001202
+v -0.517316 0.002134 -0.001202
+v -0.507709 -0.095411 -0.001202
+v -0.479256 -0.189207 -0.001202
+v -0.433051 -0.275651 -0.001202
+v -0.370869 -0.351419 -0.001202
+v -0.295101 -0.413600 -0.001202
+v -0.208658 -0.459805 -0.001202
+v -0.114861 -0.488258 -0.001202
+v -0.112987 0.492527 -0.020232
+v -0.204981 0.464074 -0.038531
+v -0.289764 0.417869 -0.055395
+v -0.364076 0.355688 -0.070177
+v -0.425063 0.279919 -0.082308
+v -0.470380 0.193476 -0.091322
+v -0.498286 0.099680 -0.096873
+v -0.507709 0.002134 -0.098747
+v -0.498286 -0.095411 -0.096873
+v -0.470380 -0.189207 -0.091322
+v -0.425063 -0.275651 -0.082308
+v -0.364076 -0.351419 -0.070177
+v -0.289764 -0.413600 -0.055395
+v -0.204981 -0.459805 -0.038531
+v -0.112987 -0.488258 -0.020232
+v -0.107436 0.492527 -0.038531
+v -0.194093 0.464074 -0.074425
+v -0.273956 0.417869 -0.107505
+v -0.343957 0.355688 -0.136501
+v -0.401405 0.279919 -0.160297
+v -0.444093 0.193476 -0.177978
+v -0.470380 0.099680 -0.188867
+v -0.479256 0.002134 -0.192543
+v -0.470380 -0.095411 -0.188867
+v -0.444093 -0.189207 -0.177978
+v -0.401405 -0.275651 -0.160297
+v -0.343957 -0.351419 -0.136501
+v -0.273956 -0.413600 -0.107505
+v -0.194093 -0.459805 -0.074425
+v -0.107436 -0.488258 -0.038531
+v -0.098422 0.492527 -0.055395
+v -0.176411 0.464074 -0.107505
+v -0.248286 0.417869 -0.155531
+v -0.311285 0.355688 -0.197625
+v -0.362987 0.279919 -0.232172
+v -0.401405 0.193476 -0.257842
+v -0.425063 0.099680 -0.273649
+v -0.433051 0.002134 -0.278987
+v -0.425063 -0.095411 -0.273649
+v -0.401405 -0.189207 -0.257842
+v -0.362987 -0.275651 -0.232172
+v -0.311285 -0.351419 -0.197625
+v -0.248286 -0.413600 -0.155531
+v -0.176411 -0.459805 -0.107505
+v -0.098422 -0.488258 -0.055395
+v -0.086291 0.492527 -0.070177
+v -0.152615 0.464074 -0.136501
+v -0.213740 0.417869 -0.197625
+v -0.267316 0.355688 -0.251202
+v -0.311285 0.279919 -0.295171
+v -0.343957 0.193476 -0.327842
+v -0.364076 0.099680 -0.347962
+v -0.370869 0.002134 -0.354755
+v -0.364076 -0.095411 -0.347962
+v -0.343957 -0.189207 -0.327842
+v -0.311285 -0.275651 -0.295171
+v -0.267316 -0.351419 -0.251202
+v -0.213740 -0.413600 -0.197625
+v -0.152615 -0.459805 -0.136501
+v -0.086291 -0.488258 -0.070176
+v -0.071509 0.492527 -0.082308
+v -0.123620 0.464074 -0.160297
+v -0.171645 0.417869 -0.232172
+v -0.213740 0.355688 -0.295171
+v -0.248286 0.279919 -0.346873
+v -0.273956 0.193476 -0.385291
+v -0.289764 0.099680 -0.408948
+v -0.295101 0.002134 -0.416937
+v -0.289764 -0.095411 -0.408948
+v -0.273956 -0.189207 -0.385291
+v -0.248286 -0.275651 -0.346873
+v -0.213740 -0.351419 -0.295171
+v -0.171645 -0.413600 -0.232172
+v -0.123620 -0.459805 -0.160297
+v -0.071509 -0.488258 -0.082307
+v -0.054645 0.492527 -0.091322
+v -0.090539 0.464074 -0.177978
+v -0.123620 0.417869 -0.257842
+v -0.152615 0.355688 -0.327842
+v -0.176411 0.279919 -0.385291
+v -0.194093 0.193476 -0.427978
+v -0.204981 0.099680 -0.454265
+v -0.208658 0.002134 -0.463142
+v -0.204981 -0.095411 -0.454265
+v -0.194093 -0.189207 -0.427978
+v -0.176411 -0.275651 -0.385291
+v -0.152615 -0.351419 -0.327842
+v -0.123620 -0.413600 -0.257842
+v -0.090539 -0.459805 -0.177978
+v -0.054645 -0.488258 -0.091322
+v -0.036346 0.492527 -0.096873
+v -0.054645 0.464074 -0.188867
+v -0.071509 0.417869 -0.273649
+v -0.086291 0.355688 -0.347962
+v -0.098422 0.279919 -0.408948
+v -0.107436 0.193476 -0.454265
+v -0.112987 0.099680 -0.482172
+v -0.114861 0.002134 -0.491594
+v -0.112987 -0.095411 -0.482172
+v -0.107436 -0.189207 -0.454265
+v -0.098422 -0.275651 -0.408948
+v -0.086291 -0.351419 -0.347962
+v -0.071509 -0.413600 -0.273649
+v -0.054645 -0.459805 -0.188867
+v -0.036346 -0.488258 -0.096873
+v -0.017316 0.492527 -0.098747
+v -0.017316 0.464074 -0.192543
+v -0.017316 0.417869 -0.278987
+v -0.017316 0.355688 -0.354755
+v -0.017316 0.279919 -0.416937
+v -0.017316 0.193476 -0.463141
+v -0.017316 0.099680 -0.491594
+v -0.017316 0.002134 -0.501202
+v -0.017316 -0.095411 -0.491594
+v -0.017316 -0.189207 -0.463141
+v -0.017316 -0.275651 -0.416937
+v -0.017316 -0.351419 -0.354755
+v -0.017316 -0.413600 -0.278987
+v -0.017316 -0.459805 -0.192543
+v -0.017316 -0.488258 -0.098747
+v 0.001714 0.492527 -0.096873
+v 0.020013 0.464074 -0.188867
+v 0.036877 0.417869 -0.273649
+v 0.051659 0.355688 -0.347962
+v 0.063790 0.279919 -0.408948
+v 0.072804 0.193476 -0.454265
+v 0.078355 0.099680 -0.482171
+v 0.080229 0.002134 -0.491594
+v 0.078355 -0.095411 -0.482171
+v 0.072804 -0.189207 -0.454265
+v 0.063790 -0.275651 -0.408948
+v 0.051659 -0.351419 -0.347962
+v 0.036877 -0.413600 -0.273649
+v 0.020013 -0.459805 -0.188867
+v 0.001714 -0.488258 -0.096873
+v 0.020013 0.492527 -0.091322
+v 0.055907 0.464074 -0.177978
+v 0.088988 0.417869 -0.257842
+v 0.117983 0.355688 -0.327842
+v 0.141779 0.279919 -0.385291
+v 0.159461 0.193476 -0.427978
+v 0.170349 0.099680 -0.454265
+v 0.174026 0.002134 -0.463141
+v 0.170349 -0.095411 -0.454265
+v 0.159461 -0.189207 -0.427978
+v 0.141779 -0.275651 -0.385291
+v 0.117983 -0.351419 -0.327842
+v 0.088988 -0.413600 -0.257842
+v 0.055907 -0.459805 -0.177978
+v 0.020013 -0.488258 -0.091322
+v 0.036877 0.492527 -0.082308
+v 0.088988 0.464074 -0.160297
+v 0.137013 0.417869 -0.232172
+v 0.179108 0.355688 -0.295171
+v 0.213654 0.279919 -0.346873
+v 0.239324 0.193476 -0.385291
+v 0.255131 0.099680 -0.408948
+v 0.260469 0.002134 -0.416936
+v 0.255131 -0.095411 -0.408948
+v 0.239324 -0.189207 -0.385291
+v 0.213654 -0.275651 -0.346873
+v 0.179108 -0.351419 -0.295171
+v 0.137013 -0.413600 -0.232172
+v 0.088988 -0.459805 -0.160296
+v 0.036877 -0.488258 -0.082307
+v 0.051659 0.492527 -0.070177
+v 0.117983 0.464074 -0.136501
+v 0.179108 0.417869 -0.197625
+v 0.232684 0.355688 -0.251202
+v 0.276653 0.279919 -0.295171
+v 0.309325 0.193476 -0.327842
+v 0.329444 0.099680 -0.347962
+v 0.336237 0.002134 -0.354755
+v 0.329444 -0.095411 -0.347962
+v 0.309325 -0.189207 -0.327842
+v 0.276653 -0.275651 -0.295171
+v 0.232684 -0.351419 -0.251202
+v 0.179108 -0.413600 -0.197625
+v 0.117983 -0.459805 -0.136501
+v 0.051659 -0.488258 -0.070176
+v 0.063790 0.492527 -0.055395
+v 0.141779 0.464074 -0.107505
+v 0.213654 0.417869 -0.155531
+v 0.276653 0.355688 -0.197625
+v 0.328355 0.279919 -0.232171
+v 0.366773 0.193476 -0.257842
+v 0.390430 0.099680 -0.273649
+v 0.398419 0.002134 -0.278987
+v 0.390430 -0.095411 -0.273649
+v 0.366773 -0.189207 -0.257842
+v 0.328355 -0.275651 -0.232171
+v 0.276653 -0.351419 -0.197625
+v 0.213654 -0.413600 -0.155531
+v 0.141779 -0.459805 -0.107505
+v 0.063790 -0.488258 -0.055395
+v 0.072804 0.492527 -0.038531
+v 0.159461 0.464074 -0.074425
+v 0.239324 0.417869 -0.107505
+v 0.309325 0.355688 -0.136501
+v 0.366773 0.279919 -0.160296
+v 0.409461 0.193476 -0.177978
+v 0.435748 0.099680 -0.188867
+v 0.444624 0.002134 -0.192543
+v 0.435748 -0.095411 -0.188867
+v 0.409461 -0.189207 -0.177978
+v 0.366773 -0.275651 -0.160296
+v 0.309325 -0.351419 -0.136501
+v 0.239324 -0.413600 -0.107505
+v 0.159461 -0.459805 -0.074425
+v 0.072804 -0.488258 -0.038531
+v 0.078355 0.492527 -0.020232
+v 0.170349 0.464074 -0.038531
+v 0.255132 0.417869 -0.055395
+v 0.329444 0.355688 -0.070176
+v 0.390430 0.279919 -0.082307
+v 0.435748 0.193476 -0.091322
+v 0.463654 0.099680 -0.096872
+v 0.473077 0.002134 -0.098747
+v 0.463654 -0.095411 -0.096872
+v 0.435748 -0.189207 -0.091322
+v 0.390430 -0.275651 -0.082307
+v 0.329444 -0.351419 -0.070176
+v 0.255131 -0.413600 -0.055395
+v 0.170349 -0.459805 -0.038531
+v 0.078355 -0.488258 -0.020232
+v 0.080229 0.492527 -0.001202
+v 0.174026 0.464074 -0.001202
+v 0.260469 0.417869 -0.001202
+v 0.336237 0.355688 -0.001202
+v 0.398419 0.279919 -0.001202
+v 0.444624 0.193476 -0.001202
+v 0.473076 0.099680 -0.001202
+v 0.482684 0.002134 -0.001202
+v 0.473076 -0.095411 -0.001202
+v 0.444624 -0.189207 -0.001202
+v 0.398419 -0.275651 -0.001202
+v 0.336237 -0.351419 -0.001202
+v 0.260469 -0.413600 -0.001202
+v 0.174026 -0.459805 -0.001202
+v 0.080229 -0.488258 -0.001202
+v 0.078355 0.492527 0.017829
+v 0.170349 0.464074 0.036127
+v 0.255132 0.417869 0.052992
+v 0.329444 0.355688 0.067773
+v 0.390430 0.279919 0.079904
+v 0.435748 0.193476 0.088918
+v 0.463654 0.099680 0.094469
+v 0.473077 0.002134 0.096344
+v 0.463654 -0.095411 0.094469
+v 0.435748 -0.189207 0.088918
+v 0.390430 -0.275651 0.079904
+v 0.329444 -0.351419 0.067773
+v 0.255131 -0.413600 0.052992
+v 0.170349 -0.459805 0.036127
+v 0.078355 -0.488258 0.017828
+v 0.072804 0.492527 0.036127
+v 0.159461 0.464074 0.072022
+v 0.239324 0.417869 0.105102
+v 0.309325 0.355688 0.134097
+v 0.366773 0.279919 0.157893
+v 0.409461 0.193476 0.175575
+v 0.435747 0.099680 0.186464
+v 0.444624 0.002134 0.190140
+v 0.435747 -0.095411 0.186464
+v 0.409461 -0.189207 0.175575
+v 0.366773 -0.275651 0.157893
+v 0.309325 -0.351419 0.134097
+v 0.239324 -0.413600 0.105102
+v 0.159461 -0.459805 0.072022
+v 0.072804 -0.488258 0.036127
+v 0.063790 0.492527 0.052992
+v 0.141779 0.464074 0.105102
+v 0.213654 0.417869 0.153128
+v 0.276653 0.355688 0.195222
+v 0.328355 0.279919 0.229768
+v 0.366773 0.193476 0.255438
+v 0.390430 0.099680 0.271246
+v 0.398419 0.002134 0.276584
+v 0.390430 -0.095411 0.271246
+v 0.366773 -0.189207 0.255438
+v 0.328355 -0.275651 0.229768
+v 0.276653 -0.351419 0.195222
+v 0.213654 -0.413600 0.153127
+v 0.141779 -0.459805 0.105102
+v 0.063790 -0.488258 0.052992
+v 0.051659 0.492527 0.067773
+v 0.117983 0.464074 0.134097
+v 0.179108 0.417869 0.195222
+v 0.232684 0.355688 0.248798
+v 0.276653 0.279919 0.292767
+v 0.309325 0.193476 0.325439
+v 0.329444 0.099680 0.345558
+v 0.336237 0.002134 0.352352
+v 0.329444 -0.095411 0.345558
+v 0.309325 -0.189207 0.325439
+v 0.276653 -0.275651 0.292767
+v 0.232684 -0.351419 0.248798
+v 0.179108 -0.413600 0.195222
+v 0.117983 -0.459805 0.134097
+v 0.051659 -0.488258 0.067773
+v 0.036877 0.492527 0.079904
+v 0.088988 0.464074 0.157893
+v 0.137013 0.417869 0.229768
+v 0.179108 0.355688 0.292767
+v 0.213654 0.279919 0.344469
+v 0.239324 0.193476 0.382887
+v 0.255131 0.099680 0.406545
+v 0.260469 0.002134 0.414533
+v 0.255131 -0.095411 0.406545
+v 0.239324 -0.189207 0.382887
+v 0.213654 -0.275651 0.344469
+v 0.179108 -0.351419 0.292767
+v 0.137013 -0.413600 0.229768
+v 0.088988 -0.459805 0.157893
+v 0.036877 -0.488258 0.079904
+v -0.017316 -0.497866 -0.001202
+v 0.020013 0.492527 0.088918
+v 0.055907 0.464074 0.175575
+v 0.088988 0.417869 0.255438
+v 0.117983 0.355688 0.325439
+v 0.141779 0.279919 0.382887
+v 0.159461 0.193476 0.425575
+v 0.170349 0.099680 0.451862
+v 0.174026 0.002134 0.460738
+v 0.170349 -0.095411 0.451862
+v 0.159461 -0.189207 0.425575
+v 0.141779 -0.275651 0.382887
+v 0.117983 -0.351419 0.325439
+v 0.088988 -0.413600 0.255438
+v 0.055907 -0.459805 0.175575
+v 0.020013 -0.488258 0.088918
+v 0.001714 0.492527 0.094469
+v 0.020013 0.464074 0.186463
+v 0.036877 0.417869 0.271246
+v 0.051659 0.355688 0.345558
+v 0.063790 0.279919 0.406545
+v 0.072804 0.193476 0.451862
+v 0.078355 0.099680 0.479768
+v 0.080229 0.002134 0.489191
+v 0.078355 -0.095411 0.479768
+v 0.072804 -0.189207 0.451862
+v 0.063790 -0.275651 0.406545
+v 0.051659 -0.351419 0.345558
+v 0.036877 -0.413600 0.271246
+v 0.020013 -0.459805 0.186463
+v 0.001714 -0.488258 0.094469
+v -0.017316 0.492527 0.096344
+v -0.017316 0.464074 0.190140
+v -0.017316 0.417869 0.276583
+v -0.017316 0.355688 0.352352
+v -0.017316 0.279919 0.414533
+v -0.017316 0.193476 0.460738
+v -0.017316 0.099680 0.489191
+v -0.017316 0.002134 0.498798
+v -0.017316 -0.095411 0.489191
+v -0.017316 -0.189207 0.460738
+v -0.017316 -0.275651 0.414533
+v -0.017316 -0.351419 0.352352
+v -0.017316 -0.413600 0.276583
+v -0.017316 -0.459805 0.190140
+v -0.017316 -0.488258 0.096343
+v -0.036346 0.492527 0.094469
+v -0.054645 0.464074 0.186463
+v -0.071509 0.417869 0.271246
+v -0.086291 0.355688 0.345558
+v -0.098422 0.279919 0.406545
+v -0.107436 0.193476 0.451862
+v -0.112987 0.099680 0.479768
+v -0.114861 0.002134 0.489191
+v -0.112987 -0.095411 0.479768
+v -0.107436 -0.189207 0.451862
+v -0.098422 -0.275651 0.406545
+v -0.086291 -0.351419 0.345558
+v -0.071509 -0.413600 0.271246
+v -0.054645 -0.459805 0.186463
+v -0.036346 -0.488258 0.094469
+v -0.054645 0.492527 0.088918
+v -0.090539 0.464074 0.175575
+v -0.123620 0.417869 0.255438
+v -0.152615 0.355688 0.325439
+v -0.176411 0.279919 0.382887
+v -0.194093 0.193476 0.425575
+v -0.204981 0.099680 0.451862
+v -0.208658 0.002134 0.460738
+v -0.204981 -0.095411 0.451862
+v -0.194093 -0.189207 0.425575
+v -0.176411 -0.275651 0.382887
+v -0.152615 -0.351419 0.325439
+v -0.123620 -0.413600 0.255438
+v -0.090539 -0.459805 0.175575
+v -0.054645 -0.488258 0.088918
+v -0.071509 0.492527 0.079904
+v -0.123620 0.464074 0.157893
+v -0.171645 0.417869 0.229768
+v -0.213740 0.355688 0.292767
+v -0.248286 0.279919 0.344469
+v -0.273956 0.193476 0.382887
+v -0.289763 0.099680 0.406545
+v -0.295101 0.002134 0.414533
+v -0.289763 -0.095411 0.406545
+v -0.273956 -0.189207 0.382887
+v -0.248286 -0.275651 0.344469
+v -0.213740 -0.351419 0.292767
+v -0.171645 -0.413600 0.229768
+v -0.123620 -0.459805 0.157893
+v -0.071509 -0.488258 0.079904
+v -0.086291 0.492527 0.067773
+v -0.152615 0.464074 0.134097
+v -0.213740 0.417869 0.195222
+v -0.267316 0.355688 0.248798
+v -0.311285 0.279919 0.292767
+v -0.343957 0.193476 0.325439
+v -0.364076 0.099680 0.345558
+v -0.370869 0.002134 0.352352
+v -0.364076 -0.095411 0.345558
+v -0.343957 -0.189207 0.325439
+v -0.311285 -0.275651 0.292767
+v -0.267316 -0.351419 0.248798
+v -0.213740 -0.413600 0.195222
+v -0.152615 -0.459805 0.134097
+v -0.086291 -0.488258 0.067773
+v -0.098422 0.492527 0.052992
+v -0.176411 0.464074 0.105102
+v -0.248286 0.417869 0.153127
+v -0.311285 0.355688 0.195222
+v -0.362987 0.279919 0.229768
+v -0.401405 0.193476 0.255438
+v -0.425062 0.099680 0.271246
+v -0.433051 0.002134 0.276583
+v -0.425062 -0.095411 0.271246
+v -0.401405 -0.189207 0.255438
+v -0.362987 -0.275651 0.229768
+v -0.311285 -0.351419 0.195222
+v -0.248286 -0.413600 0.153127
+v -0.176411 -0.459805 0.105102
+v -0.098422 -0.488258 0.052992
+v -0.017316 0.502134 -0.001202
+v -0.107436 0.492527 0.036127
+v -0.194093 0.464074 0.072022
+v -0.273956 0.417869 0.105102
+v -0.343957 0.355688 0.134097
+v -0.401405 0.279919 0.157893
+v -0.444093 0.193476 0.175575
+v -0.470379 0.099680 0.186463
+v -0.479255 0.002134 0.190140
+v -0.470379 -0.095411 0.186463
+v -0.444093 -0.189207 0.175575
+v -0.401405 -0.275651 0.157893
+v -0.343957 -0.351419 0.134097
+v -0.273956 -0.413600 0.105102
+v -0.194093 -0.459805 0.072022
+v -0.107436 -0.488258 0.036127
+v -0.112987 0.492527 0.017828
+v -0.204981 0.464074 0.036127
+v -0.289763 0.417869 0.052991
+v -0.364076 0.355688 0.067773
+v -0.425062 0.279919 0.079904
+v -0.470380 0.193476 0.088918
+v -0.498285 0.099680 0.094469
+v -0.507708 0.002134 0.096343
+v -0.498285 -0.095411 0.094469
+v -0.470380 -0.189207 0.088918
+v -0.425062 -0.275651 0.079904
+v -0.364076 -0.351419 0.067773
+v -0.289763 -0.413600 0.052991
+v -0.204981 -0.459805 0.036127
+v -0.112987 -0.488258 0.017828
+vt 0.586802 0.532921
+vt 0.576820 0.575078
+vt 0.543690 0.553854
+vt 0.558627 0.514871
+vt 0.563996 0.624009
+vt 0.548822 0.677835
+vt 0.509743 0.652799
+vt 0.527184 0.600426
+vt 0.531882 0.734486
+vt 0.513827 0.791785
+vt 0.474747 0.766750
+vt 0.492037 0.708960
+vt 0.495350 0.847531
+vt 0.477161 0.899581
+vt 0.444031 0.878357
+vt 0.458538 0.823948
+vt 0.459960 0.945936
+vt 0.444408 0.984812
+vt 0.422271 0.970631
+vt 0.431785 0.927886
+vt 0.785658 0.829096
+vt 0.744146 0.818344
+vt 0.747401 0.795647
+vt 0.787317 0.817525
+vt 0.921632 0.150742
+vt 0.880121 0.151334
+vt 0.878462 0.139763
+vt 0.918378 0.128044
+vt 0.705890 0.804868
+vt 0.672357 0.789185
+vt 0.678370 0.747245
+vt 0.710614 0.771916
+vt 0.959889 0.153763
+vt 0.955164 0.120810
+vt 0.422110 0.801871
+vt 0.436076 0.743313
+vt 0.403905 0.910989
+vt 0.411247 0.858488
+vt 0.911994 0.106663
+vt 0.945897 0.089770
+vt 0.719881 0.740876
+vt 0.753784 0.774266
+vt 0.510906 0.533985
+vt 0.530746 0.497974
+vt 0.471071 0.629362
+vt 0.490756 0.578349
+vt 0.452608 0.685064
+vt 0.400365 0.957355
+vt 0.790571 0.806625
+vt 0.875208 0.128863
+vt 0.690165 0.707739
+vt 0.387466 0.782147
+vt 0.399298 0.722374
+vt 0.377390 0.895893
+vt 0.380068 0.840738
+vt 0.902727 0.087421
+vt 0.932443 0.061835
+vt 0.733335 0.712940
+vt 0.763051 0.755023
+vt 0.665778 0.676127
+vt 0.685914 0.634319
+vt 0.707289 0.672183
+vt 0.434293 0.608424
+vt 0.456112 0.558626
+vt 0.415110 0.663715
+vt 0.379532 0.945494
+vt 0.795295 0.796815
+vt 0.870483 0.119054
+vt 0.479727 0.516234
+vt 0.355938 0.765536
+vt 0.365828 0.704739
+vt 0.353259 0.883179
+vt 0.351694 0.825787
+vt 0.890932 0.071056
+vt 0.915319 0.038078
+vt 0.750459 0.689183
+vt 0.774846 0.738659
+vt 0.711541 0.598764
+vt 0.729084 0.641946
+vt 0.400823 0.590789
+vt 0.424584 0.542014
+vt 0.380984 0.645734
+vt 0.958489 0.043280
+vt 0.936695 0.013043
+vt 0.801308 0.788473
+vt 0.864470 0.110711
+vt 0.451352 0.501284
+vt 0.000000 0.230779
+vt 0.012533 0.170911
+vt 0.040381 0.186029
+vt 0.029563 0.246828
+vt 0.031050 0.113931
+vt 0.054840 0.062028
+vt 0.076154 0.073599
+vt 0.056113 0.127536
+vt 0.877062 0.058199
+vt 0.895184 0.019412
+vt 0.770595 0.670517
+vt 0.788716 0.725802
+vt 0.741676 0.570828
+vt 0.754711 0.618189
+vt 0.371946 0.577135
+vt 0.397382 0.529152
+vt 0.336951 0.691085
+vt 0.351541 0.631813
+vt 0.082988 0.017198
+vt 0.099734 0.026289
+vt 0.808379 0.781918
+vt 0.857399 0.104157
+vt 0.698506 0.561295
+vt 0.731990 0.530255
+vt 0.073348 0.198961
+vt 0.064560 0.260557
+vt 0.101385 0.083497
+vt 0.085782 0.139175
+vt 0.861651 0.049343
+vt 0.872810 0.006555
+vt 0.792968 0.657660
+vt 0.804127 0.716946
+vt 0.775160 0.551586
+vt 0.783187 0.601825
+vt 0.029717 0.419799
+vt 0.024127 0.366003
+vt 0.059124 0.379732
+vt 0.062684 0.432731
+vt 0.024075 0.307598
+vt 0.059758 0.321596
+vt 0.119558 0.034066
+vt 0.816235 0.777404
+vt 0.849543 0.099642
+vt 0.769195 0.508874
+vt 0.110166 0.209212
+vt 0.103647 0.271439
+vt 0.129565 0.091343
+vt 0.118918 0.148401
+vt 0.845291 0.044828
+vt 0.849059 0.000000
+vt 0.816719 0.651105
+vt 0.820487 0.712431
+vt 0.810706 0.541776
+vt 0.813416 0.593482
+vt 0.098210 0.390614
+vt 0.099502 0.442982
+vt 0.099609 0.332692
+vt 0.141699 0.040230
+vt 0.824576 0.775102
+vt 0.841203 0.097340
+vt 0.808692 0.497974
+vt 0.149421 0.216387
+vt 0.145320 0.279057
+vt 0.159610 0.096834
+vt 0.154246 0.154858
+vt 0.828610 0.044828
+vt 0.824842 0.000000
+vt 0.840936 0.651105
+vt 0.837168 0.712431
+vt 0.846949 0.541776
+vt 0.844238 0.593482
+vt 0.139883 0.398232
+vt 0.138758 0.450157
+vt 0.142099 0.340458
+vt 0.165305 0.044545
+vt 0.833079 0.775102
+vt 0.832699 0.097340
+vt 0.848963 0.497974
+vt 0.189605 0.220211
+vt 0.187978 0.283116
+vt 0.190365 0.099761
+vt 0.190411 0.158300
+vt 0.171113 0.000000
+vt 0.187757 0.001584
+vt 0.189469 0.046844
+vt 0.864687 0.657660
+vt 0.853528 0.716946
+vt 0.882495 0.551586
+vt 0.874468 0.601825
+vt 0.182542 0.402291
+vt 0.178941 0.453981
+vt 0.185594 0.344597
+vt 0.841420 0.777404
+vt 0.824358 0.099642
+vt 0.812250 0.049343
+vt 0.138765 0.494240
+vt 0.174929 0.497681
+vt 0.229173 0.220536
+vt 0.229983 0.283461
+vt 0.220649 0.100010
+vt 0.226021 0.158592
+vt 0.801091 0.006555
+vt 0.796839 0.058199
+vt 0.778718 0.019412
+vt 0.887060 0.670517
+vt 0.868939 0.725802
+vt 0.915979 0.570828
+vt 0.902944 0.618189
+vt 0.224547 0.402636
+vt 0.218509 0.454306
+vt 0.228421 0.344949
+vt 0.213263 0.047040
+vt 0.849276 0.781918
+vt 0.816502 0.104157
+vt 0.210539 0.497974
+vt 0.266604 0.217351
+vt 0.269720 0.280079
+vt 0.249297 0.097572
+vt 0.259708 0.155725
+vt 0.782970 0.071056
+vt 0.758582 0.038078
+vt 0.907196 0.689183
+vt 0.882809 0.738659
+vt 0.946114 0.598764
+vt 0.928571 0.641946
+vt 0.264284 0.399254
+vt 0.255940 0.451121
+vt 0.268937 0.341501
+vt 0.235772 0.045124
+vt 0.856347 0.788473
+vt 0.809431 0.110711
+vt 0.925665 0.530255
+vt 0.959149 0.561295
+vt 0.300461 0.210777
+vt 0.305662 0.273100
+vt 0.275210 0.092540
+vt 0.290178 0.149809
+vt 0.771175 0.087421
+vt 0.741459 0.061835
+vt 0.924320 0.712940
+vt 0.894604 0.755023
+vt 0.971741 0.634319
+vt 0.950366 0.672183
+vt 0.300226 0.392275
+vt 0.289797 0.444547
+vt 0.305583 0.334385
+vt 0.256131 0.041171
+vt 0.862360 0.796815
+vt 0.803418 0.119054
+vt 0.987625 0.600802
+vt 0.329441 0.201067
+vt 0.336427 0.262792
+vt 0.297391 0.085108
+vt 0.316260 0.141070
+vt 0.761907 0.106663
+vt 0.728005 0.089770
+vt 0.937774 0.740876
+vt 0.903871 0.774266
+vt 0.991877 0.676127
+vt 0.967490 0.707738
+vt 0.282103 0.559315
+vt 0.308199 0.609448
+vt 0.281809 0.631199
+vt 0.257244 0.579804
+vt 0.336951 0.323875
+vt 0.273559 0.035332
+vt 0.867084 0.806625
+vt 0.798694 0.128863
+vt 0.251541 0.516380
+vt 0.229168 0.534820
+vt 0.328827 0.664852
+vt 0.301920 0.687030
+vt 0.316805 0.745150
+vt 0.325890 0.803326
+vt 0.296202 0.825931
+vt 0.285288 0.769147
+vt 0.314987 0.075562
+vt 0.336951 0.129845
+vt 0.755524 0.128044
+vt 0.718737 0.120811
+vt 0.947041 0.771916
+vt 0.910254 0.795646
+vt 0.198662 0.497974
+vt 0.202450 0.555164
+vt 0.175940 0.515275
+vt 0.250293 0.655197
+vt 0.227556 0.602409
+vt 0.269787 0.711497
+vt 0.328827 0.859323
+vt 0.302109 0.879667
+vt 0.698289 0.078835
+vt 0.686494 0.118342
+vt 0.870339 0.817525
+vt 0.795440 0.139763
+vt 0.979285 0.747245
+vt 0.262826 0.849783
+vt 0.249857 0.794468
+vt 0.302782 0.928290
+vt 0.277237 0.946545
+vt 0.272071 0.901133
+vt 0.752270 0.150742
+vt 0.714013 0.153763
+vt 0.951765 0.804868
+vt 0.913509 0.818344
+vt 0.172413 0.576629
+vt 0.150395 0.533530
+vt 0.214861 0.680517
+vt 0.194180 0.626261
+vt 0.233661 0.737314
+vt 0.680481 0.160282
+vt 0.871998 0.829096
+vt 0.793781 0.151334
+vt 0.985298 0.789185
+vt 0.227045 0.873965
+vt 0.211871 0.820139
+vt 0.249851 0.965053
+vt 0.239869 0.922896
+vt 0.752270 0.173884
+vt 0.714013 0.187360
+vt 0.951765 0.838466
+vt 0.913509 0.841487
+vt 0.140210 0.598393
+vt 0.123009 0.552038
+vt 0.176876 0.706189
+vt 0.158399 0.650443
+vt 0.194931 0.763488
+vt 0.680481 0.203043
+vt 0.871998 0.840894
+vt 0.793781 0.163132
+vt 0.985298 0.831947
+vt 0.190233 0.897548
+vt 0.172792 0.845175
+vt 0.221676 0.983103
+vt 0.206739 0.944120
+vt 0.755524 0.196582
+vt 0.718737 0.220312
+vt 0.947041 0.871418
+vt 0.910254 0.864185
+vt 0.107080 0.619617
+vt 0.094834 0.570088
+vt 0.137796 0.731224
+vt 0.121587 0.674026
+vt 0.155086 0.789014
+vt 0.686494 0.244983
+vt 0.870339 0.852465
+vt 0.795440 0.174704
+vt 0.107457 0.513162
+vt 0.085320 0.527343
+vt 0.153805 0.919625
+vt 0.134120 0.868612
+vt 0.193795 1.000000
+vt 0.173955 0.963989
+vt 0.761907 0.217963
+vt 0.728005 0.251353
+vt 0.937774 0.902458
+vt 0.903871 0.885565
+vt 0.074296 0.639486
+vt 0.066954 0.586985
+vt 0.099125 0.754661
+vt 0.085159 0.696103
+vt 0.115657 0.812911
+vt 0.698289 0.284490
+vt 0.867084 0.863365
+vt 0.798694 0.185603
+vt 0.063414 0.540619
+vt 0.119161 0.939349
+vt 0.097343 0.889550
+vt 0.673902 0.316101
+vt 0.715413 0.320045
+vt 0.694037 0.357910
+vt 0.771175 0.237205
+vt 0.741459 0.279288
+vt 0.924320 0.930393
+vt 0.894604 0.904808
+vt 0.043117 0.657236
+vt 0.040439 0.602081
+vt 0.062347 0.775600
+vt 0.050515 0.715827
+vt 0.078159 0.834259
+vt 0.142776 0.981740
+vt 0.862360 0.873175
+vt 0.803418 0.195413
+vt 0.042581 0.552480
+vt 0.087633 0.955960
+vt 0.063872 0.907185
+vt 0.737207 0.350282
+vt 0.719665 0.393465
+vt 0.782970 0.253569
+vt 0.758582 0.303045
+vt 0.907196 0.954151
+vt 0.882809 0.921172
+vt 0.014743 0.672187
+vt 0.016308 0.614795
+vt 0.028877 0.793235
+vt 0.018987 0.732438
+vt 0.044033 0.852240
+vt 0.114402 0.996690
+vt 0.856347 0.881517
+vt 0.809431 0.203755
+vt 0.950366 0.948949
+vt 0.928571 0.979186
+vt 0.060432 0.968822
+vt 0.034995 0.920839
+vt 0.762834 0.374039
+vt 0.749799 0.421400
+vt 0.796839 0.266426
+vt 0.778718 0.321711
+vt 0.887060 0.972816
+vt 0.868939 0.934029
+vt 0.391791 0.435945
+vt 0.368001 0.384043
+vt 0.393064 0.370438
+vt 0.413105 0.424375
+vt 0.349484 0.327063
+vt 0.336951 0.267195
+vt 0.366514 0.251146
+vt 0.377332 0.311945
+vt 0.014590 0.866161
+vt 0.706629 0.430933
+vt 0.740113 0.461974
+vt 0.849276 0.888072
+vt 0.816502 0.210310
+vt 0.419939 0.480776
+vt 0.436685 0.471685
+vt 0.000000 0.806889
+vt 0.361078 0.131971
+vt 0.366668 0.078175
+vt 0.399635 0.065243
+vt 0.396075 0.118242
+vt 0.791310 0.390403
+vt 0.783283 0.440642
+vt 0.812250 0.275283
+vt 0.801091 0.334568
+vt 0.864687 0.985674
+vt 0.853528 0.942885
+vt 0.422733 0.358799
+vt 0.438336 0.414477
+vt 0.401511 0.237417
+vt 0.410298 0.299013
+vt 0.361026 0.190376
+vt 0.396708 0.176378
+vt 0.777318 0.483354
+vt 0.841420 0.892587
+vt 0.824359 0.214825
+vt 0.456509 0.463908
+vt 0.436453 0.054992
+vt 0.435161 0.107360
+vt 0.821540 0.398746
+vt 0.818829 0.450452
+vt 0.828610 0.279797
+vt 0.824842 0.341123
+vt 0.840936 0.992228
+vt 0.837168 0.947400
+vt 0.455868 0.349573
+vt 0.466516 0.406631
+vt 0.440597 0.226535
+vt 0.447117 0.288762
+vt 0.436560 0.165282
+vt 0.816815 0.494254
+vt 0.833079 0.894888
+vt 0.832699 0.217127
+vt 0.478650 0.457744
+vt 0.475708 0.047817
+vt 0.476834 0.099743
+vt 0.852362 0.398746
+vt 0.855072 0.450452
+vt 0.845291 0.279797
+vt 0.849059 0.341123
+vt 0.816719 0.992228
+vt 0.820487 0.947400
+vt 0.491197 0.343116
+vt 0.496560 0.401140
+vt 0.482270 0.218917
+vt 0.486372 0.281587
+vt 0.479050 0.157516
+vt 0.857086 0.494254
+vt 0.824576 0.894888
+vt 0.841203 0.217127
+vt 0.502255 0.453429
+vt 0.515892 0.043993
+vt 0.519493 0.095683
+vt 0.882591 0.390403
+vt 0.890619 0.440642
+vt 0.861651 0.275283
+vt 0.872810 0.334568
+vt 0.508064 0.497974
+vt 0.526420 0.451130
+vt 0.524708 0.496390
+vt 0.527362 0.339674
+vt 0.527316 0.398213
+vt 0.524929 0.214858
+vt 0.526556 0.277763
+vt 0.522544 0.153377
+vt 0.475716 0.003734
+vt 0.511880 0.000293
+vt 0.804127 0.942885
+vt 0.816235 0.892587
+vt 0.849543 0.214825
+vt 0.555460 0.043668
+vt 0.561498 0.095338
+vt 0.911067 0.374039
+vt 0.924102 0.421400
+vt 0.877062 0.266426
+vt 0.895184 0.321711
+vt 0.792968 0.985674
+vt 0.770595 0.972816
+vt 0.788716 0.934029
+vt 0.562972 0.339382
+vt 0.557599 0.397964
+vt 0.566934 0.214513
+vt 0.566123 0.277438
+vt 0.565372 0.153025
+vt 0.547490 0.000000
+vt 0.808379 0.888072
+vt 0.857399 0.210310
+vt 0.550214 0.450934
+vt 0.592891 0.046853
+vt 0.601235 0.098720
+vt 0.936695 0.350282
+vt 0.954237 0.393464
+vt 0.890932 0.253569
+vt 0.915319 0.303045
+vt 0.750459 0.954151
+vt 0.774847 0.921172
+vt 0.596659 0.342249
+vt 0.586248 0.400402
+vt 0.606671 0.217895
+vt 0.603555 0.280623
+vt 0.605888 0.156473
+vt 0.933789 0.461973
+vt 0.967272 0.430933
+vt 0.801308 0.881517
+vt 0.864470 0.203755
+vt 0.572723 0.452850
+vt 0.626747 0.053427
+vt 0.637176 0.105699
+vt 0.958489 0.320045
+vt 0.979865 0.357909
+vt 0.902727 0.237205
+vt 0.932443 0.279288
+vt 0.733336 0.930394
+vt 0.763052 0.904808
+vt 0.627129 0.348165
+vt 0.612161 0.405434
+vt 0.642613 0.224874
+vt 0.637411 0.287197
+vt 0.642533 0.163589
+vt 0.995748 0.391427
+vt 0.795295 0.873175
+vt 0.870483 0.195413
+vt 0.593082 0.456803
+vt 0.645150 0.888526
+vt 0.619054 0.938659
+vt 0.594195 0.918170
+vt 0.618760 0.866775
+vt 0.975613 0.284490
+vt 1.000000 0.316101
+vt 0.911994 0.217963
+vt 0.945897 0.251353
+vt 0.719882 0.902458
+vt 0.753784 0.885566
+vt 0.653211 0.356904
+vt 0.634341 0.412865
+vt 0.673378 0.235182
+vt 0.666392 0.296907
+vt 0.665778 0.833122
+vt 0.638871 0.810944
+vt 0.588492 0.981594
+vt 0.566119 0.963154
+vt 0.790571 0.863365
+vt 0.875208 0.185603
+vt 0.610509 0.462642
+vt 0.673902 0.174099
+vt 0.564507 0.895565
+vt 0.587244 0.842777
+vt 0.535613 1.000000
+vt 0.512891 0.982699
+vt 0.539401 0.942810
+vt 0.918378 0.196582
+vt 0.955164 0.220312
+vt 0.710614 0.871418
+vt 0.747401 0.864185
+vt 0.673902 0.368129
+vt 0.651938 0.422412
+vt 0.662841 0.694648
+vt 0.653755 0.752824
+vt 0.622239 0.728827
+vt 0.633153 0.672043
+vt 0.606737 0.786477
+vt 0.987408 0.244983
+vt 0.787317 0.852465
+vt 0.878462 0.174703
+vt 0.690166 0.913394
+vt 0.678371 0.873887
+vt 0.665778 0.638651
+vt 0.639060 0.618307
+vt 0.531131 0.871713
+vt 0.551812 0.817457
+vt 0.487346 0.964444
+vt 0.509364 0.921345
+vt 0.921632 0.173884
+vt 0.959888 0.187360
+vt 0.705890 0.838466
+vt 0.744147 0.841487
+vt 0.639733 0.569684
+vt 0.609022 0.596841
+vt 0.614188 0.551429
+vt 0.586807 0.703506
+vt 0.599777 0.648191
+vt 0.570612 0.760660
+vt 0.993421 0.203043
+vt 0.785658 0.840894
+vt 0.880121 0.163132
+vt 0.672357 0.831947
+vt 0.836951 0.155518
+vt 0.828828 0.836710
+vt 0.993421 0.160281
+usemtl None
+s off
+f 12/1 11/2 26/3 27/4
+f 10/5 9/6 24/7 25/8
+f 8/9 7/10 22/11 23/12
+f 6/13 5/14 20/15 21/16
+f 4/17 3/18 18/19 19/20
+f 15/21 14/22 29/23 30/24
+f 2/25 1/26 16/27 17/28
+f 13/29 12/30 27/31 28/32
+f 11/2 10/5 25/8 26/3
+f 9/6 8/9 23/12 24/7
+f 7/10 6/13 21/16 22/11
+f 5/14 4/17 19/20 20/15
+f 3/33 2/25 17/28 18/34
+f 14/22 13/29 28/32 29/23
+f 22/11 21/16 36/35 37/36
+f 20/15 19/20 34/37 35/38
+f 18/34 17/28 32/39 33/40
+f 29/23 28/32 43/41 44/42
+f 27/4 26/3 41/43 42/44
+f 25/8 24/7 39/45 40/46
+f 23/12 22/11 37/36 38/47
+f 21/16 20/15 35/38 36/35
+f 19/20 18/19 33/48 34/37
+f 30/24 29/23 44/42 45/49
+f 17/28 16/27 31/50 32/39
+f 28/32 27/31 42/51 43/41
+f 26/3 25/8 40/46 41/43
+f 24/7 23/12 38/47 39/45
+f 37/36 36/35 51/52 52/53
+f 35/38 34/37 49/54 50/55
+f 33/40 32/39 47/56 48/57
+f 44/42 43/41 58/58 59/59
+f 42/51 41/60 56/61 57/62
+f 40/46 39/45 54/63 55/64
+f 38/47 37/36 52/53 53/65
+f 36/35 35/38 50/55 51/52
+f 34/37 33/48 48/66 49/54
+f 45/49 44/42 59/59 60/67
+f 32/39 31/50 46/68 47/56
+f 43/41 42/51 57/62 58/58
+f 41/43 40/46 55/64 56/69
+f 39/45 38/47 53/65 54/63
+f 52/53 51/52 66/70 67/71
+f 50/55 49/54 64/72 65/73
+f 48/57 47/56 62/74 63/75
+f 59/59 58/58 73/76 74/77
+f 57/62 56/61 71/78 72/79
+f 55/64 54/63 69/80 70/81
+f 53/65 52/53 67/71 68/82
+f 51/52 50/55 65/73 66/70
+f 49/83 48/57 63/75 64/84
+f 60/67 59/59 74/77 75/85
+f 47/56 46/68 61/86 62/74
+f 58/58 57/62 72/79 73/76
+f 56/69 55/64 70/81 71/87
+f 54/63 53/65 68/82 69/80
+f 67/88 66/89 81/90 82/91
+f 65/92 64/93 79/94 80/95
+f 63/75 62/74 77/96 78/97
+f 74/77 73/76 88/98 89/99
+f 72/79 71/78 86/100 87/101
+f 70/81 69/80 84/102 85/103
+f 68/82 67/71 82/104 83/105
+f 66/89 65/92 80/95 81/90
+f 64/93 63/106 78/107 79/94
+f 75/85 74/77 89/99 90/108
+f 62/74 61/86 76/109 77/96
+f 73/76 72/79 87/101 88/98
+f 71/78 70/110 85/111 86/100
+f 69/80 68/82 83/105 84/102
+f 82/91 81/90 96/112 97/113
+f 80/95 79/94 94/114 95/115
+f 78/97 77/96 92/116 93/117
+f 89/99 88/98 103/118 104/119
+f 87/101 86/100 101/120 102/121
+f 85/122 84/123 99/124 100/125
+f 83/126 82/91 97/113 98/127
+f 81/90 80/95 95/115 96/112
+f 79/94 78/107 93/128 94/114
+f 90/108 89/99 104/119 105/129
+f 77/96 76/109 91/130 92/116
+f 88/98 87/101 102/121 103/118
+f 86/100 85/111 100/131 101/120
+f 84/123 83/126 98/127 99/124
+f 97/113 96/112 111/132 112/133
+f 95/115 94/114 109/134 110/135
+f 93/117 92/116 107/136 108/137
+f 104/119 103/118 118/138 119/139
+f 102/121 101/120 116/140 117/141
+f 100/125 99/124 114/142 115/143
+f 98/127 97/113 112/133 113/144
+f 96/112 95/115 110/135 111/132
+f 94/114 93/128 108/145 109/134
+f 105/129 104/119 119/139 120/146
+f 92/116 91/130 106/147 107/136
+f 103/118 102/121 117/141 118/138
+f 101/120 100/131 115/148 116/140
+f 99/124 98/127 113/144 114/142
+f 112/133 111/132 126/149 127/150
+f 110/135 109/134 124/151 125/152
+f 108/137 107/136 122/153 123/154
+f 119/139 118/138 133/155 134/156
+f 117/141 116/140 131/157 132/158
+f 115/143 114/142 129/159 130/160
+f 113/144 112/133 127/150 128/161
+f 111/132 110/135 125/152 126/149
+f 109/134 108/145 123/162 124/151
+f 120/146 119/139 134/156 135/163
+f 107/136 106/147 121/164 122/153
+f 118/138 117/141 132/158 133/155
+f 116/140 115/148 130/165 131/157
+f 114/142 113/144 128/161 129/159
+f 127/150 126/149 141/166 142/167
+f 125/152 124/151 139/168 140/169
+f 123/162 122/170 137/171 138/172
+f 134/156 133/155 148/173 149/174
+f 132/158 131/157 146/175 147/176
+f 130/160 129/159 144/177 145/178
+f 128/161 127/150 142/167 143/179
+f 126/149 125/152 140/169 141/166
+f 124/151 123/162 138/172 139/168
+f 135/163 134/156 149/174 150/180
+f 122/153 121/164 136/181 137/182
+f 133/155 132/158 147/176 148/173
+f 131/183 130/160 145/178 146/184
+f 129/159 128/161 143/179 144/177
+f 142/167 141/166 156/185 157/186
+f 140/169 139/168 154/187 155/188
+f 138/189 137/182 152/190 153/191
+f 149/174 148/173 163/192 164/193
+f 147/176 146/175 161/194 162/195
+f 145/178 144/177 159/196 160/197
+f 143/179 142/167 157/186 158/198
+f 141/166 140/169 155/188 156/185
+f 139/168 138/172 153/199 154/187
+f 150/180 149/174 164/193 165/200
+f 137/182 136/181 151/201 152/190
+f 148/173 147/176 162/195 163/192
+f 146/184 145/178 160/197 161/202
+f 144/177 143/179 158/198 159/196
+f 157/186 156/185 171/203 172/204
+f 155/188 154/187 169/205 170/206
+f 153/191 152/190 167/207 168/208
+f 164/193 163/192 178/209 179/210
+f 162/195 161/194 176/211 177/212
+f 160/197 159/196 174/213 175/214
+f 158/198 157/186 172/204 173/215
+f 156/185 155/188 170/206 171/203
+f 154/187 153/199 168/216 169/205
+f 165/200 164/193 179/210 180/217
+f 152/190 151/201 166/218 167/207
+f 163/192 162/195 177/212 178/209
+f 161/194 160/219 175/220 176/211
+f 159/196 158/198 173/215 174/213
+f 172/204 171/203 186/221 187/222
+f 170/206 169/205 184/223 185/224
+f 168/208 167/207 182/225 183/226
+f 179/210 178/209 193/227 194/228
+f 177/212 176/211 191/229 192/230
+f 175/214 174/213 189/231 190/232
+f 173/215 172/204 187/222 188/233
+f 171/203 170/206 185/224 186/221
+f 169/205 168/216 183/234 184/223
+f 180/217 179/210 194/228 195/235
+f 167/207 166/218 181/236 182/225
+f 178/209 177/212 192/230 193/227
+f 176/211 175/220 190/237 191/229
+f 174/213 173/215 188/233 189/231
+f 187/222 186/221 201/238 202/239
+f 185/224 184/223 199/240 200/241
+f 183/226 182/225 197/242 198/243
+f 194/228 193/227 208/244 209/245
+f 192/230 191/229 206/246 207/247
+f 190/248 189/249 204/250 205/251
+f 188/233 187/222 202/239 203/252
+f 186/221 185/224 200/241 201/238
+f 184/223 183/234 198/253 199/240
+f 195/235 194/228 209/245 210/254
+f 182/225 181/236 196/255 197/242
+f 193/227 192/230 207/247 208/244
+f 191/256 190/248 205/251 206/257
+f 189/249 188/258 203/259 204/250
+f 202/260 201/261 216/262 217/263
+f 200/241 199/240 214/264 215/265
+f 198/243 197/242 212/266 213/267
+f 209/245 208/244 223/268 224/269
+f 207/270 206/257 221/271 222/272
+f 205/251 204/250 219/273 220/274
+f 203/259 202/260 217/263 218/275
+f 201/261 200/276 215/277 216/262
+f 199/278 198/243 213/267 214/279
+f 210/254 209/245 224/269 225/280
+f 197/242 196/255 211/281 212/266
+f 208/244 207/247 222/282 223/268
+f 206/257 205/251 220/274 221/271
+f 204/250 203/259 218/275 219/273
+f 217/263 216/262 231/283 232/284
+f 215/277 214/285 229/286 230/287
+f 213/267 212/266 227/288 228/289
+f 224/269 223/268 238/290 239/291
+f 222/272 221/271 236/292 237/293
+f 220/274 219/273 234/294 235/295
+f 218/275 217/263 232/284 233/296
+f 216/262 215/277 230/287 231/283
+f 214/279 213/267 228/289 229/297
+f 225/280 224/269 239/291 240/298
+f 212/266 211/281 226/299 227/288
+f 223/268 222/282 237/300 238/290
+f 221/271 220/274 235/295 236/292
+f 219/273 218/275 233/296 234/294
+f 232/284 231/283 246/301 247/302
+f 230/287 229/286 244/303 245/304
+f 228/289 227/288 242/305 243/306
+f 239/291 238/290 253/307 254/308
+f 237/293 236/292 251/309 252/310
+f 235/295 234/294 249/311 250/312
+f 233/296 232/284 247/302 248/313
+f 231/283 230/287 245/304 246/301
+f 229/297 228/289 243/306 244/314
+f 240/298 239/291 254/308 255/315
+f 227/288 226/299 241/316 242/305
+f 238/290 237/300 252/317 253/307
+f 236/292 235/295 250/312 251/309
+f 234/294 233/296 248/313 249/311
+f 247/302 246/301 261/318 262/319
+f 245/304 244/303 259/320 260/321
+f 243/306 242/305 257/322 258/323
+f 254/308 253/307 268/324 269/325
+f 252/310 251/309 266/326 267/327
+f 250/312 249/311 264/328 265/329
+f 248/313 247/302 262/319 263/330
+f 246/301 245/304 260/321 261/318
+f 244/314 243/306 258/323 259/331
+f 255/315 254/308 269/325 270/332
+f 242/305 241/316 256/333 257/322
+f 253/334 252/310 267/327 268/335
+f 251/309 250/312 265/329 266/326
+f 249/311 248/313 263/330 264/328
+f 262/319 261/318 276/336 277/337
+f 260/321 259/320 274/338 275/339
+f 258/323 257/322 272/340 273/341
+f 269/325 268/324 283/342 284/343
+f 267/327 266/326 281/344 282/345
+f 265/329 264/328 279/346 280/347
+f 263/330 262/319 277/337 278/348
+f 261/318 260/321 275/339 276/336
+f 259/331 258/323 273/341 274/349
+f 270/332 269/325 284/343 285/350
+f 257/322 256/333 271/351 272/340
+f 268/335 267/327 282/345 283/352
+f 266/326 265/329 280/347 281/344
+f 264/328 263/330 278/348 279/346
+f 277/337 276/336 291/353 292/354
+f 275/355 274/349 289/356 290/357
+f 273/341 272/340 287/358 288/359
+f 284/343 283/342 298/360 299/361
+f 282/345 281/344 296/362 297/363
+f 280/347 279/346 294/364 295/365
+f 278/348 277/337 292/354 293/366
+f 276/336 275/339 290/367 291/353
+f 274/349 273/341 288/359 289/356
+f 285/350 284/343 299/361 300/368
+f 272/340 271/351 286/369 287/358
+f 283/352 282/345 297/363 298/370
+f 281/344 280/347 295/365 296/362
+f 279/346 278/348 293/366 294/364
+f 292/354 291/353 306/371 307/372
+f 290/357 289/356 304/373 305/374
+f 288/359 287/358 302/375 303/376
+f 299/361 298/360 313/377 314/378
+f 297/363 296/362 311/379 312/380
+f 295/365 294/364 309/381 310/382
+f 293/366 292/354 307/372 308/383
+f 291/353 290/367 305/384 306/371
+f 289/356 288/359 303/376 304/373
+f 300/368 299/361 314/378 315/385
+f 287/358 286/369 301/386 302/375
+f 298/360 297/387 312/388 313/377
+f 296/362 295/365 310/382 311/379
+f 294/364 293/366 308/383 309/381
+f 307/372 306/371 321/389 322/390
+f 305/374 304/373 319/391 320/392
+f 303/376 302/375 317/393 318/394
+f 314/378 313/377 328/395 329/396
+f 312/397 311/398 326/399 327/400
+f 310/401 309/402 324/403 325/404
+f 308/383 307/372 322/390 323/405
+f 306/406 305/374 320/392 321/407
+f 304/373 303/376 318/394 319/391
+f 315/385 314/378 329/396 330/408
+f 302/375 301/386 316/409 317/393
+f 313/410 312/397 327/400 328/411
+f 311/398 310/401 325/404 326/399
+f 309/381 308/383 323/405 324/412
+f 322/413 321/414 337/415 338/416
+f 320/392 319/391 335/417 336/418
+f 318/394 317/393 333/419 334/420
+f 329/396 328/395 344/421 345/422
+f 327/400 326/399 342/423 343/424
+f 325/404 324/403 340/425 341/426
+f 323/427 322/413 338/416 339/428
+f 321/407 320/392 336/418 337/429
+f 319/391 318/394 334/420 335/417
+f 330/408 329/396 345/422 346/430
+f 317/393 316/409 332/431 333/419
+f 328/411 327/400 343/424 344/432
+f 326/399 325/404 341/426 342/423
+f 324/403 323/427 339/428 340/425
+f 338/416 337/415 352/433 353/434
+f 336/418 335/417 350/435 351/436
+f 334/420 333/419 348/437 349/438
+f 345/422 344/421 359/439 360/440
+f 343/424 342/423 357/441 358/442
+f 341/426 340/425 355/443 356/444
+f 339/428 338/416 353/434 354/445
+f 337/429 336/418 351/436 352/446
+f 335/417 334/420 349/438 350/435
+f 346/430 345/422 360/440 361/447
+f 333/419 332/431 347/448 348/437
+f 344/432 343/424 358/442 359/449
+f 342/423 341/426 356/444 357/441
+f 340/425 339/428 354/445 355/443
+f 353/434 352/433 367/450 368/451
+f 351/436 350/435 365/452 366/453
+f 349/438 348/437 363/454 364/455
+f 360/440 359/439 374/456 375/457
+f 358/442 357/441 372/458 373/459
+f 356/444 355/443 370/460 371/461
+f 354/445 353/434 368/451 369/462
+f 352/446 351/436 366/453 367/463
+f 350/435 349/438 364/455 365/452
+f 361/447 360/440 375/457 376/464
+f 348/437 347/448 362/465 363/454
+f 359/449 358/442 373/459 374/466
+f 357/441 356/444 371/461 372/458
+f 355/443 354/445 369/462 370/460
+f 368/451 367/450 382/467 383/468
+f 366/453 365/452 380/469 381/470
+f 364/455 363/454 378/471 379/472
+f 375/473 374/466 389/474 390/475
+f 373/459 372/458 387/476 388/477
+f 371/461 370/460 385/478 386/479
+f 369/462 368/451 383/468 384/480
+f 367/450 366/481 381/482 382/467
+f 365/452 364/455 379/472 380/469
+f 376/464 375/457 390/483 391/484
+f 363/454 362/465 377/485 378/471
+f 374/466 373/459 388/477 389/474
+f 372/458 371/461 386/479 387/476
+f 370/460 369/462 384/480 385/478
+f 383/468 382/467 397/486 398/487
+f 381/470 380/469 395/488 396/489
+f 379/472 378/471 393/490 394/491
+f 390/483 389/492 404/493 405/494
+f 388/477 387/476 402/495 403/496
+f 386/479 385/478 400/497 401/498
+f 384/480 383/468 398/487 399/499
+f 382/467 381/482 396/500 397/486
+f 380/469 379/472 394/491 395/488
+f 391/484 390/483 405/494 406/501
+f 378/471 377/485 392/502 393/490
+f 389/474 388/477 403/496 404/503
+f 387/476 386/479 401/498 402/495
+f 385/478 384/480 399/499 400/497
+f 398/487 397/486 412/504 413/505
+f 396/489 395/488 410/506 411/507
+f 394/491 393/490 408/508 409/509
+f 405/494 404/493 419/510 420/511
+f 403/496 402/495 417/512 418/513
+f 401/498 400/497 415/514 416/515
+f 399/499 398/487 413/505 414/516
+f 397/517 396/489 411/507 412/518
+f 395/488 394/491 409/509 410/506
+f 406/501 405/494 420/511 421/519
+f 393/490 392/502 407/520 408/508
+f 404/503 403/496 418/513 419/521
+f 402/495 401/498 416/515 417/512
+f 400/497 399/499 414/516 415/514
+f 413/505 412/504 427/522 428/523
+f 411/507 410/506 425/524 426/525
+f 409/509 408/508 423/526 424/527
+f 420/511 419/510 434/528 435/529
+f 418/513 417/512 432/530 433/531
+f 416/515 415/514 430/532 431/533
+f 414/516 413/505 428/523 429/534
+f 412/518 411/507 426/525 427/535
+f 410/506 409/509 424/527 425/524
+f 421/519 420/511 435/529 436/536
+f 408/508 407/520 422/537 423/526
+f 419/521 418/513 433/531 434/538
+f 417/512 416/515 431/533 432/530
+f 415/514 414/516 429/534 430/532
+f 428/539 427/540 442/541 443/542
+f 426/525 425/524 440/543 441/544
+f 424/527 423/526 438/545 439/546
+f 435/529 434/528 449/547 450/548
+f 433/531 432/530 447/549 448/550
+f 431/533 430/532 445/551 446/552
+f 429/553 428/539 443/542 444/554
+f 427/540 426/555 441/556 442/541
+f 425/524 424/527 439/546 440/543
+f 436/536 435/529 450/548 451/557
+f 423/526 422/537 437/558 438/545
+f 434/538 433/531 448/550 449/559
+f 432/530 431/533 446/552 447/549
+f 430/532 429/534 444/560 445/551
+f 443/542 442/541 458/561 459/562
+f 441/556 440/563 456/564 457/565
+f 439/546 438/545 454/566 455/567
+f 450/548 449/547 465/568 466/569
+f 448/550 447/549 463/570 464/571
+f 446/572 445/573 461/574 462/575
+f 444/554 443/542 459/562 460/576
+f 442/541 441/556 457/565 458/561
+f 440/543 439/546 455/567 456/577
+f 451/557 450/548 466/569 467/578
+f 438/545 437/558 453/579 454/566
+f 449/547 448/580 464/581 465/568
+f 447/582 446/572 462/575 463/583
+f 445/573 444/554 460/576 461/574
+f 459/562 458/561 473/584 474/585
+f 457/565 456/564 471/586 472/587
+f 455/567 454/566 469/588 470/589
+f 466/569 465/568 480/590 481/591
+f 464/592 463/583 478/593 479/594
+f 462/575 461/574 476/595 477/596
+f 460/576 459/562 474/585 475/597
+f 458/561 457/565 472/587 473/584
+f 456/577 455/567 470/589 471/598
+f 467/578 466/569 481/591 482/599
+f 454/566 453/579 468/600 469/588
+f 465/568 464/581 479/601 480/590
+f 463/583 462/575 477/596 478/593
+f 461/574 460/576 475/597 476/595
+f 1/26 452/602 16/27
+f 331/603 15/21 30/24
+f 331/603 30/24 45/49
+f 16/27 452/602 31/50
+f 331/603 45/49 60/67
+f 31/50 452/602 46/68
+f 331/603 60/67 75/85
+f 46/68 452/602 61/86
+f 331/603 75/85 90/108
+f 61/86 452/602 76/109
+f 331/603 90/108 105/129
+f 76/109 452/602 91/130
+f 331/603 105/129 120/146
+f 91/130 452/602 106/147
+f 331/603 120/146 135/163
+f 106/147 452/602 121/164
+f 331/603 135/163 150/180
+f 121/164 452/602 136/181
+f 331/603 150/180 165/200
+f 136/181 452/602 151/201
+f 331/603 165/200 180/217
+f 151/201 452/602 166/218
+f 331/603 180/217 195/235
+f 166/218 452/602 181/236
+f 331/603 195/235 210/254
+f 181/236 452/602 196/255
+f 331/603 210/254 225/280
+f 196/255 452/602 211/281
+f 331/603 225/280 240/298
+f 211/281 452/602 226/299
+f 331/603 240/298 255/315
+f 226/299 452/602 241/316
+f 331/603 255/315 270/332
+f 241/316 452/602 256/333
+f 331/603 270/332 285/350
+f 256/333 452/602 271/351
+f 331/603 285/350 300/368
+f 271/351 452/602 286/369
+f 331/603 300/368 315/385
+f 286/369 452/602 301/386
+f 331/603 315/385 330/408
+f 301/386 452/602 316/409
+f 331/603 330/408 346/430
+f 316/409 452/602 332/431
+f 331/603 346/430 361/447
+f 332/431 452/602 347/448
+f 331/603 361/447 376/464
+f 347/448 452/602 362/465
+f 331/603 376/464 391/484
+f 362/465 452/602 377/485
+f 331/603 391/484 406/501
+f 377/485 452/602 392/502
+f 331/603 406/501 421/519
+f 392/502 452/602 407/520
+f 331/603 421/519 436/536
+f 407/520 452/602 422/537
+f 331/603 436/536 451/557
+f 422/537 452/602 437/558
+f 331/603 451/557 467/578
+f 437/558 452/602 453/579
+f 331/603 467/578 482/599
+f 453/579 452/602 468/600
+f 474/585 473/584 6/13 7/10
+f 472/587 471/586 4/17 5/14
+f 331/603 482/599 15/21
+f 470/589 469/588 2/25 3/33
+f 481/591 480/590 13/29 14/22
+f 468/600 452/602 1/26
+f 479/594 478/593 11/2 12/1
+f 477/596 476/595 9/6 10/5
+f 475/597 474/585 7/10 8/9
+f 473/584 472/587 5/14 6/13
+f 471/598 470/589 3/33 4/604
+f 482/599 481/591 14/22 15/21
+f 469/588 468/600 1/26 2/25
+f 480/590 479/601 12/30 13/29
+f 478/593 477/596 10/5 11/2
+f 476/595 475/597 8/9 9/6
diff --git a/SurgSim/Testing/Data/Geometry/staple.ply b/SurgSim/Testing/Data/Geometry/staple.ply
new file mode 100644
index 0000000..e2d71cd
--- /dev/null
+++ b/SurgSim/Testing/Data/Geometry/staple.ply
@@ -0,0 +1,49 @@
+ply
+format ascii 1.0
+comment Created by hand
+element dimension 1
+property uint x
+property uint y
+property uint z
+element spacing 1
+property double x
+property double y
+property double z
+element bounds 1
+property double xMin
+property double xMax
+property double yMin
+property double yMax
+property double zMin
+property double zMax
+element voxel 23
+property double x
+property double y
+property double z
+end_header
+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/MeshShapeData/staple_collision.ply b/SurgSim/Testing/Data/Geometry/staple_collision.ply
similarity index 100%
rename from SurgSim/Testing/MeshShapeData/staple_collision.ply
rename to SurgSim/Testing/Data/Geometry/staple_collision.ply
diff --git a/SurgSim/Testing/Data/Geometry/stapler_collision.ply b/SurgSim/Testing/Data/Geometry/stapler_collision.ply
new file mode 100644
index 0000000..09ecfbc
--- /dev/null
+++ b/SurgSim/Testing/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/SurgSim/Testing/Data/Geometry/wound_deformable_with_texture.ply b/SurgSim/Testing/Data/Geometry/wound_deformable_with_texture.ply
new file mode 100644
index 0000000..e71b564
--- /dev/null
+++ b/SurgSim/Testing/Data/Geometry/wound_deformable_with_texture.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/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Brdf0.png b/SurgSim/Testing/Data/Textures/Brdf0.png
similarity index 100%
rename from SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Brdf0.png
rename to SurgSim/Testing/Data/Textures/Brdf0.png
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Brdf1.png b/SurgSim/Testing/Data/Textures/Brdf1.png
similarity index 100%
rename from SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Brdf1.png
rename to SurgSim/Testing/Data/Textures/Brdf1.png
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/CubeMap.png b/SurgSim/Testing/Data/Textures/CubeMap_axes.png
similarity index 100%
rename from SurgSim/Graphics/UnitTests/Data/OsgTextureTests/CubeMap.png
rename to SurgSim/Testing/Data/Textures/CubeMap_axes.png
diff --git a/SurgSim/Testing/Data/Textures/CubeMap_reflection_diffuse.png b/SurgSim/Testing/Data/Textures/CubeMap_reflection_diffuse.png
new file mode 100644
index 0000000..19719e1
Binary files /dev/null and b/SurgSim/Testing/Data/Textures/CubeMap_reflection_diffuse.png differ
diff --git a/SurgSim/Testing/Data/Textures/CubeMap_reflection_specular.png b/SurgSim/Testing/Data/Textures/CubeMap_reflection_specular.png
new file mode 100644
index 0000000..74b3d8d
Binary files /dev/null and b/SurgSim/Testing/Data/Textures/CubeMap_reflection_specular.png differ
diff --git a/SurgSim/Testing/Data/Textures/CubeMap_rgb.png b/SurgSim/Testing/Data/Textures/CubeMap_rgb.png
new file mode 100644
index 0000000..397d5cd
Binary files /dev/null and b/SurgSim/Testing/Data/Textures/CubeMap_rgb.png differ
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/cube.png b/SurgSim/Testing/Data/Textures/CubeMap_rgb_rotate.png
similarity index 100%
rename from SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/cube.png
rename to SurgSim/Testing/Data/Textures/CubeMap_rgb_rotate.png
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Gradient.png b/SurgSim/Testing/Data/Textures/Gradient.png
similarity index 100%
rename from SurgSim/Graphics/UnitTests/Data/OsgTextureTests/Gradient.png
rename to SurgSim/Testing/Data/Textures/Gradient.png
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeX.png b/SurgSim/Testing/Data/Textures/NegativeX.png
similarity index 100%
rename from SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeX.png
rename to SurgSim/Testing/Data/Textures/NegativeX.png
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeY.png b/SurgSim/Testing/Data/Textures/NegativeY.png
similarity index 100%
rename from SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeY.png
rename to SurgSim/Testing/Data/Textures/NegativeY.png
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeZ.png b/SurgSim/Testing/Data/Textures/NegativeZ.png
similarity index 100%
rename from SurgSim/Graphics/UnitTests/Data/OsgTextureTests/NegativeZ.png
rename to SurgSim/Testing/Data/Textures/NegativeZ.png
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveX.png b/SurgSim/Testing/Data/Textures/PositiveX.png
similarity index 100%
rename from SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveX.png
rename to SurgSim/Testing/Data/Textures/PositiveX.png
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveY.png b/SurgSim/Testing/Data/Textures/PositiveY.png
similarity index 100%
rename from SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveY.png
rename to SurgSim/Testing/Data/Textures/PositiveY.png
diff --git a/SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveZ.png b/SurgSim/Testing/Data/Textures/PositiveZ.png
similarity index 100%
rename from SurgSim/Graphics/UnitTests/Data/OsgTextureTests/PositiveZ.png
rename to SurgSim/Testing/Data/Textures/PositiveZ.png
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgScreenSpaceQuadRenderTests/Rectangle.png b/SurgSim/Testing/Data/Textures/Rectangle.png
similarity index 100%
rename from SurgSim/Graphics/RenderTests/Data/OsgScreenSpaceQuadRenderTests/Rectangle.png
rename to SurgSim/Testing/Data/Textures/Rectangle.png
diff --git a/SurgSim/Testing/Data/Textures/bricks.png b/SurgSim/Testing/Data/Textures/bricks.png
new file mode 100644
index 0000000..665ec5b
Binary files /dev/null and b/SurgSim/Testing/Data/Textures/bricks.png differ
diff --git a/SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/wound.png b/SurgSim/Testing/Data/Textures/wound_deformable.png
similarity index 100%
rename from SurgSim/Graphics/RenderTests/Data/OsgMeshRepresentationRenderTests/wound.png
rename to SurgSim/Testing/Data/Textures/wound_deformable.png
diff --git a/SurgSim/Testing/MlcpIO/CMakeLists.txt b/SurgSim/Testing/MlcpIO/CMakeLists.txt
index 8eb1e53..e4640d6 100644
--- a/SurgSim/Testing/MlcpIO/CMakeLists.txt
+++ b/SurgSim/Testing/MlcpIO/CMakeLists.txt
@@ -33,13 +33,11 @@ set(MLCP_IO_HEADERS
# 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"
+add_library(MlcpTestIO
+ ${MLCP_IO_SOURCES}
+ ${MLCP_IO_HEADERS}
)
+surgsim_show_ide_folders("${MLCP_IO_SOURCES}" "${MLCP_IO_HEADERS}")
SET(LIBS
SurgSimMath
@@ -48,8 +46,5 @@ SET(LIBS
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/ReadText.cpp b/SurgSim/Testing/MlcpIO/ReadText.cpp
index 746aee5..ac02f77 100644
--- a/SurgSim/Testing/MlcpIO/ReadText.cpp
+++ b/SurgSim/Testing/MlcpIO/ReadText.cpp
@@ -105,12 +105,30 @@ static bool readEigenRowVector(const std::string& fileName, FILE* in, const char
return false;
}
- std::string format = std::string(" ") + label + " (";
- if (fscanf(in, format.c_str()) != 0)
+ if (std::string(label) == "")
{
- fprintf(stderr, "Bad label for Eigen input in '%s'\n near text '%s'\n",
- fileName.c_str(), getRawLine(in).c_str());
- return false;
+ if (fscanf(in, " (") != 0)
+ {
+ fprintf(stderr, "Unable to read label for Eigen input in '%s'\n near text '%s'\n",
+ fileName.c_str(), getRawLine(in).c_str());
+ return false;
+ }
+ }
+ else
+ {
+ char readLabel[100];
+ if (fscanf(in, " %[^(](", readLabel) != 1)
+ {
+ fprintf(stderr, "Unable to read label for Eigen input in '%s'\n near text '%s'\n",
+ fileName.c_str(), getRawLine(in).c_str());
+ return false;
+ }
+ if (std::string(label) + " " != std::string(readLabel))
+ {
+ fprintf(stderr, "Bad label for Eigen input in '%s'\n near text '%s'\n",
+ fileName.c_str(), getRawLine(in).c_str());
+ return false;
+ }
}
}
@@ -197,8 +215,14 @@ static bool readEigenMatrix(const std::string& fileName, FILE* in, const char* l
return false;
}
- std::string format = std::string(" ") + label + " (";
- if (fscanf(in, format.c_str()) != 0)
+ char readLabel[100];
+ if (fscanf(in, " %[^(](", readLabel) != 1)
+ {
+ fprintf(stderr, "Unable to read label for Eigen input in '%s'\n near text '%s'\n",
+ fileName.c_str(), getRawLine(in).c_str());
+ return false;
+ }
+ if (std::string(label) + " " != std::string(readLabel))
{
fprintf(stderr, "Bad label for Eigen input in '%s'\n near text '%s'\n",
fileName.c_str(), getRawLine(in).c_str());
@@ -471,10 +495,10 @@ bool readMlcpTestDataAsText(const std::string& fileName, MlcpTestData* testData)
static_cast<int>(testData->problem.A.cols()), fileName.c_str());
return false;
}
- if ((testData->problem.mu.rows() != numConstraints) || (testData->problem.mu.cols() != 1))
+ if ((testData->problem.mu.rows() != numAtomicConstraints) || (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()),
+ numAtomicConstraints, 1, static_cast<int>(testData->problem.mu.rows()),
static_cast<int>(testData->problem.mu.cols()), fileName.c_str());
return false;
}
diff --git a/SurgSim/Testing/MockInputComponent.cpp b/SurgSim/Testing/MockInputComponent.cpp
new file mode 100644
index 0000000..53f17d7
--- /dev/null
+++ b/SurgSim/Testing/MockInputComponent.cpp
@@ -0,0 +1,35 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2013-2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES 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/MockInputComponent.h"
+
+#include "SurgSim/Input/InputConsumerInterface.h"
+
+namespace SurgSim
+{
+namespace Testing
+{
+
+MockInputComponent::MockInputComponent(const std::string& name) : SurgSim::Input::InputComponent(name)
+{
+}
+
+void MockInputComponent::setData(const SurgSim::DataStructures::DataGroup& data)
+{
+ handleInput("fake device", data);
+}
+
+}; // Testing
+}; // SurgSim
diff --git a/SurgSim/Testing/MockInputComponent.h b/SurgSim/Testing/MockInputComponent.h
new file mode 100644
index 0000000..1d6ccce
--- /dev/null
+++ b/SurgSim/Testing/MockInputComponent.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_TESTING_MOCKINPUTCOMPONENT_H
+#define SURGSIM_TESTING_MOCKINPUTCOMPONENT_H
+
+#include "SurgSim/DataStructures/DataGroup.h"
+#include "SurgSim/Input/InputComponent.h"
+
+namespace SurgSim
+{
+namespace Testing
+{
+
+/// A MockInputComponent allows Components to test connections with devices without using an
+/// instance of DeviceInterface (and then an InputManager to connect the device with the InputComponent, etc).
+class MockInputComponent : public SurgSim::Input::InputComponent
+{
+public:
+ /// Constructor
+ /// \param name The name.
+ explicit MockInputComponent(const std::string& name);
+
+ /// Set the data that this mock InputComponent will provide via getData.
+ /// \param data The datagroup as if from a device.
+ void setData(const SurgSim::DataStructures::DataGroup& data);
+};
+
+};
+};
+#endif // SURGSIM_TESTING_MOCKINPUTCOMPONENT_H
diff --git a/SurgSim/Testing/MockInputOutput.cpp b/SurgSim/Testing/MockInputOutput.cpp
index 675b9b7..32ff124 100644
--- a/SurgSim/Testing/MockInputOutput.cpp
+++ b/SurgSim/Testing/MockInputOutput.cpp
@@ -50,6 +50,7 @@ void MockInputOutput::handleInput(const std::string& device, const DataGroup& in
void MockInputOutput::initializeInput(const std::string& device, const DataGroup& inputData)
{
++m_numTimesInitializedInput;
+ m_lastReceivedInput = inputData;
}
}; // Testing
diff --git a/SurgSim/Testing/MockPhysicsManager.h b/SurgSim/Testing/MockPhysicsManager.h
index 3eb6bea..1b0d091 100644
--- a/SurgSim/Testing/MockPhysicsManager.h
+++ b/SurgSim/Testing/MockPhysicsManager.h
@@ -27,22 +27,22 @@ namespace Testing
class MockPhysicsManager : public SurgSim::Physics::PhysicsManager
{
public:
- virtual bool executeAdditions(const std::shared_ptr<SurgSim::Framework::Component>& component) override
+ 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
+ bool executeRemovals(const std::shared_ptr<SurgSim::Framework::Component>& component) override
{
return SurgSim::Physics::PhysicsManager::executeRemovals(component);
}
- virtual bool doInitialize() override
+ bool doInitialize() override
{
return SurgSim::Physics::PhysicsManager::doInitialize();
}
- virtual bool doStartUp() override
+ bool doStartUp() override
{
return SurgSim::Physics::PhysicsManager::doStartUp();
}
diff --git a/SurgSim/Testing/OctreeShapeData/staple.vox b/SurgSim/Testing/OctreeShapeData/staple.vox
deleted file mode 100644
index e2a3ad7..0000000
--- a/SurgSim/Testing/OctreeShapeData/staple.vox
+++ /dev/null
@@ -1,26 +0,0 @@
-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/TestingMain.cpp b/SurgSim/Testing/TestingMain.cpp
index 646863b..732a071 100644
--- a/SurgSim/Testing/TestingMain.cpp
+++ b/SurgSim/Testing/TestingMain.cpp
@@ -24,7 +24,6 @@ int main(int argc, char** argv)
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
deleted file mode 100644
index 33fe119..0000000
--- a/SurgSim/Testing/TriangleMeshBaseTests/Cube.ply
+++ /dev/null
@@ -1,51 +0,0 @@
-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/Utilities.h b/SurgSim/Testing/Utilities.h
new file mode 100644
index 0000000..a6876ca
--- /dev/null
+++ b/SurgSim/Testing/Utilities.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_TESTING_UTILITIES_H
+#define SURGSIM_TESTING_UTILITIES_H
+
+namespace SurgSim
+{
+
+namespace Testing
+{
+
+/// Predicate to use to test whether a container contains a certain element
+/// \tparam Container type, can usually be deduced
+/// \param container the container to be tested
+/// \param value the value to check for
+/// \return true when the container contains the given value
+/// \note for maps the value type is std::pair<key, mapped_type>
+template <class Container>
+bool doesContain(const Container& container, const typename Container::value_type& value)
+{
+ return std::find(container.begin(), container.end(), value) != container.end();
+}
+
+}
+}
+
+#endif
diff --git a/SurgSim/Testing/VisualTestCommon/CMakeLists.txt b/SurgSim/Testing/VisualTestCommon/CMakeLists.txt
index 4925014..23d1472 100644
--- a/SurgSim/Testing/VisualTestCommon/CMakeLists.txt
+++ b/SurgSim/Testing/VisualTestCommon/CMakeLists.txt
@@ -14,12 +14,7 @@
# limitations under the License.
-link_directories(
- ${Boost_LIBRARY_DIRS}
-)
-
include_directories(
- "${CMAKE_CURRENT_SOURCE_DIR}"
"${GLUT_INCLUDE_DIR}"
)
@@ -38,7 +33,13 @@ set(VISUAL_TEST_COMMON_HEADERS
)
set(LIBS
+ SurgSimDataStructures
+ SurgSimFramework
+ SurgSimInput
+ SurgSimMath
+ ${Boost_LIBRARIES}
${GLUT_LIBRARIES}
+ ${OPENGL_LIBRARIES}
)
@@ -49,7 +50,4 @@ add_library(VisualTestCommon
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
index aecfeab..f19655a 100644
--- a/SurgSim/Testing/VisualTestCommon/GlutRenderer.cpp
+++ b/SurgSim/Testing/VisualTestCommon/GlutRenderer.cpp
@@ -33,7 +33,7 @@ GlutRenderObject::~GlutRenderObject()
{
}
-void GlutSquare::draw() const
+void GlutSquare::draw()
{
glEnable(GL_LIGHTING);
@@ -73,7 +73,7 @@ void GlutSquare::draw() const
glPopMatrix();
}
-void GlutAxes::draw() const
+void GlutAxes::draw()
{
glDisable(GL_LIGHTING);
@@ -106,7 +106,7 @@ void GlutAxes::draw() const
glPopMatrix();
}
-void GlutSphere::draw() const
+void GlutSphere::draw()
{
glEnable(GL_LIGHTING);
@@ -124,7 +124,87 @@ void GlutSphere::draw() const
glPopMatrix();
}
-void GlutGroup::draw() const
+void GlutImage::draw()
+{
+ if (m_firstRun)
+ {
+ glGenTextures(1, &m_texture);
+ m_firstRun = false;
+ }
+
+ glDisable(GL_LIGHTING);
+ glMatrixMode(GL_PROJECTION);
+ glPushMatrix();
+ glLoadIdentity();
+ glOrtho(-1.0, 1.0, -1.0, 1.0, -1.0, 1.0);
+ glMatrixMode(GL_MODELVIEW);
+ glPushMatrix();
+ glLoadIdentity();
+
+ glEnable(GL_TEXTURE_2D);
+ glEnable(GL_BLEND);
+ glBindTexture(GL_TEXTURE_2D, m_texture);
+
+ ImageType imageData;
+ if(image.tryTakeChanged(&imageData))
+ {
+ float maxPixel = imageData.getAsVector().maxCoeff();
+ if (maxPixel > 1.0)
+ {
+ float scaleFactor = 1.0 / maxPixel;
+ glPixelTransferf(GL_RED_SCALE, scaleFactor);
+ glPixelTransferf(GL_BLUE_SCALE, scaleFactor);
+ glPixelTransferf(GL_GREEN_SCALE, scaleFactor);
+ }
+ else
+ {
+ glPixelTransferf(GL_RED_SCALE, 1.0);
+ glPixelTransferf(GL_BLUE_SCALE, 1.0);
+ glPixelTransferf(GL_GREEN_SCALE, 1.0);
+ }
+
+ switch (imageData.getNumChannels())
+ {
+ case 1:
+ glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, static_cast<GLsizei>(imageData.getWidth()),
+ static_cast<GLsizei>(imageData.getHeight()), 0, GL_LUMINANCE, GL_FLOAT, imageData.getData());
+ break;
+ case 3:
+ glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, static_cast<GLsizei>(imageData.getWidth()),
+ static_cast<GLsizei>(imageData.getHeight()), 0, GL_RGB, GL_FLOAT, imageData.getData());
+ break;
+ }
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+ }
+
+ glColor3d(1, 1, 1);
+ glBegin(GL_QUADS);
+
+ glTexCoord2f(1, 1);
+ glVertex2f(m_bounds.min()(0), m_bounds.min()(1));
+
+ glTexCoord2f(0, 1);
+ glVertex2f(m_bounds.max()(0), m_bounds.min()(1));
+
+ glTexCoord2f(0, 0);
+ glVertex2f(m_bounds.max()(0), m_bounds.max()(1));
+
+ glTexCoord2f(1, 0);
+ glVertex2f(m_bounds.min()(0), m_bounds.max()(1));
+
+ glEnd();
+
+ glDisable(GL_TEXTURE_2D);
+ glDisable(GL_BLEND);
+
+ glPopMatrix();
+ glMatrixMode(GL_PROJECTION);
+ glPopMatrix();
+ glMatrixMode(GL_MODELVIEW);
+}
+
+void GlutGroup::draw()
{
glPushMatrix();
@@ -164,18 +244,25 @@ void GlutRenderer::initialize()
glutDisplayFunc(display);
glutReshapeFunc(reshape);
}
+
void GlutRenderer::display()
{
- glViewport(0, 0, (GLsizei) m_width, (GLsizei) m_height);
+ glViewport(0, 0, static_cast<GLsizei>(m_width), static_cast<GLsizei>(m_height));
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
- gluPerspective(m_camera->fovY, static_cast<float>(m_width) / m_height, m_camera->zNear, m_camera->zFar);
+ if (m_camera)
+ {
+ 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());
+ if (m_camera)
+ {
+ 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);
diff --git a/SurgSim/Testing/VisualTestCommon/GlutRenderer.h b/SurgSim/Testing/VisualTestCommon/GlutRenderer.h
index 19176ee..f2cb307 100644
--- a/SurgSim/Testing/VisualTestCommon/GlutRenderer.h
+++ b/SurgSim/Testing/VisualTestCommon/GlutRenderer.h
@@ -16,15 +16,19 @@
#ifndef SURGSIM_TESTING_VISUALTESTCOMMON_GLUTRENDERER_H
#define SURGSIM_TESTING_VISUALTESTCOMMON_GLUTRENDERER_H
-#include <vector>
+#include <Eigen/Geometry>
#include <memory>
+#include <vector>
#ifdef __APPLE__
#include <GLUT/glut.h>
#else
+#define GLUT_NO_LIB_PRAGMA 1
#include <GL/glut.h>
#endif
+#include "SurgSim/DataStructures/Image.h"
+#include "SurgSim/Framework/LockedContainer.h"
#include "SurgSim/Math/Vector.h"
#include "SurgSim/Math/RigidTransform.h"
@@ -42,7 +46,7 @@ struct GlutRenderObject
virtual ~GlutRenderObject();
/// Pure virtual draw method for subclasses to define how to draw themselves with Glut.
- virtual void draw() const = 0;
+ virtual void draw() = 0;
};
/// Square with center at local origin.
@@ -71,7 +75,7 @@ struct GlutSquare : public GlutRenderObject
}
/// Draws the square with Glut.
- virtual void draw() const;
+ virtual void draw();
};
/// Axes with center at local origin, red axis along the local X-axis, green axis along the local Y-axis, and blue axis
@@ -91,7 +95,7 @@ struct GlutAxes : GlutRenderObject
}
/// Draws the axes with Glut.
- virtual void draw() const;
+ virtual void draw();
};
/// Sphere with center at local origin.
@@ -111,13 +115,43 @@ struct GlutSphere : GlutRenderObject
}
/// Draws the sphere with Glut.
- virtual void draw() const;
+ virtual void draw();
private:
/// GLU quadric object for the quadric operations required to build the sphere.
GLUquadric* quadratic;
};
+/// An Image drawn to the screen
+struct GlutImage : GlutRenderObject
+{
+ typedef SurgSim::DataStructures::Image<float> ImageType;
+
+ /// The image to draw
+ SurgSim::Framework::LockedContainer<ImageType> image;
+
+ /// Constuctor
+ /// \param bounds The bounds, in window coordinates ([-1..1], [-1..1]), to draw the image.
+ explicit GlutImage(const Eigen::AlignedBox<double, 2>& bounds) :
+ m_bounds(bounds),
+ m_firstRun(true)
+ {
+ }
+
+ /// Draws the image with Glut
+ virtual void draw();
+
+private:
+ /// Window coordinates to draw the image
+ Eigen::AlignedBox<double, 2> m_bounds;
+
+ /// Texture used for holding the image
+ unsigned int m_texture;
+
+ /// Is this the fist run of draw
+ bool m_firstRun;
+};
+
/// Group of objects which provides a transform hierarchy.
struct GlutGroup : public GlutRenderObject
{
@@ -130,7 +164,7 @@ struct GlutGroup : public GlutRenderObject
}
/// Draws the group with Glut and iterates through its children to draw them.
- virtual void draw() const;
+ virtual void draw();
};
/// Camera which controls the view of the scene.
diff --git a/SurgSim/Testing/VisualTestCommon/MovingSquareForce.h b/SurgSim/Testing/VisualTestCommon/MovingSquareForce.h
index 70b8340..c3c3130 100644
--- a/SurgSim/Testing/VisualTestCommon/MovingSquareForce.h
+++ b/SurgSim/Testing/VisualTestCommon/MovingSquareForce.h
@@ -34,12 +34,11 @@ public:
/// Constructor.
MovingSquareForce(const std::string& toolDeviceName, const std::string& squareDeviceName);
- virtual void initializeInput(const std::string& device,
- const SurgSim::DataStructures::DataGroup& inputData) override;
+ 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;
+ void handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override;
- virtual bool requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData) override;
+ bool requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData) override;
protected:
/// Updates the state of the tool as described by toolInputData.
diff --git a/SurgSim/Testing/VisualTestCommon/MovingSquareGlutWindow.h b/SurgSim/Testing/VisualTestCommon/MovingSquareGlutWindow.h
index fd9f0e1..064333b 100644
--- a/SurgSim/Testing/VisualTestCommon/MovingSquareGlutWindow.h
+++ b/SurgSim/Testing/VisualTestCommon/MovingSquareGlutWindow.h
@@ -35,10 +35,9 @@ public:
/// Destructor.
~MovingSquareGlutWindow();
- virtual void initializeInput(const std::string& device,
- const SurgSim::DataStructures::DataGroup& inputData) override;
+ 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;
+ void handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override;
protected:
diff --git a/SurgSim/Testing/VisualTestCommon/ToolSquareTest.cpp b/SurgSim/Testing/VisualTestCommon/ToolSquareTest.cpp
index 891c567..5565e11 100644
--- a/SurgSim/Testing/VisualTestCommon/ToolSquareTest.cpp
+++ b/SurgSim/Testing/VisualTestCommon/ToolSquareTest.cpp
@@ -18,6 +18,7 @@
#ifdef __APPLE__
#include <GLUT/glut.h>
#else
+#define GLUT_NO_LIB_PRAGMA 1
#include <GL/glut.h>
#endif
diff --git a/ThirdParty/google-style-lint/cpplint.py b/ThirdParty/google-style-lint/cpplint.py
index 526b955..ccc25d4 100644
--- a/ThirdParty/google-style-lint/cpplint.py
+++ b/ThirdParty/google-style-lint/cpplint.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/env python
#
# Copyright (c) 2009 Google Inc. All rights reserved.
#
@@ -28,44 +28,6 @@
# (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*
@@ -80,6 +42,7 @@ same line, but it is far from perfect (in either direction).
"""
import codecs
+import copy
import getopt
import math # for log
import os
@@ -92,7 +55,8 @@ import unicodedata
_USAGE = """
Syntax: cpplint.py [--verbose=#] [--output=vs7] [--filter=-x,+y,...]
- [--counting=total|toplevel|detailed]
+ [--counting=total|toplevel|detailed] [--root=subdir]
+ [--linelength=digits]
<file> [file] ...
The style guidelines this tries to follow are those in
@@ -107,7 +71,8 @@ Syntax: cpplint.py [--verbose=#] [--output=vs7] [--filter=-x,+y,...]
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.
+ Default linted extensions are .cc, .cpp, .cu, .cuh and .h. Change the
+ extensions with the --extensions flag.
Flags:
@@ -139,75 +104,150 @@ Syntax: cpplint.py [--verbose=#] [--output=vs7] [--filter=-x,+y,...]
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'.
+
+ root=subdir
+ The root directory used for deriving header guard CPP variable.
+ By default, the header guard CPP variable is calculated as the relative
+ path to the directory that contains .git, .hg, or .svn. When this flag
+ is specified, the relative path is calculated from the specified
+ directory. If the specified directory does not exist, this flag is
+ ignored.
+
+ Examples:
+ Assuming that src/.git exists, the header guard CPP variables for
+ src/chrome/browser/ui/browser.h are:
+
+ No flag => CHROME_BROWSER_UI_BROWSER_H_
+ --root=chrome => BROWSER_UI_BROWSER_H_
+ --root=chrome/browser => UI_BROWSER_H_
+
+ linelength=digits
+ This is the allowed line length for the project. The default value is
+ 80 characters.
+
+ Examples:
+ --linelength=120
+
+ extensions=extension,extension,...
+ The allowed file extensions that cpplint will check
+
+ Examples:
+ --extensions=hpp,cpp
+
+ cpplint.py supports per-directory configurations specified in CPPLINT.cfg
+ files. CPPLINT.cfg file can contain a number of key=value pairs.
+ Currently the following options are supported:
+
+ set noparent
+ filter=+filter1,-filter2,...
+ exclude_files=regex
+ linelength=80
+
+ "set noparent" option prevents cpplint from traversing directory tree
+ upwards looking for more .cfg files in parent directories. This option
+ is usually placed in the top-level project directory.
+
+ The "filter" option is similar in function to --filter flag. It specifies
+ message filters in addition to the |_DEFAULT_FILTERS| and those specified
+ through --filter command-line flag.
+
+ "exclude_files" allows to specify a regular expression to be matched against
+ a file name. If the expression matches, the file is skipped and not run
+ through liner.
+
+ "linelength" allows to specify the allowed line length for the project.
+
+ CPPLINT.cfg has an effect on files in the same directory and all
+ sub-directories, unless overridden by a nested configuration file.
+
+ Example file:
+ filter=-build/include_order,+build/include_alpha
+ exclude_files=.*\.cc
+
+ The above example disables build/include_order warning and enables
+ build/include_alpha as well as excludes all .cc from being
+ processed by linter, in the current directory (where the .cfg
+ file is located) and all sub-directories.
"""
# 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=
+ 'build/class',
+ 'build/c++11',
+ '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/alt_tokens',
+ 'readability/braces',
+ 'readability/casting',
+ 'readability/check',
+ 'readability/constructors',
+ 'readability/fn_size',
+ 'readability/function',
+ 'readability/inheritance',
+ 'readability/multiline_comment',
+ 'readability/multiline_string',
+ 'readability/namespace',
+ 'readability/nolint',
+ 'readability/nul',
+ 'readability/strings',
+ 'readability/todo',
+ 'readability/utf8',
+ 'runtime/arrays',
+ 'runtime/casting',
+ 'runtime/explicit',
+ 'runtime/int',
+ 'runtime/init',
+ 'runtime/invalid_increment',
+ 'runtime/member_string_references',
+ 'runtime/memset',
+ 'runtime/indentation_namespace',
+ 'runtime/operator',
+ 'runtime/printf',
+ 'runtime/printf_format',
+ 'runtime/references',
+ 'runtime/string',
+ 'runtime/threadsafe_fn',
+ 'runtime/vlog',
+ 'whitespace/blank_line',
+ 'whitespace/braces',
+ 'whitespace/comma',
+ 'whitespace/comments',
+ 'whitespace/empty_conditional_body',
+ 'whitespace/empty_loop_body',
+ 'whitespace/end_of_line',
+ 'whitespace/ending_newline',
+ 'whitespace/forcolon',
+ 'whitespace/indent',
+ 'whitespace/line_length',
+ 'whitespace/newline',
+ 'whitespace/operators',
+ 'whitespace/parens',
+ 'whitespace/semicolon',
+ 'whitespace/tab',
+ 'whitespace/todo',
+ ]
+
+# These error categories are no longer enforced by cpplint, but for backwards-
+# compatibility they may still appear in NOLINT comments.
+_LEGACY_ERROR_CATEGORIES = [
+ 'readability/streams',
+ ]
+
+# The default state of the category filter. This is overridden 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.
@@ -217,33 +257,150 @@ _DEFAULT_FILTERS = ['-build/include_alpha']
# 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',
+# C++ headers
+_CPP_HEADERS = frozenset([
+ # Legacy
+ 'algobase.h',
+ 'algo.h',
+ 'alloc.h',
+ 'builtinbuf.h',
+ 'bvector.h',
+ 'complex.h',
+ 'defalloc.h',
+ 'deque.h',
+ 'editbuf.h',
+ 'fstream.h',
+ 'function.h',
+ 'hash_map',
+ 'hash_map.h',
+ 'hash_set',
+ 'hash_set.h',
+ 'hashtable.h',
+ 'heap.h',
+ 'indstream.h',
+ 'iomanip.h',
+ 'iostream.h',
+ 'istream.h',
+ 'iterator.h',
+ 'list.h',
+ 'map.h',
+ 'multimap.h',
+ 'multiset.h',
+ 'ostream.h',
+ 'pair.h',
+ 'parsestream.h',
+ 'pfstream.h',
+ 'procbuf.h',
+ 'pthread_alloc',
+ 'pthread_alloc.h',
+ 'rope',
+ 'rope.h',
+ 'ropeimpl.h',
+ 'set.h',
+ 'slist',
+ 'slist.h',
+ 'stack.h',
+ 'stdiostream.h',
+ 'stl_alloc.h',
+ 'stl_relops.h',
+ 'streambuf.h',
+ 'stream.h',
+ 'strfile.h',
+ 'strstream.h',
+ 'tempbuf.h',
+ 'tree.h',
+ 'type_traits.h',
+ 'vector.h',
+ # 17.6.1.2 C++ library headers
+ 'algorithm',
+ 'array',
+ 'atomic',
+ 'bitset',
+ 'chrono',
+ 'codecvt',
+ 'complex',
+ 'condition_variable',
+ 'deque',
+ 'exception',
+ 'forward_list',
+ 'fstream',
+ 'functional',
+ 'future',
+ 'initializer_list',
+ 'iomanip',
+ 'ios',
+ 'iosfwd',
+ 'iostream',
+ 'istream',
+ 'iterator',
+ 'limits',
+ 'list',
+ 'locale',
+ 'map',
+ 'memory',
+ 'mutex',
+ 'new',
+ 'numeric',
+ 'ostream',
+ 'queue',
+ 'random',
+ 'ratio',
+ 'regex',
+ 'set',
+ 'sstream',
+ 'stack',
+ 'stdexcept',
+ 'streambuf',
+ 'string',
+ 'strstream',
+ 'system_error',
+ 'thread',
+ 'tuple',
+ 'typeindex',
+ 'typeinfo',
+ 'type_traits',
+ 'unordered_map',
+ 'unordered_set',
+ 'utility',
+ 'valarray',
+ 'vector',
+ # 17.6.1.2 C++ headers for C library facilities
+ 'cassert',
+ 'ccomplex',
+ 'cctype',
+ 'cerrno',
+ 'cfenv',
+ 'cfloat',
+ 'cinttypes',
+ 'ciso646',
+ 'climits',
+ 'clocale',
+ 'cmath',
+ 'csetjmp',
+ 'csignal',
+ 'cstdalign',
+ 'cstdarg',
+ 'cstdbool',
+ 'cstddef',
+ 'cstdint',
+ 'cstdio',
+ 'cstdlib',
+ 'cstring',
+ 'ctgmath',
+ 'ctime',
+ 'cuchar',
+ 'cwchar',
+ 'cwctype',
])
-# 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',
- ])
+# These headers are excluded from [build/include] and [build/include_order]
+# checks:
+# - Anything not following google file name conventions (containing an
+# uppercase character, such as Python.h or nsStringAPI.h, for example).
+# - Lua headers.
+_THIRD_PARTY_HEADERS_PATTERN = re.compile(
+ r'^(?:[^/]*[A-Z][^/]*\.h|lua\.h|lauxlib\.h|lualib\.h)$')
# Assertion macros. These are defined in base/logging.h and
@@ -278,6 +435,33 @@ for op, inv_replacement in [('==', 'NE'), ('!=', 'EQ'),
_CHECK_REPLACEMENT['EXPECT_FALSE_M'][op] = 'EXPECT_%s_M' % inv_replacement
_CHECK_REPLACEMENT['ASSERT_FALSE_M'][op] = 'ASSERT_%s_M' % inv_replacement
+# Alternative tokens and their replacements. For full list, see section 2.5
+# Alternative tokens [lex.digraph] in the C++ standard.
+#
+# Digraphs (such as '%:') are not included here since it's a mess to
+# match those on a word boundary.
+_ALT_TOKEN_REPLACEMENT = {
+ 'and': '&&',
+ 'bitor': '|',
+ 'or': '||',
+ 'xor': '^',
+ 'compl': '~',
+ 'bitand': '&',
+ 'and_eq': '&=',
+ 'or_eq': '|=',
+ 'xor_eq': '^=',
+ 'not': '!',
+ 'not_eq': '!='
+ }
+
+# Compile regular expression that matches all the above keywords. The "[ =()]"
+# bit is meant to avoid matching these keywords outside of boolean expressions.
+#
+# False positives include C-style multi-line comments and multi-line strings
+# but those have always been troublesome for cpplint.
+_ALT_TOKEN_REPLACEMENT_PATTERN = re.compile(
+ r'[ =()](' + ('|'.join(_ALT_TOKEN_REPLACEMENT.keys())) + r')(?=[ (]|$)')
+
# These constants define types of headers for use with
# _IncludeState.CheckNextIncludeOrder().
@@ -287,16 +471,36 @@ _LIKELY_MY_HEADER = 3
_POSSIBLE_MY_HEADER = 4
_OTHER_HEADER = 5
+# These constants define the current inline assembly state
+_NO_ASM = 0 # Outside of inline assembly block
+_INSIDE_ASM = 1 # Inside inline assembly block
+_END_ASM = 2 # Last line of inline assembly block
+_BLOCK_ASM = 3 # The whole block is an inline assembly block
-_regexp_compile_cache = {}
+# Match start of assembly blocks
+_MATCH_ASM = re.compile(r'^\s*(?:asm|_asm|__asm|__asm__)'
+ r'(?:\s+(volatile|__volatile__))?'
+ r'\s*[{(]')
-# Finds occurrences of NOLINT or NOLINT(...).
-_RE_SUPPRESSION = re.compile(r'\bNOLINT\b(\([^)]*\))?')
+
+_regexp_compile_cache = {}
# {str, set(int)}: a map from error categories to sets of linenumbers
# on which those errors are expected and should be suppressed.
_error_suppressions = {}
+# The root directory used for deriving header guard CPP variable.
+# This is set by --root flag.
+_root = None
+
+# The allowed line length of files.
+# This is set by --linelength flag.
+_line_length = 80
+
+# The allowed extensions for file names
+# This is set by --extensions flag.
+_valid_extensions = set(['cc', 'h', 'cpp', 'cu', 'cuh'])
+
def ParseNolintSuppressions(filename, raw_line, linenum, error):
"""Updates the global list of error-suppressions.
@@ -310,24 +514,27 @@ def ParseNolintSuppressions(filename, raw_line, linenum, error):
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)
+ matched = Search(r'\bNOLINT(NEXTLINE)?\b(\([^)]+\))?', raw_line)
if matched:
- category = matched.group(1)
+ if matched.group(1):
+ suppressed_line = linenum + 1
+ else:
+ suppressed_line = linenum
+ category = matched.group(2)
if category in (None, '(*)'): # => "suppress all"
- _error_suppressions.setdefault(None, set()).add(linenum)
+ _error_suppressions.setdefault(None, set()).add(suppressed_line)
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_suppressions.setdefault(category, set()).add(suppressed_line)
+ elif category not in _LEGACY_ERROR_CATEGORIES:
error(filename, linenum, 'readability/nolint', 5,
'Unknown NOLINT error category: %s' % category)
def ResetNolintSuppressions():
- "Resets the set of NOLINT suppressions to empty."
+ """Resets the set of NOLINT suppressions to empty."""
_error_suppressions.clear()
@@ -346,28 +553,48 @@ def IsErrorSuppressedByNolint(category, linenum):
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:
+ if pattern not in _regexp_compile_cache:
_regexp_compile_cache[pattern] = sre_compile.compile(pattern)
return _regexp_compile_cache[pattern].match(s)
+def ReplaceAll(pattern, rep, s):
+ """Replaces instances of pattern in a string with a replacement.
+
+ The compiled regex is kept in a cache shared by Match and Search.
+
+ Args:
+ pattern: regex pattern
+ rep: replacement text
+ s: search string
+
+ Returns:
+ string with replacements made (or original string if no replacements)
+ """
+ if pattern not in _regexp_compile_cache:
+ _regexp_compile_cache[pattern] = sre_compile.compile(pattern)
+ return _regexp_compile_cache[pattern].sub(rep, s)
+
+
def Search(pattern, s):
"""Searches the string for the pattern, caching the compiled regexp."""
- if not pattern in _regexp_compile_cache:
+ if pattern not in _regexp_compile_cache:
_regexp_compile_cache[pattern] = sre_compile.compile(pattern)
return _regexp_compile_cache[pattern].search(s)
-class _IncludeState(dict):
+class _IncludeState(object):
"""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.
+ include_list contains list of lists of (header, line number) pairs.
+ It's a lists of lists rather than just one flat list to make it
+ easier to update across preprocessor boundaries.
Call CheckNextIncludeOrder() once for each header in the file, passing
in the type constants defined above. Calls in an illegal order will
@@ -398,12 +625,45 @@ class _IncludeState(dict):
}
def __init__(self):
- dict.__init__(self)
+ self.include_list = [[]]
+ self.ResetSection('')
+
+ def FindHeader(self, header):
+ """Check if a header has already been included.
+
+ Args:
+ header: header to check.
+ Returns:
+ Line number of previous occurrence, or -1 if the header has not
+ been seen before.
+ """
+ for section_list in self.include_list:
+ for f in section_list:
+ if f[0] == header:
+ return f[1]
+ return -1
+
+ def ResetSection(self, directive):
+ """Reset section checking for preprocessor directive.
+
+ Args:
+ directive: preprocessor directive (e.g. "if", "else").
+ """
# The name of the current section.
self._section = self._INITIAL_SECTION
# The path of last found header.
self._last_header = ''
+ # Update list of includes. Note that we never pop from the
+ # include list.
+ if directive in ('if', 'ifdef', 'ifndef'):
+ self.include_list.append([])
+ elif directive in ('else', 'elif'):
+ self.include_list[-1] = []
+
+ def SetLastHeader(self, header_path):
+ self._last_header = header_path
+
def CanonicalizeAlphabeticalOrder(self, header_path):
"""Returns a path canonicalized for alphabetical comparison.
@@ -419,19 +679,25 @@ class _IncludeState(dict):
"""
return header_path.replace('-inl.h', '.h').replace('-', '_').lower()
- def IsInAlphabeticalOrder(self, header_path):
+ def IsInAlphabeticalOrder(self, clean_lines, linenum, header_path):
"""Check if a header is in alphabetical order with the previous header.
Args:
- header_path: Header to be checked.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ header_path: Canonicalized 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:
+ # If previous section is different from current section, _last_header will
+ # be reset to empty string, so it's always less than current header.
+ #
+ # If previous line was a blank line, assume that the headers are
+ # intentionally sorted the way they are.
+ if (self._last_header > header_path and
+ Match(r'^\s*#\s*include\b', clean_lines.elided[linenum - 1])):
return False
- self._last_header = canonical_header
return True
def CheckNextIncludeOrder(self, header_type):
@@ -496,6 +762,8 @@ class _CppLintState(object):
self.error_count = 0 # global count of reported errors
# filters to apply when emitting error messages
self.filters = _DEFAULT_FILTERS[:]
+ # backup of filter list. Used to restore the state after each file.
+ self._filters_backup = self.filters[:]
self.counting = 'total' # In what way are we counting errors?
self.errors_by_category = {} # string to int dict storing error counts
@@ -534,6 +802,10 @@ class _CppLintState(object):
"""
# Default filters always have less priority than the flag ones.
self.filters = _DEFAULT_FILTERS[:]
+ self.AddFilters(filters)
+
+ def AddFilters(self, filters):
+ """ Adds more filters to the existing list of error-message filters. """
for filt in filters.split(','):
clean_filt = filt.strip()
if clean_filt:
@@ -543,6 +815,14 @@ class _CppLintState(object):
raise ValueError('Every filter in --filters must start with + or -'
' (%s does not)' % filt)
+ def BackupFilters(self):
+ """ Saves the current filter list to backup storage."""
+ self._filters_backup = self.filters[:]
+
+ def RestoreFilters(self):
+ """ Restores filters previously backed up."""
+ self.filters = self._filters_backup[:]
+
def ResetErrorCounts(self):
"""Sets the module's error statistic back to zero."""
self.error_count = 0
@@ -610,6 +890,25 @@ def _SetFilters(filters):
"""
_cpplint_state.SetFilters(filters)
+def _AddFilters(filters):
+ """Adds more filter overrides.
+
+ Unlike _SetFilters, this function does not reset the current list of filters
+ available.
+
+ Args:
+ filters: A string of comma-separated filters (eg "whitespace/indent").
+ Each filter should start with + or -; else we die.
+ """
+ _cpplint_state.AddFilters(filters)
+
+def _BackupFilters():
+ """ Saves the current filter list to backup storage."""
+ _cpplint_state.BackupFilters()
+
+def _RestoreFilters():
+ """ Restores filters previously backed up."""
+ _cpplint_state.RestoreFilters()
class _FunctionState(object):
"""Tracks current function name and the number of lines in its body."""
@@ -672,7 +971,7 @@ class _IncludeError(Exception):
pass
-class FileInfo:
+class FileInfo(object):
"""Provides utility functions for filenames.
FileInfo provides easy access to the components of a file's path
@@ -770,6 +1069,7 @@ def _ShouldPrintError(category, confidence, linenum):
# 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
@@ -816,19 +1116,20 @@ def Error(filename, linenum, category, confidence, message):
if _cpplint_state.output_format == 'vs7':
sys.stderr.write('%s(%s): %s [%s] [%d]\n' % (
filename, linenum, message, category, confidence))
+ elif _cpplint_state.output_format == 'eclipse':
+ sys.stderr.write('%s:%s: warning: %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.
+# Matches standard C++ escape sequences 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.
+# Match a single C style comment on the same line.
+_RE_PATTERN_C_COMMENTS = r'/\*(?:[^*]|\*(?!/))*\*/'
+# Matches multi-line C style 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.
@@ -837,10 +1138,10 @@ _RE_PATTERN_CLEANSE_LINE_SINGLE_QUOTES = re.compile(r"'.'")
# 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)
+ r'(\s*' + _RE_PATTERN_C_COMMENTS + r'\s*$|' +
+ _RE_PATTERN_C_COMMENTS + r'\s+|' +
+ r'\s+' + _RE_PATTERN_C_COMMENTS + r'(?=\W)|' +
+ _RE_PATTERN_C_COMMENTS + r')')
def IsCppString(line):
@@ -860,6 +1161,72 @@ def IsCppString(line):
return ((line.count('"') - line.count(r'\"') - line.count("'\"'")) & 1) == 1
+def CleanseRawStrings(raw_lines):
+ """Removes C++11 raw strings from lines.
+
+ Before:
+ static const char kData[] = R"(
+ multi-line string
+ )";
+
+ After:
+ static const char kData[] = ""
+ (replaced by blank line)
+ "";
+
+ Args:
+ raw_lines: list of raw lines.
+
+ Returns:
+ list of lines with C++11 raw strings replaced by empty strings.
+ """
+
+ delimiter = None
+ lines_without_raw_strings = []
+ for line in raw_lines:
+ if delimiter:
+ # Inside a raw string, look for the end
+ end = line.find(delimiter)
+ if end >= 0:
+ # Found the end of the string, match leading space for this
+ # line and resume copying the original lines, and also insert
+ # a "" on the last line.
+ leading_space = Match(r'^(\s*)\S', line)
+ line = leading_space.group(1) + '""' + line[end + len(delimiter):]
+ delimiter = None
+ else:
+ # Haven't found the end yet, append a blank line.
+ line = '""'
+
+ # Look for beginning of a raw string, and replace them with
+ # empty strings. This is done in a loop to handle multiple raw
+ # strings on the same line.
+ while delimiter is None:
+ # Look for beginning of a raw string.
+ # See 2.14.15 [lex.string] for syntax.
+ matched = Match(r'^(.*)\b(?:R|u8R|uR|UR|LR)"([^\s\\()]*)\((.*)$', line)
+ if matched:
+ delimiter = ')' + matched.group(2) + '"'
+
+ end = matched.group(3).find(delimiter)
+ if end >= 0:
+ # Raw string ended on same line
+ line = (matched.group(1) + '""' +
+ matched.group(3)[end + len(delimiter):])
+ delimiter = None
+ else:
+ # Start of a multi-line raw string
+ line = matched.group(1) + '""'
+ else:
+ break
+
+ lines_without_raw_strings.append(line)
+
+ # TODO(unknown): if delimiter is not None here, we might want to
+ # emit a warning for unterminated string.
+ return lines_without_raw_strings
+
+
def FindNextMultiLineCommentStart(lines, lineix):
"""Find the beginning marker for a multiline comment."""
while lineix < len(lines):
@@ -885,7 +1252,7 @@ def RemoveMultiLineCommentsFromRange(lines, begin, end):
# 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'
+ lines[i] = '/**/'
def RemoveMultiLineComments(filename, lines, error):
@@ -921,12 +1288,14 @@ def CleanseComments(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.
+ """Holds 4 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.
+ 3) raw_lines member contains all the lines without processing.
+ 4) lines_without_raw_strings member is same as raw_lines, but with C++11 raw
+ strings removed.
+ All these members are of <type 'list'>, and of the same length.
"""
def __init__(self, lines):
@@ -934,9 +1303,11 @@ class CleansedLines(object):
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.lines_without_raw_strings = CleanseRawStrings(lines)
+ for linenum in range(len(self.lines_without_raw_strings)):
+ self.lines.append(CleanseComments(
+ self.lines_without_raw_strings[linenum]))
+ elided = self._CollapseStrings(self.lines_without_raw_strings[linenum])
self.elided.append(CleanseComments(elided))
def NumLines(self):
@@ -955,22 +1326,151 @@ class CleansedLines(object):
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
+ if _RE_PATTERN_INCLUDE.match(elided):
+ return 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)
+
+ # Replace quoted strings and digit separators. Both single quotes
+ # and double quotes are processed in the same loop, otherwise
+ # nested quotes wouldn't work.
+ collapsed = ''
+ while True:
+ # Find the first quote character
+ match = Match(r'^([^\'"]*)([\'"])(.*)$', elided)
+ if not match:
+ collapsed += elided
+ break
+ head, quote, tail = match.groups()
+
+ if quote == '"':
+ # Collapse double quoted strings
+ second_quote = tail.find('"')
+ if second_quote >= 0:
+ collapsed += head + '""'
+ elided = tail[second_quote + 1:]
+ else:
+ # Unmatched double quote, don't bother processing the rest
+ # of the line since this is probably a multiline string.
+ collapsed += elided
+ break
+ else:
+ # Found single quote, check nearby text to eliminate digit separators.
+ #
+ # There is no special handling for floating point here, because
+ # the integer/fractional/exponent parts would all be parsed
+ # correctly as long as there are digits on both sides of the
+ # separator. So we are fine as long as we don't see something
+ # like "0.'3" (gcc 4.9.0 will not allow this literal).
+ if Search(r'\b(?:0[bBxX]?|[1-9])[0-9a-fA-F]*$', head):
+ match_literal = Match(r'^((?:\'?[0-9a-zA-Z_])*)(.*)$', "'" + tail)
+ collapsed += head + match_literal.group(1).replace("'", '')
+ elided = match_literal.group(2)
+ else:
+ second_quote = tail.find('\'')
+ if second_quote >= 0:
+ collapsed += head + "''"
+ elided = tail[second_quote + 1:]
+ else:
+ # Unmatched single quote
+ collapsed += elided
+ break
+
+ return collapsed
+
+
+def FindEndOfExpressionInLine(line, startpos, stack):
+ """Find the position just after the end of current parenthesized expression.
+
+ Args:
+ line: a CleansedLines line.
+ startpos: start searching at this position.
+ stack: nesting stack at startpos.
+
+ Returns:
+ On finding matching end: (index just after matching end, None)
+ On finding an unclosed expression: (-1, None)
+ Otherwise: (-1, new stack at end of this line)
+ """
+ for i in xrange(startpos, len(line)):
+ char = line[i]
+ if char in '([{':
+ # Found start of parenthesized expression, push to expression stack
+ stack.append(char)
+ elif char == '<':
+ # Found potential start of template argument list
+ if i > 0 and line[i - 1] == '<':
+ # Left shift operator
+ if stack and stack[-1] == '<':
+ stack.pop()
+ if not stack:
+ return (-1, None)
+ elif i > 0 and Search(r'\boperator\s*$', line[0:i]):
+ # operator<, don't add to stack
+ continue
+ else:
+ # Tentative start of template argument list
+ stack.append('<')
+ elif char in ')]}':
+ # Found end of parenthesized expression.
+ #
+ # If we are currently expecting a matching '>', the pending '<'
+ # must have been an operator. Remove them from expression stack.
+ while stack and stack[-1] == '<':
+ stack.pop()
+ if not stack:
+ return (-1, None)
+ if ((stack[-1] == '(' and char == ')') or
+ (stack[-1] == '[' and char == ']') or
+ (stack[-1] == '{' and char == '}')):
+ stack.pop()
+ if not stack:
+ return (i + 1, None)
+ else:
+ # Mismatched parentheses
+ return (-1, None)
+ elif char == '>':
+ # Found potential end of template argument list.
+
+ # Ignore "->" and operator functions
+ if (i > 0 and
+ (line[i - 1] == '-' or Search(r'\boperator\s*$', line[0:i - 1]))):
+ continue
+
+ # Pop the stack if there is a matching '<'. Otherwise, ignore
+ # this '>' since it must be an operator.
+ if stack:
+ if stack[-1] == '<':
+ stack.pop()
+ if not stack:
+ return (i + 1, None)
+ elif char == ';':
+ # Found something that look like end of statements. If we are currently
+ # expecting a '>', the matching '<' must have been an operator, since
+ # template argument list should not contain statements.
+ while stack and stack[-1] == '<':
+ stack.pop()
+ if not stack:
+ return (-1, None)
+
+ # Did not find end of expression or unbalanced parentheses on this line
+ return (-1, stack)
def CloseExpression(clean_lines, linenum, pos):
- """If input points to ( or { or [, finds the position that closes it.
+ """If input points to ( or { or [ or <, finds the position that closes it.
- If lines[linenum][pos] points to a '(' or '{' or '[', finds the
+ If lines[linenum][pos] points to a '(' or '{' or '[' or '<', finds the
linenum/pos that correspond to the closing of the expression.
+ TODO(unknown): cpplint spends a fair bit of time matching parentheses.
+ Ideally we would want to index all opening and closing parentheses once
+ and have CloseExpression be just a simple lookup, but due to preprocessor
+ tricks, this is not so easy.
+
Args:
clean_lines: A CleansedLines instance containing the file.
linenum: The number of the line to check.
@@ -984,24 +1484,139 @@ def CloseExpression(clean_lines, linenum, pos):
"""
line = clean_lines.elided[linenum]
- startchar = line[pos]
- if startchar not in '({[':
+ if (line[pos] not in '({[<') or Match(r'<[<=]', line[pos:]):
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:
+ # Check first line
+ (end_pos, stack) = FindEndOfExpressionInLine(line, pos, [])
+ if end_pos > -1:
+ return (line, linenum, end_pos)
+
+ # Continue scanning forward
+ while stack and linenum < clean_lines.NumLines() - 1:
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)
+ (end_pos, stack) = FindEndOfExpressionInLine(line, 0, stack)
+ if end_pos > -1:
+ return (line, linenum, end_pos)
+
+ # Did not find end of expression before end of file, give up
+ return (line, clean_lines.NumLines(), -1)
+
+
+def FindStartOfExpressionInLine(line, endpos, stack):
+ """Find position at the matching start of current expression.
+
+ This is almost the reverse of FindEndOfExpressionInLine, but note
+ that the input position and returned position differs by 1.
+
+ Args:
+ line: a CleansedLines line.
+ endpos: start searching at this position.
+ stack: nesting stack at endpos.
+
+ Returns:
+ On finding matching start: (index at matching start, None)
+ On finding an unclosed expression: (-1, None)
+ Otherwise: (-1, new stack at beginning of this line)
+ """
+ i = endpos
+ while i >= 0:
+ char = line[i]
+ if char in ')]}':
+ # Found end of expression, push to expression stack
+ stack.append(char)
+ elif char == '>':
+ # Found potential end of template argument list.
+ #
+ # Ignore it if it's a "->" or ">=" or "operator>"
+ if (i > 0 and
+ (line[i - 1] == '-' or
+ Match(r'\s>=\s', line[i - 1:]) or
+ Search(r'\boperator\s*$', line[0:i]))):
+ i -= 1
+ else:
+ stack.append('>')
+ elif char == '<':
+ # Found potential start of template argument list
+ if i > 0 and line[i - 1] == '<':
+ # Left shift operator
+ i -= 1
+ else:
+ # If there is a matching '>', we can pop the expression stack.
+ # Otherwise, ignore this '<' since it must be an operator.
+ if stack and stack[-1] == '>':
+ stack.pop()
+ if not stack:
+ return (i, None)
+ elif char in '([{':
+ # Found start of expression.
+ #
+ # If there are any unmatched '>' on the stack, they must be
+ # operators. Remove those.
+ while stack and stack[-1] == '>':
+ stack.pop()
+ if not stack:
+ return (-1, None)
+ if ((char == '(' and stack[-1] == ')') or
+ (char == '[' and stack[-1] == ']') or
+ (char == '{' and stack[-1] == '}')):
+ stack.pop()
+ if not stack:
+ return (i, None)
+ else:
+ # Mismatched parentheses
+ return (-1, None)
+ elif char == ';':
+ # Found something that look like end of statements. If we are currently
+ # expecting a '<', the matching '>' must have been an operator, since
+ # template argument list should not contain statements.
+ while stack and stack[-1] == '>':
+ stack.pop()
+ if not stack:
+ return (-1, None)
+
+ i -= 1
+
+ return (-1, stack)
+
+
+def ReverseCloseExpression(clean_lines, linenum, pos):
+ """If input points to ) or } or ] or >, finds the position that opens it.
+
+ If lines[linenum][pos] points to a ')' or '}' or ']' or '>', finds the
+ linenum/pos that correspond to the opening 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 *at* the opening brace, or
+ (line, 0, -1) if we never find the matching opening brace. Note
+ we ignore strings and comments when matching; and the line we
+ return is the 'cleansed' line at linenum.
+ """
+ line = clean_lines.elided[linenum]
+ if line[pos] not in ')}]>':
+ return (line, 0, -1)
+
+ # Check last line
+ (start_pos, stack) = FindStartOfExpressionInLine(line, pos, [])
+ if start_pos > -1:
+ return (line, linenum, start_pos)
+
+ # Continue scanning backward
+ while stack and linenum > 0:
+ linenum -= 1
+ line = clean_lines.elided[linenum]
+ (start_pos, stack) = FindStartOfExpressionInLine(line, len(line) - 1, stack)
+ if start_pos > -1:
+ return (line, linenum, start_pos)
+
+ # Did not find start of expression before beginning of file, give up
+ return (line, 0, -1)
def CheckForCopyright(filename, lines, error):
@@ -1017,6 +1632,22 @@ def CheckForCopyright(filename, lines, error):
'You should have a line: "Copyright [year] <Copyright Owner>"')
+def GetIndentLevel(line):
+ """Return the number of leading spaces in line.
+
+ Args:
+ line: A string to check.
+
+ Returns:
+ An integer count of leading spaces, possibly zero.
+ """
+ indent = Match(r'^( *)\S', line)
+ if indent:
+ return len(indent.group(1))
+ else:
+ return 0
+
+
def GetHeaderGuardCPPVariable(filename):
"""Returns the CPP variable that should be used as a header guard.
@@ -1032,12 +1663,18 @@ def GetHeaderGuardCPPVariable(filename):
# Restores original filename in case that cpplint is invoked from Emacs's
# flymake.
filename = re.sub(r'_flymake\.h$', '.h', filename)
-
+ filename = re.sub(r'/\.flymake/([^/]*)$', r'/\1', filename)
+ # Replace 'c++' with 'cpp'.
+ filename = filename.replace('C++', 'cpp').replace('c++', 'cpp')
+
fileinfo = FileInfo(filename)
- return re.sub(r'[-./\s]', '_', fileinfo.RepositoryName()).upper() + '_'
+ file_path_from_root = fileinfo.RepositoryName()
+ if _root:
+ file_path_from_root = re.sub('^' + _root + os.sep, '', file_path_from_root)
+ return re.sub(r'[^a-zA-Z0-9]', '_', file_path_from_root).upper() + '_'
-def CheckForHeaderGuard(filename, lines, error):
+def CheckForHeaderGuard(filename, clean_lines, error):
"""Checks that the file contains a header guard.
Logs an error if no #ifndef header guard is present. For other
@@ -1045,18 +1682,29 @@ def CheckForHeaderGuard(filename, lines, error):
Args:
filename: The name of the C++ header file.
- lines: An array of strings, each representing a line of the file.
+ clean_lines: A CleansedLines instance containing the file.
error: The function to call with any errors found.
"""
+ # Don't check for header guards if there are error suppression
+ # comments somewhere in this file.
+ #
+ # Because this is silencing a warning for a nonexistent line, we
+ # only support the very specific NOLINT(build/header_guard) syntax,
+ # and not the general NOLINT or NOLINT(*) syntax.
+ raw_lines = clean_lines.lines_without_raw_strings
+ for i in raw_lines:
+ if Search(r'//\s*NOLINT\(build/header_guard\)', i):
+ return
+
cppvar = GetHeaderGuardCPPVariable(filename)
- ifndef = None
+ ifndef = ''
ifndef_linenum = 0
- define = None
- endif = None
+ define = ''
+ endif = ''
endif_linenum = 0
- for linenum, line in enumerate(lines):
+ for linenum, line in enumerate(raw_lines):
linesplit = line.split()
if len(linesplit) >= 2:
# find the first occurrence of #ifndef and #define, save arg
@@ -1071,18 +1719,12 @@ def CheckForHeaderGuard(filename, lines, error):
endif = line
endif_linenum = linenum
- if not ifndef:
+ if not ifndef or not define or ifndef != define:
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:
@@ -1090,35 +1732,82 @@ def CheckForHeaderGuard(filename, lines, error):
if ifndef != cppvar + '_':
error_level = 5
- ParseNolintSuppressions(filename, lines[ifndef_linenum], ifndef_linenum,
+ ParseNolintSuppressions(filename, raw_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)
+ # Check for "//" comments on endif line.
+ ParseNolintSuppressions(filename, raw_lines[endif_linenum], endif_linenum,
+ error)
+ match = Match(r'#endif\s*//\s*' + cppvar + r'(_)?\b', endif)
+ if match:
+ if match.group(1) == '_':
+ # Issue low severity warning for deprecated double trailing underscore
+ error(filename, endif_linenum, 'build/header_guard', 0,
+ '#endif line should be "#endif // %s"' % cppvar)
return
- if endif != ('#endif // %s' % cppvar):
- error_level = 0
- if endif != ('#endif // %s' % (cppvar + '_')):
- error_level = 5
+ # Didn't find the corresponding "//" comment. If this file does not
+ # contain any "//" comments at all, it could be that the compiler
+ # only wants "/**/" comments, look for those instead.
+ no_single_line_comments = True
+ for i in xrange(1, len(raw_lines) - 1):
+ line = raw_lines[i]
+ if Match(r'^(?:(?:\'(?:\.|[^\'])*\')|(?:"(?:\.|[^"])*")|[^\'"])*//', line):
+ no_single_line_comments = False
+ break
- ParseNolintSuppressions(filename, lines[endif_linenum], endif_linenum,
- error)
- error(filename, endif_linenum, 'build/header_guard', error_level,
- '#endif line should be "#endif // %s"' % cppvar)
+ if no_single_line_comments:
+ match = Match(r'#endif\s*/\*\s*' + cppvar + r'(_)?\s*\*/', endif)
+ if match:
+ if match.group(1) == '_':
+ # Low severity warning for double trailing underscore
+ error(filename, endif_linenum, 'build/header_guard', 0,
+ '#endif line should be "#endif /* %s */"' % cppvar)
+ return
+
+ # Didn't find anything
+ error(filename, endif_linenum, 'build/header_guard', 5,
+ '#endif line should be "#endif // %s"' % cppvar)
+
+
+def CheckHeaderFileIncluded(filename, include_state, error):
+ """Logs an error if a .cc file does not include its header."""
+
+ # Do not check test files
+ if filename.endswith('_test.cc') or filename.endswith('_unittest.cc'):
+ return
+
+ fileinfo = FileInfo(filename)
+ headerfile = filename[0:len(filename) - 2] + 'h'
+ if not os.path.exists(headerfile):
+ return
+ headername = FileInfo(headerfile).RepositoryName()
+ first_include = 0
+ for section_list in include_state.include_list:
+ for f in section_list:
+ if headername in f[0] or f[0] in headername:
+ return
+ if not first_include:
+ first_include = f[1]
+
+ error(filename, first_include, 'build/include', 5,
+ '%s should include its header file %s' % (fileinfo.RepositoryName(),
+ headername))
-def CheckForUnicodeReplacementCharacters(filename, lines, error):
- """Logs an error for each line containing Unicode replacement characters.
+def CheckForBadCharacters(filename, lines, error):
+ """Logs an error for each line containing bad 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.
+ Two kinds of bad characters:
+
+ 1. 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.
+
+ 2. NUL bytes. These are problematic for some tools.
Args:
filename: The name of the current file.
@@ -1129,6 +1818,8 @@ def CheckForUnicodeReplacementCharacters(filename, lines, error):
if u'\ufffd' in line:
error(filename, linenum, 'readability/utf8', 5,
'Line contains invalid UTF-8 (or Unicode replacement character).')
+ if '\0' in line:
+ error(filename, linenum, 'readability/nul', 5, 'Line contains NUL byte.')
def CheckForNewlineAtEOF(filename, lines, error):
@@ -1183,24 +1874,37 @@ def CheckForMultilineCommentsAndStrings(filename, clean_lines, linenum, error):
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('),
+ 'do well with such strings, and may give bogus warnings. '
+ 'Use C++11 raw strings or concatenation instead.')
+
+
+# (non-threadsafe name, thread-safe alternative, validation pattern)
+#
+# The validation pattern is used to eliminate false positives such as:
+# _rand(); // false positive due to substring match.
+# ->rand(); // some member function rand().
+# ACMRandom rand(seed); // some variable named rand.
+# ISAACRandom rand(); // another variable named rand.
+#
+# Basically we require the return value of these functions to be used
+# in some expression context on the same line by matching on some
+# operator before the function name. This eliminates constructors and
+# member function calls.
+_UNSAFE_FUNC_PREFIX = r'(?:[-+*/=%^&|(<]\s*|>\s+)'
+_THREADING_LIST = (
+ ('asctime(', 'asctime_r(', _UNSAFE_FUNC_PREFIX + r'asctime\([^)]+\)'),
+ ('ctime(', 'ctime_r(', _UNSAFE_FUNC_PREFIX + r'ctime\([^)]+\)'),
+ ('getgrgid(', 'getgrgid_r(', _UNSAFE_FUNC_PREFIX + r'getgrgid\([^)]+\)'),
+ ('getgrnam(', 'getgrnam_r(', _UNSAFE_FUNC_PREFIX + r'getgrnam\([^)]+\)'),
+ ('getlogin(', 'getlogin_r(', _UNSAFE_FUNC_PREFIX + r'getlogin\(\)'),
+ ('getpwnam(', 'getpwnam_r(', _UNSAFE_FUNC_PREFIX + r'getpwnam\([^)]+\)'),
+ ('getpwuid(', 'getpwuid_r(', _UNSAFE_FUNC_PREFIX + r'getpwuid\([^)]+\)'),
+ ('gmtime(', 'gmtime_r(', _UNSAFE_FUNC_PREFIX + r'gmtime\([^)]+\)'),
+ ('localtime(', 'localtime_r(', _UNSAFE_FUNC_PREFIX + r'localtime\([^)]+\)'),
+ ('rand(', 'rand_r(', _UNSAFE_FUNC_PREFIX + r'rand\(\)'),
+ ('strtok(', 'strtok_r(',
+ _UNSAFE_FUNC_PREFIX + r'strtok\([^)]+\)'),
+ ('ttyname(', 'ttyname_r(', _UNSAFE_FUNC_PREFIX + r'ttyname\([^)]+\)'),
)
@@ -1220,17 +1924,34 @@ def CheckPosixThreading(filename, clean_lines, linenum, error):
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 ('_', '.', '>'))):
+ for single_thread_func, multithread_safe_func, pattern in _THREADING_LIST:
+ # Additional pattern matching check to confirm that this is the
+ # function we are looking for
+ if Search(pattern, line):
error(filename, linenum, 'runtime/threadsafe_fn', 2,
- 'Consider using ' + multithread_safe_function +
- '...) instead of ' + single_thread_function +
+ 'Consider using ' + multithread_safe_func +
+ '...) instead of ' + single_thread_func +
'...) for improved thread safety.')
+def CheckVlogArguments(filename, clean_lines, linenum, error):
+ """Checks that VLOG() is only used for defining a logging level.
+
+ For example, VLOG(2) is correct. VLOG(INFO), VLOG(WARNING), VLOG(ERROR), and
+ VLOG(FATAL) are not.
+
+ 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 Search(r'\bVLOG\((INFO|ERROR|WARNING|DFATAL|FATAL)\)', line):
+ error(filename, linenum, 'runtime/vlog', 5,
+ 'VLOG() should be used with numeric verbosity level. '
+ 'Use LOG() if you want symbolic severity levels.')
+
# Matches invalid increment: *count++, which moves pointer instead of
# incrementing a value.
_RE_PATTERN_INVALID_INCREMENT = re.compile(
@@ -1259,17 +1980,95 @@ def CheckInvalidIncrement(filename, clean_lines, linenum, error):
'Changing pointer instead of value (or unused value of operator*).')
-class _ClassInfo(object):
+def IsMacroDefinition(clean_lines, linenum):
+ if Search(r'^#define', clean_lines[linenum]):
+ return True
+
+ if linenum > 0 and Search(r'\\$', clean_lines[linenum - 1]):
+ return True
+
+ return False
+
+
+def IsForwardClassDeclaration(clean_lines, linenum):
+ return Match(r'^\s*(\btemplate\b)*.*class\s+\w+;\s*$', clean_lines[linenum])
+
+
+class _BlockInfo(object):
+ """Stores information about a generic block of code."""
+
+ def __init__(self, seen_open_brace):
+ self.seen_open_brace = seen_open_brace
+ self.open_parentheses = 0
+ self.inline_asm = _NO_ASM
+ self.check_namespace_indentation = False
+
+ def CheckBegin(self, filename, clean_lines, linenum, error):
+ """Run checks that applies to text up to the opening brace.
+
+ This is mostly for checking the text after the class identifier
+ and the "{", usually where the base class is specified. For other
+ blocks, there isn't much to check, so we always pass.
+
+ 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.
+ """
+ pass
+
+ def CheckEnd(self, filename, clean_lines, linenum, error):
+ """Run checks that applies to text after the closing brace.
+
+ This is mostly used for checking end of namespace comments.
+
+ 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.
+ """
+ pass
+
+ def IsBlockInfo(self):
+ """Returns true if this block is a _BlockInfo.
+
+ This is convenient for verifying that an object is an instance of
+ a _BlockInfo, but not an instance of any of the derived classes.
+
+ Returns:
+ True for this class, False for derived classes.
+ """
+ return self.__class__ == _BlockInfo
+
+
+class _ExternCInfo(_BlockInfo):
+ """Stores information about an 'extern "C"' block."""
+
+ def __init__(self):
+ _BlockInfo.__init__(self, True)
+
+
+class _ClassInfo(_BlockInfo):
"""Stores information about a class."""
- def __init__(self, name, clean_lines, linenum):
+ def __init__(self, name, class_or_struct, clean_lines, linenum):
+ _BlockInfo.__init__(self, False)
self.name = name
- self.linenum = linenum
- self.seen_open_brace = False
+ self.starting_linenum = linenum
self.is_derived = False
- self.virtual_method_linenumber = None
- self.has_virtual_destructor = False
- self.brace_depth = 0
+ self.check_namespace_indentation = True
+ if class_or_struct == 'struct':
+ self.access = 'public'
+ self.is_struct = True
+ else:
+ self.access = 'private'
+ self.is_struct = False
+
+ # Remember initial indentation level for this class. Using raw_lines here
+ # instead of elided to account for leading comments.
+ self.class_indent = GetIndentLevel(clean_lines.raw_lines[linenum])
# Try to find the end of the class. This will be confused by things like:
# class A {
@@ -1279,58 +2078,512 @@ class _ClassInfo(object):
self.last_line = 0
depth = 0
for i in range(linenum, clean_lines.NumLines()):
- line = clean_lines.lines[i]
+ line = clean_lines.elided[i]
depth += line.count('{') - line.count('}')
if not depth:
self.last_line = i
break
+ def CheckBegin(self, filename, clean_lines, linenum, error):
+ # Look for a bare ':'
+ if Search('(^|[^:]):($|[^:])', clean_lines.elided[linenum]):
+ self.is_derived = True
+
+ def CheckEnd(self, filename, clean_lines, linenum, error):
+ # If there is a DISALLOW macro, it should appear near the end of
+ # the class.
+ seen_last_thing_in_class = False
+ for i in xrange(linenum - 1, self.starting_linenum, -1):
+ match = Search(
+ r'\b(DISALLOW_COPY_AND_ASSIGN|DISALLOW_IMPLICIT_CONSTRUCTORS)\(' +
+ self.name + r'\)',
+ clean_lines.elided[i])
+ if match:
+ if seen_last_thing_in_class:
+ error(filename, i, 'readability/constructors', 3,
+ match.group(1) + ' should be the last thing in the class')
+ break
-class _ClassState(object):
- """Holds the current state of the parse relating to class declarations.
+ if not Match(r'^\s*$', clean_lines.elided[i]):
+ seen_last_thing_in_class = True
- 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.
- """
+ # Check that closing brace is aligned with beginning of the class.
+ # Only do this if the closing brace is indented by only whitespaces.
+ # This means we will not check single-line class definitions.
+ indent = Match(r'^( *)\}', clean_lines.elided[linenum])
+ if indent and len(indent.group(1)) != self.class_indent:
+ if self.is_struct:
+ parent = 'struct ' + self.name
+ else:
+ parent = 'class ' + self.name
+ error(filename, linenum, 'whitespace/indent', 3,
+ 'Closing brace should be aligned with beginning of %s' % parent)
- def __init__(self):
- self.classinfo_stack = []
- def CheckFinished(self, filename, error):
- """Checks that all classes have been completely parsed.
+class _NamespaceInfo(_BlockInfo):
+ """Stores information about a namespace."""
- 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 __init__(self, name, linenum):
+ _BlockInfo.__init__(self, False)
+ self.name = name or ''
+ self.starting_linenum = linenum
+ self.check_namespace_indentation = True
+ def CheckEnd(self, filename, clean_lines, linenum, error):
+ """Check end of namespace comments."""
+ line = clean_lines.raw_lines[linenum]
-def CheckForNonStandardConstructs(filename, clean_lines, linenum,
- class_state, error):
- """Logs an error if we see certain non-ANSI constructs ignored by gcc-2.
+ # Check how many lines is enclosed in this namespace. Don't issue
+ # warning for missing namespace comments if there aren't enough
+ # lines. However, do apply checks if there is already an end of
+ # namespace comment and it's incorrect.
+ #
+ # TODO(unknown): We always want to check end of namespace comments
+ # if a namespace is large, but sometimes we also want to apply the
+ # check if a short namespace contained nontrivial things (something
+ # other than forward declarations). There is currently no logic on
+ # deciding what these nontrivial things are, so this check is
+ # triggered by namespace size only, which works most of the time.
+ if (linenum - self.starting_linenum < 10
+ and not Match(r'};*\s*(//|/\*).*\bnamespace\b', line)):
+ return
- 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.
+ # Look for matching comment at end of namespace.
+ #
+ # Note that we accept C style "/* */" comments for terminating
+ # namespaces, so that code that terminate namespaces inside
+ # preprocessor macros can be cpplint clean.
+ #
+ # We also accept stuff like "// end of namespace <name>." with the
+ # period at the end.
+ #
+ # Besides these, we don't accept anything else, otherwise we might
+ # get false negatives when existing comment is a substring of the
+ # expected namespace.
+ if self.name:
+ # Named namespace
+ if not Match((r'};*\s*(//|/\*).*\bnamespace\s+' + re.escape(self.name) +
+ r'[\*/\.\\\s]*$'),
+ line):
+ error(filename, linenum, 'readability/namespace', 5,
+ 'Namespace should be terminated with "// namespace %s"' %
+ self.name)
+ else:
+ # Anonymous namespace
+ if not Match(r'};*\s*(//|/\*).*\bnamespace[\*/\.\\\s]*$', line):
+ # If "// namespace anonymous" or "// anonymous namespace (more text)",
+ # mention "// anonymous namespace" as an acceptable form
+ if Match(r'}.*\b(namespace anonymous|anonymous namespace)\b', line):
+ error(filename, linenum, 'readability/namespace', 5,
+ 'Anonymous namespace should be terminated with "// namespace"'
+ ' or "// anonymous namespace"')
+ else:
+ error(filename, linenum, 'readability/namespace', 5,
+ 'Anonymous namespace should be terminated with "// namespace"')
+
+
+class _PreprocessorInfo(object):
+ """Stores checkpoints of nesting stacks when #if/#else is seen."""
+
+ def __init__(self, stack_before_if):
+ # The entire nesting stack before #if
+ self.stack_before_if = stack_before_if
+
+ # The entire nesting stack up to #else
+ self.stack_before_else = []
+
+ # Whether we have already seen #else or #elif
+ self.seen_else = False
+
+
+class NestingState(object):
+ """Holds states related to parsing braces."""
+
+ def __init__(self):
+ # Stack for tracking all braces. An object is pushed whenever we
+ # see a "{", and popped when we see a "}". Only 3 types of
+ # objects are possible:
+ # - _ClassInfo: a class or struct.
+ # - _NamespaceInfo: a namespace.
+ # - _BlockInfo: some other type of block.
+ self.stack = []
+
+ # Top of the previous stack before each Update().
+ #
+ # Because the nesting_stack is updated at the end of each line, we
+ # had to do some convoluted checks to find out what is the current
+ # scope at the beginning of the line. This check is simplified by
+ # saving the previous top of nesting stack.
+ #
+ # We could save the full stack, but we only need the top. Copying
+ # the full nesting stack would slow down cpplint by ~10%.
+ self.previous_stack_top = []
+
+ # Stack of _PreprocessorInfo objects.
+ self.pp_stack = []
+
+ def SeenOpenBrace(self):
+ """Check if we have seen the opening brace for the innermost block.
+
+ Returns:
+ True if we have seen the opening brace, False if the innermost
+ block is still expecting an opening brace.
+ """
+ return (not self.stack) or self.stack[-1].seen_open_brace
+
+ def InNamespaceBody(self):
+ """Check if we are currently one level inside a namespace body.
+
+ Returns:
+ True if top of the stack is a namespace block, False otherwise.
+ """
+ return self.stack and isinstance(self.stack[-1], _NamespaceInfo)
+
+ def InExternC(self):
+ """Check if we are currently one level inside an 'extern "C"' block.
+
+ Returns:
+ True if top of the stack is an extern block, False otherwise.
+ """
+ return self.stack and isinstance(self.stack[-1], _ExternCInfo)
+
+ def InClassDeclaration(self):
+ """Check if we are currently one level inside a class or struct declaration.
+
+ Returns:
+ True if top of the stack is a class/struct, False otherwise.
+ """
+ return self.stack and isinstance(self.stack[-1], _ClassInfo)
+
+ def InAsmBlock(self):
+ """Check if we are currently one level inside an inline ASM block.
+
+ Returns:
+ True if the top of the stack is a block containing inline ASM.
+ """
+ return self.stack and self.stack[-1].inline_asm != _NO_ASM
+
+ def InTemplateArgumentList(self, clean_lines, linenum, pos):
+ """Check if current position is inside template argument list.
+
+ Args:
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ pos: position just after the suspected template argument.
+ Returns:
+ True if (linenum, pos) is inside template arguments.
+ """
+ while linenum < clean_lines.NumLines():
+ # Find the earliest character that might indicate a template argument
+ line = clean_lines.elided[linenum]
+ match = Match(r'^[^{};=\[\]\.<>]*(.)', line[pos:])
+ if not match:
+ linenum += 1
+ pos = 0
+ continue
+ token = match.group(1)
+ pos += len(match.group(0))
+
+ # These things do not look like template argument list:
+ # class Suspect {
+ # class Suspect x; }
+ if token in ('{', '}', ';'): return False
+
+ # These things look like template argument list:
+ # template <class Suspect>
+ # template <class Suspect = default_value>
+ # template <class Suspect[]>
+ # template <class Suspect...>
+ if token in ('>', '=', '[', ']', '.'): return True
+
+ # Check if token is an unmatched '<'.
+ # If not, move on to the next character.
+ if token != '<':
+ pos += 1
+ if pos >= len(line):
+ linenum += 1
+ pos = 0
+ continue
+
+ # We can't be sure if we just find a single '<', and need to
+ # find the matching '>'.
+ (_, end_line, end_pos) = CloseExpression(clean_lines, linenum, pos - 1)
+ if end_pos < 0:
+ # Not sure if template argument list or syntax error in file
+ return False
+ linenum = end_line
+ pos = end_pos
+ return False
+
+ def UpdatePreprocessor(self, line):
+ """Update preprocessor stack.
+
+ We need to handle preprocessors due to classes like this:
+ #ifdef SWIG
+ struct ResultDetailsPageElementExtensionPoint {
+ #else
+ struct ResultDetailsPageElementExtensionPoint : public Extension {
+ #endif
+
+ We make the following assumptions (good enough for most files):
+ - Preprocessor condition evaluates to true from #if up to first
+ #else/#elif/#endif.
+
+ - Preprocessor condition evaluates to false from #else/#elif up
+ to #endif. We still perform lint checks on these lines, but
+ these do not affect nesting stack.
+
+ Args:
+ line: current line to check.
+ """
+ if Match(r'^\s*#\s*(if|ifdef|ifndef)\b', line):
+ # Beginning of #if block, save the nesting stack here. The saved
+ # stack will allow us to restore the parsing state in the #else case.
+ self.pp_stack.append(_PreprocessorInfo(copy.deepcopy(self.stack)))
+ elif Match(r'^\s*#\s*(else|elif)\b', line):
+ # Beginning of #else block
+ if self.pp_stack:
+ if not self.pp_stack[-1].seen_else:
+ # This is the first #else or #elif block. Remember the
+ # whole nesting stack up to this point. This is what we
+ # keep after the #endif.
+ self.pp_stack[-1].seen_else = True
+ self.pp_stack[-1].stack_before_else = copy.deepcopy(self.stack)
+
+ # Restore the stack to how it was before the #if
+ self.stack = copy.deepcopy(self.pp_stack[-1].stack_before_if)
+ else:
+ # TODO(unknown): unexpected #else, issue warning?
+ pass
+ elif Match(r'^\s*#\s*endif\b', line):
+ # End of #if or #else blocks.
+ if self.pp_stack:
+ # If we saw an #else, we will need to restore the nesting
+ # stack to its former state before the #else, otherwise we
+ # will just continue from where we left off.
+ if self.pp_stack[-1].seen_else:
+ # Here we can just use a shallow copy since we are the last
+ # reference to it.
+ self.stack = self.pp_stack[-1].stack_before_else
+ # Drop the corresponding #if
+ self.pp_stack.pop()
+ else:
+ # TODO(unknown): unexpected #endif, issue warning?
+ pass
+
+ # TODO(unknown): Update() is too long, but we will refactor later.
+ def Update(self, filename, clean_lines, linenum, error):
+ """Update nesting state with current 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]
+
+ # Remember top of the previous nesting stack.
+ #
+ # The stack is always pushed/popped and not modified in place, so
+ # we can just do a shallow copy instead of copy.deepcopy. Using
+ # deepcopy would slow down cpplint by ~28%.
+ if self.stack:
+ self.previous_stack_top = self.stack[-1]
+ else:
+ self.previous_stack_top = None
+
+ # Update pp_stack
+ self.UpdatePreprocessor(line)
+
+ # Count parentheses. This is to avoid adding struct arguments to
+ # the nesting stack.
+ if self.stack:
+ inner_block = self.stack[-1]
+ depth_change = line.count('(') - line.count(')')
+ inner_block.open_parentheses += depth_change
+
+ # Also check if we are starting or ending an inline assembly block.
+ if inner_block.inline_asm in (_NO_ASM, _END_ASM):
+ if (depth_change != 0 and
+ inner_block.open_parentheses == 1 and
+ _MATCH_ASM.match(line)):
+ # Enter assembly block
+ inner_block.inline_asm = _INSIDE_ASM
+ else:
+ # Not entering assembly block. If previous line was _END_ASM,
+ # we will now shift to _NO_ASM state.
+ inner_block.inline_asm = _NO_ASM
+ elif (inner_block.inline_asm == _INSIDE_ASM and
+ inner_block.open_parentheses == 0):
+ # Exit assembly block
+ inner_block.inline_asm = _END_ASM
+
+ # Consume namespace declaration at the beginning of the line. Do
+ # this in a loop so that we catch same line declarations like this:
+ # namespace proto2 { namespace bridge { class MessageSet; } }
+ while True:
+ # Match start of namespace. The "\b\s*" below catches namespace
+ # declarations even if it weren't followed by a whitespace, this
+ # is so that we don't confuse our namespace checker. The
+ # missing spaces will be flagged by CheckSpacing.
+ namespace_decl_match = Match(r'^\s*namespace\b\s*([:\w]+)?(.*)$', line)
+ if not namespace_decl_match:
+ break
+
+ new_namespace = _NamespaceInfo(namespace_decl_match.group(1), linenum)
+ self.stack.append(new_namespace)
+
+ line = namespace_decl_match.group(2)
+ if line.find('{') != -1:
+ new_namespace.seen_open_brace = True
+ line = line[line.find('{') + 1:]
+
+ # Look for a class declaration in whatever is left of the line
+ # after parsing namespaces. The regexp accounts for decorated classes
+ # such as in:
+ # class LOCKABLE API Object {
+ # };
+ class_decl_match = Match(
+ r'^(\s*(?:template\s*<[\w\s<>,:]*>\s*)?'
+ r'(class|struct)\s+(?:[A-Z_]+\s+)*(\w+(?:::\w+)*))'
+ r'(.*)$', line)
+ if (class_decl_match and
+ (not self.stack or self.stack[-1].open_parentheses == 0)):
+ # We do not want to accept classes that are actually template arguments:
+ # template <class Ignore1,
+ # class Ignore2 = Default<Args>,
+ # template <Args> class Ignore3>
+ # void Function() {};
+ #
+ # To avoid template argument cases, we scan forward and look for
+ # an unmatched '>'. If we see one, assume we are inside a
+ # template argument list.
+ end_declaration = len(class_decl_match.group(1))
+ if not self.InTemplateArgumentList(clean_lines, linenum, end_declaration):
+ self.stack.append(_ClassInfo(
+ class_decl_match.group(3), class_decl_match.group(2),
+ clean_lines, linenum))
+ line = class_decl_match.group(4)
+
+ # If we have not yet seen the opening brace for the innermost block,
+ # run checks here.
+ if not self.SeenOpenBrace():
+ self.stack[-1].CheckBegin(filename, clean_lines, linenum, error)
+
+ # Update access control if we are inside a class/struct
+ if self.stack and isinstance(self.stack[-1], _ClassInfo):
+ classinfo = self.stack[-1]
+ access_match = Match(
+ r'^(.*)\b(public|private|protected|signals)(\s+(?:slots\s*)?)?'
+ r':(?:[^:]|$)',
+ line)
+ if access_match:
+ classinfo.access = access_match.group(2)
+
+ # Check that access keywords are indented +1 space. Skip this
+ # check if the keywords are not preceded by whitespaces.
+ indent = access_match.group(1)
+ if (len(indent) != classinfo.class_indent + 1 and
+ Match(r'^\s*$', indent)):
+ if classinfo.is_struct:
+ parent = 'struct ' + classinfo.name
+ else:
+ parent = 'class ' + classinfo.name
+ slots = ''
+ if access_match.group(3):
+ slots = access_match.group(3)
+ error(filename, linenum, 'whitespace/indent', 3,
+ '%s%s: should be indented +1 space inside %s' % (
+ access_match.group(2), slots, parent))
+
+ # Consume braces or semicolons from what's left of the line
+ while True:
+ # Match first brace, semicolon, or closed parenthesis.
+ matched = Match(r'^[^{;)}]*([{;)}])(.*)$', line)
+ if not matched:
+ break
+
+ token = matched.group(1)
+ if token == '{':
+ # If namespace or class hasn't seen a opening brace yet, mark
+ # namespace/class head as complete. Push a new block onto the
+ # stack otherwise.
+ if not self.SeenOpenBrace():
+ self.stack[-1].seen_open_brace = True
+ elif Match(r'^extern\s*"[^"]*"\s*\{', line):
+ self.stack.append(_ExternCInfo())
+ else:
+ self.stack.append(_BlockInfo(True))
+ if _MATCH_ASM.match(line):
+ self.stack[-1].inline_asm = _BLOCK_ASM
+
+ elif token == ';' or token == ')':
+ # If we haven't seen an opening brace yet, but we already saw
+ # a semicolon, this is probably a forward declaration. Pop
+ # the stack for these.
+ #
+ # Similarly, if we haven't seen an opening brace yet, but we
+ # already saw a closing parenthesis, then these are probably
+ # function arguments with extra "class" or "struct" keywords.
+ # Also pop these stack for these.
+ if not self.SeenOpenBrace():
+ self.stack.pop()
+ else: # token == '}'
+ # Perform end of block checks and pop the stack.
+ if self.stack:
+ self.stack[-1].CheckEnd(filename, clean_lines, linenum, error)
+ self.stack.pop()
+ line = matched.group(2)
+
+ def InnermostClass(self):
+ """Get class info on the top of the stack.
+
+ Returns:
+ A _ClassInfo object if we are inside a class, or None otherwise.
+ """
+ for i in range(len(self.stack), 0, -1):
+ classinfo = self.stack[i - 1]
+ if isinstance(classinfo, _ClassInfo):
+ return classinfo
+ return None
+
+ def CheckCompletedBlocks(self, filename, error):
+ """Checks that all classes and namespaces 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.
+ """
+ # 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.
+ for obj in self.stack:
+ if isinstance(obj, _ClassInfo):
+ error(filename, obj.starting_linenum, 'build/class', 5,
+ 'Failed to find complete declaration of class %s' %
+ obj.name)
+ elif isinstance(obj, _NamespaceInfo):
+ error(filename, obj.starting_linenum, 'build/namespaces', 5,
+ 'Failed to find complete declaration of namespace %s' %
+ obj.name)
+
+
+def CheckForNonStandardConstructs(filename, clean_lines, linenum,
+ nesting_state, error):
+ r"""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
@@ -1340,8 +2593,8 @@ def CheckForNonStandardConstructs(filename, clean_lines, linenum,
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.
+ nesting_state: A NestingState instance which maintains information about
+ the current stack of nested blocks being parsed.
error: A callable to which errors are reported, which takes 4 arguments:
filename, line number, error level, and message
"""
@@ -1370,7 +2623,7 @@ def CheckForNonStandardConstructs(filename, clean_lines, 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',
+ r'\s+(register|static|extern|typedef)\b',
line):
error(filename, linenum, 'build/storage_class', 5,
'Storage class (static, extern, typedef, etc) should be first.')
@@ -1400,100 +2653,97 @@ def CheckForNonStandardConstructs(filename, clean_lines, linenum,
'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:
+ # Everything else in this function operates on class declarations.
+ # Return early if the top of the nesting stack is not a class, or if
+ # the class head is not completed yet.
+ classinfo = nesting_state.InnermostClass()
+ if not classinfo or not classinfo.seen_open_brace:
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
+ # Technically a valid construct, but against style. Also look for
+ # non-single-argument constructors which are also technically valid, but
+ # strongly suggest something is wrong.
+ explicit_constructor_match = Match(
+ r'\s+(?:inline\s+)?(explicit\s+)?(?:inline\s+)?%s\s*'
+ r'\(((?:[^()]|\([^()]*\))*)\)'
+ % re.escape(base_classname),
+ line)
+
+ if explicit_constructor_match:
+ is_marked_explicit = explicit_constructor_match.group(1)
+
+ if not explicit_constructor_match.group(2):
+ constructor_args = []
+ else:
+ constructor_args = explicit_constructor_match.group(2).split(',')
+
+ # collapse arguments so that commas in template parameter lists and function
+ # argument parameter lists don't split arguments in two
+ i = 0
+ while i < len(constructor_args):
+ constructor_arg = constructor_args[i]
+ while (constructor_arg.count('<') > constructor_arg.count('>') or
+ constructor_arg.count('(') > constructor_arg.count(')')):
+ constructor_arg += ',' + constructor_args[i + 1]
+ del constructor_args[i + 1]
+ constructor_args[i] = constructor_arg
+ i += 1
+
+ defaulted_args = [arg for arg in constructor_args if '=' in arg]
+ noarg_constructor = (not constructor_args or # empty arg list
+ # 'void' arg specifier
+ (len(constructor_args) == 1 and
+ constructor_args[0].strip() == 'void'))
+ onearg_constructor = ((len(constructor_args) == 1 and # exactly one arg
+ not noarg_constructor) or
+ # all but at most one arg defaulted
+ (len(constructor_args) >= 1 and
+ not noarg_constructor and
+ len(defaulted_args) >= len(constructor_args) - 1))
+ initializer_list_constructor = bool(
+ onearg_constructor and
+ Search(r'\bstd\s*::\s*initializer_list\b', constructor_args[0]))
+ copy_constructor = bool(
+ onearg_constructor and
+ Match(r'(const\s+)?%s(\s*<[^>]*>)?(\s+const)?\s*(?:<\w+>\s*)?&'
+ % re.escape(base_classname), constructor_args[0].strip()))
+
+ if (not is_marked_explicit and
+ onearg_constructor and
+ not initializer_list_constructor and
+ not copy_constructor):
+ if defaulted_args:
+ error(filename, linenum, 'runtime/explicit', 5,
+ 'Constructors callable with one argument '
+ 'should be marked explicit.')
+ else:
+ error(filename, linenum, 'runtime/explicit', 5,
+ 'Single-parameter constructors should be marked explicit.')
+ elif is_marked_explicit and not onearg_constructor:
+ if noarg_constructor:
+ error(filename, linenum, 'runtime/explicit', 5,
+ 'Zero-parameter constructors should not be marked explicit.')
+ else:
+ error(filename, linenum, 'runtime/explicit', 0,
+ 'Constructors that require multiple arguments '
+ 'should not be marked explicit.')
-def CheckSpacingForFunctionCall(filename, line, linenum, error):
+def CheckSpacingForFunctionCall(filename, clean_lines, 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.
+ 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]
# Since function calls often occur inside if/for/while/switch
# expressions - which have their own, more liberal conventions - we
@@ -1523,7 +2773,8 @@ def CheckSpacingForFunctionCall(filename, line, linenum, error):
# 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
+ not Search(r'\b(if|for|while|switch|return|new|delete|catch|sizeof)\b',
+ fncall) and
# Ignore pointers/references to functions.
not Search(r' \([^)]+\)\([^)]*(\)|,$)', fncall) and
# Ignore pointers/references to arrays.
@@ -1535,9 +2786,17 @@ def CheckSpacingForFunctionCall(filename, line, linenum, error):
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')
+ not Search(r'#\s*define|typedef|using\s+\w+\s*=', fncall) and
+ not Search(r'\w\s+\((\w+::)*\*\w+\)\(', fncall) and
+ not Search(r'\bcase\s+\(', fncall)):
+ # TODO(unknown): Space after an operator function seem to be a common
+ # error, silence those for now by restricting them to highest verbosity.
+ if Search(r'\boperator_*\b', line):
+ error(filename, linenum, 'whitespace/parens', 0,
+ 'Extra space before ( in function call')
+ else:
+ 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):
@@ -1566,6 +2825,20 @@ def IsBlankLine(line):
return not line or line.isspace()
+def CheckForNamespaceIndentation(filename, nesting_state, clean_lines, line,
+ error):
+ is_namespace_indent_item = (
+ len(nesting_state.stack) > 1 and
+ nesting_state.stack[-1].check_namespace_indentation and
+ isinstance(nesting_state.previous_stack_top, _NamespaceInfo) and
+ nesting_state.previous_stack_top == nesting_state.stack[-2])
+
+ if ShouldCheckNamespaceIndentation(nesting_state, is_namespace_indent_item,
+ clean_lines.elided, line):
+ CheckItemIndentationInNamespace(filename, clean_lines.elided,
+ line, error)
+
+
def CheckForFunctionLengths(filename, clean_lines, linenum,
function_state, error):
"""Reports for long function bodies.
@@ -1591,8 +2864,6 @@ def CheckForFunctionLengths(filename, clean_lines, linenum,
"""
lines = clean_lines.lines
line = lines[linenum]
- raw = clean_lines.raw_lines
- raw_line = raw[linenum]
joined_line = ''
starting_func = False
@@ -1639,37 +2910,93 @@ def CheckForFunctionLengths(filename, clean_lines, linenum,
_RE_PATTERN_TODO = re.compile(r'^//(\s*)TODO(\(.+?\))?:?(\s|$)?')
-def CheckComment(comment, filename, linenum, error):
- """Checks for common mistakes in TODO comments.
+def CheckComment(line, filename, linenum, next_line_start, error):
+ """Checks for common mistakes in comments.
Args:
- comment: The text of the comment from the line in question.
+ line: The line in question.
filename: The name of the current file.
linenum: The number of the line to check.
+ next_line_start: The first non-whitespace column of the next line.
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):
+ commentpos = line.find('//')
+ if commentpos != -1:
+ # Check if the // may be in quotes. If so, ignore it
+ # Comparisons made explicit for clarity -- pylint: disable=g-explicit-bool-comparison
+ 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'^.*{ *//', line) and next_line_start == commentpos) 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')
+
+ # Checks for common mistakes in TODO comments.
+ comment = line[commentpos:]
+ 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=g-explicit-bool-comparison
+ if middle_whitespace != ' ' and middle_whitespace != '':
+ error(filename, linenum, 'whitespace/todo', 2,
+ 'TODO(my_username) should be followed by a space')
+
+ # If the comment contains an alphanumeric character, there
+ # should be a space somewhere between it and the // unless
+ # it's a /// or //! Doxygen comment.
+ if (Match(r'//[^ ]*\w', comment) and
+ not Match(r'(///|//\!)(\s+|$)', comment)):
+ error(filename, linenum, 'whitespace/comments', 4,
+ 'Should have a space between // and comment')
+
+
+def CheckAccess(filename, clean_lines, linenum, nesting_state, error):
+ """Checks for improper use of DISALLOW* 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.
+ nesting_state: A NestingState instance which maintains information about
+ the current stack of nested blocks being parsed.
+ error: The function to call with any errors found.
+ """
+ line = clean_lines.elided[linenum] # get rid of comments and strings
+
+ matched = Match((r'\s*(DISALLOW_COPY_AND_ASSIGN|'
+ r'DISALLOW_IMPLICIT_CONSTRUCTORS)'), line)
+ if not matched:
+ return
+ if nesting_state.stack and isinstance(nesting_state.stack[-1], _ClassInfo):
+ if nesting_state.stack[-1].access != 'private':
+ error(filename, linenum, 'readability/constructors', 3,
+ '%s must be in the private: section' % matched.group(1))
+
+ else:
+ # Found DISALLOW* macro outside a class declaration, or perhaps it
+ # was used inside a function when it should have been part of the
+ # class declaration. We could issue a warning here, but it
+ # probably resulted in a compiler error already.
+ pass
+
+
+def CheckSpacing(filename, clean_lines, linenum, nesting_state, error):
"""Checks for the correctness of various spacing issues in the code.
Things we check for: spaces around operators, spaces after
@@ -1682,16 +3009,35 @@ def CheckSpacing(filename, clean_lines, linenum, error):
filename: The name of the current file.
clean_lines: A CleansedLines instance containing the file.
linenum: The number of the line to check.
+ nesting_state: A NestingState instance which maintains information about
+ the current stack of nested blocks being parsed.
error: The function to call with any errors found.
"""
- raw = clean_lines.raw_lines
+ # Don't use "elided" lines here, otherwise we can't check commented lines.
+ # Don't want to use "raw" either, because we don't want to check inside C++11
+ # raw strings,
+ raw = clean_lines.lines_without_raw_strings
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):
+ #
+ # Skip all the blank line checks if we are immediately inside a
+ # namespace body. In other words, don't issue blank line warnings
+ # for this block:
+ # namespace {
+ #
+ # }
+ #
+ # A warning about missing end of namespace comments will be issued instead.
+ #
+ # Also skip blank line checks for 'extern "C"' blocks, which are formatted
+ # like namespaces.
+ if (IsBlankLine(line) and
+ not nesting_state.InNamespaceBody() and
+ not nesting_state.InExternC()):
elided = clean_lines.elided
prev_line = elided[linenum - 1]
prevbrace = prev_line.rfind('{')
@@ -1699,8 +3045,7 @@ def CheckSpacing(filename, clean_lines, linenum, error):
# 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):
+ if prevbrace != -1 and prev_line[prevbrace:].find('}') == -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
@@ -1731,13 +3076,9 @@ def CheckSpacing(filename, clean_lines, linenum, error):
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
+ 'Redundant blank line at the start of a code block '
+ 'should be deleted.')
+ # 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
@@ -1749,59 +3090,74 @@ def CheckSpacing(filename, clean_lines, linenum, error):
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?')
+ 'Redundant blank line at the end of a code block '
+ 'should be deleted.')
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)
+ # Next, check comments
+ next_line_start = 0
+ if linenum + 1 < clean_lines.NumLines():
+ next_line = raw[linenum + 1]
+ next_line_start = len(next_line) - len(next_line.lstrip())
+ CheckComment(line, filename, linenum, next_line_start, error)
- line = clean_lines.elided[linenum] # get rid of comments and strings
+ # get rid of comments and strings
+ line = clean_lines.elided[linenum]
+
+ # You shouldn't have spaces before your brackets, except maybe after
+ # 'delete []' or 'return []() {};'
+ if Search(r'\w\s+\[', line) and not Search(r'(?:delete|return)\s+\[', line):
+ error(filename, linenum, 'whitespace/braces', 5,
+ 'Extra space before [')
+
+ # In range-based for, we wanted spaces before and after the colon, but
+ # not around "::" tokens that might appear.
+ if (Search(r'for *\(.*[^:]:[^: ]', line) or
+ Search(r'for *\(.*[^: ]:[^:]', line)):
+ error(filename, linenum, 'whitespace/forcolon', 2,
+ 'Missing space around colon in range-based for loop')
+
+
+def CheckOperatorSpacing(filename, clean_lines, linenum, error):
+ """Checks for horizontal spacing around operators.
+
+ 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]
- # Don't try to do spacing checks for operator methods
- line = re.sub(r'operator(==|!=|<|<<|<=|>=|>>|>)\(', 'operator\(', line)
+ # Don't try to do spacing checks for operator methods. Do this by
+ # replacing the troublesome characters with something else,
+ # preserving column position for all other characters.
+ #
+ # The replacement is done repeatedly to avoid false positives from
+ # operators that call operators.
+ while True:
+ match = Match(r'^(.*\boperator\b)(\S+)(\s*\(.*)$', line)
+ if match:
+ line = match.group(1) + ('_' * len(match.group(2))) + match.group(3)
+ else:
+ break
# 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):
+ if ((Search(r'[\w.]=', line) or
+ Search(r'=[\w.]', line))
+ and not Search(r'\b(if|while|for) ', line)
+ # Operators taken from [lex.operators] in C++11 standard.
+ and not Search(r'(>=|<=|==|!=|&=|\^=|\|=|\+=|\*=|\/=|\%=)', line)
+ and not Search(r'operator=', line)):
error(filename, linenum, 'whitespace/operators', 4,
'Missing spaces around =')
@@ -1810,26 +3166,71 @@ def CheckSpacing(filename, clean_lines, linenum, error):
# 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)
+ #
+ # Check <= and >= first to avoid false positives with < and >, then
+ # check non-include lines for spacing around < and >.
+ #
+ # If the operator is followed by a comma, assume it's be used in a
+ # macro context and don't do any checks. This avoids false
+ # positives.
+ #
+ # Note that && is not included here. Those are checked separately
+ # in CheckRValueReference
+ 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
+ elif not Match(r'#.*include', line):
+ # Look for < that is not surrounded by spaces. This is only
+ # triggered if both sides are missing spaces, even though
+ # technically should should flag if at least one side is missing a
+ # space. This is done to avoid some false positives with shifts.
+ match = Match(r'^(.*[^\s<])<[^\s=<,]', line)
+ if match:
+ (_, _, end_pos) = CloseExpression(
+ clean_lines, linenum, len(match.group(1)))
+ if end_pos <= -1:
+ error(filename, linenum, 'whitespace/operators', 3,
+ 'Missing spaces around <')
+
+ # Look for > that is not surrounded by spaces. Similar to the
+ # above, we only trigger if both sides are missing spaces to avoid
+ # false positives with shifts.
+ match = Match(r'^(.*[^-\s>])>[^\s=>,]', line)
+ if match:
+ (_, _, start_pos) = ReverseCloseExpression(
+ clean_lines, linenum, len(match.group(1)))
+ if start_pos <= -1:
+ error(filename, linenum, 'whitespace/operators', 3,
+ 'Missing spaces around >')
+
+ # We allow no-spaces around << 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)
+ #
+ # We also allow operators following an opening parenthesis, since
+ # those tend to be macros that deal with operators.
+ match = Search(r'(operator|[^\s(<])(?:L|UL|ULL|l|ul|ull)?<<([^\s,=<])', line)
+ if (match and not (match.group(1).isdigit() and match.group(2).isdigit()) and
+ not (match.group(1) == 'operator' and match.group(2) == ';')):
+ error(filename, linenum, 'whitespace/operators', 3,
+ 'Missing spaces around <<')
+
+ # We allow no-spaces around >> for almost anything. This is because
+ # C++11 allows ">>" to close nested templates, which accounts for
+ # most cases when ">>" is not followed by a space.
+ #
+ # We still warn on ">>" followed by alpha character, because that is
+ # likely due to ">>" being used for right shifts, e.g.:
+ # value >> alpha
+ #
+ # When ">>" is used to close templates, the alphanumeric letter that
+ # follows would be part of an identifier, and there should still be
+ # a space separating the template type and the identifier.
+ # type<type<type>> alpha
+ match = Search(r'>>[a-zA-Z_]', line)
if match:
error(filename, linenum, 'whitespace/operators', 3,
- 'Missing spaces around %s' % match.group(1))
+ 'Missing spaces around >>')
# There shouldn't be space around unary operators
match = Search(r'(!\s|~\s|[\s]--[\s;]|[\s]\+\+[\s;])', line)
@@ -1837,7 +3238,19 @@ def CheckSpacing(filename, clean_lines, linenum, error):
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
+
+def CheckParenthesisSpacing(filename, clean_lines, linenum, error):
+ """Checks for horizontal spacing around parentheses.
+
+ 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]
+
+ # 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,
@@ -1858,13 +3271,36 @@ def CheckSpacing(filename, clean_lines, linenum, error):
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]:
+ if len(match.group(2)) not in [0, 1]:
error(filename, linenum, 'whitespace/parens', 5,
'Should have zero or one spaces inside ( and ) in %s' %
match.group(1))
+
+def CheckCommaSpacing(filename, clean_lines, linenum, error):
+ """Checks for horizontal spacing near commas and semicolons.
+
+ 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.lines_without_raw_strings
+ line = clean_lines.elided[linenum]
+
# You should always have a space after a comma (either as fn arg or operator)
- if Search(r',[^\s]', line):
+ #
+ # This does not apply when the non-space character following the
+ # comma is another comma, since the only time when that happens is
+ # for empty macro arguments.
+ #
+ # We run this check in two passes: first pass on elided lines to
+ # verify that lines contain missing whitespaces, second pass on raw
+ # lines to confirm that those missing whitespaces are not due to
+ # elided comments.
+ if (Search(r',[^,\s]', ReplaceAll(r'\boperator\s*,\s*\(', 'F(', line)) and
+ Search(r',[^,\s]', raw[linenum])):
error(filename, linenum, 'whitespace/comma', 3,
'Missing space after ,')
@@ -1876,60 +3312,518 @@ def CheckSpacing(filename, clean_lines, linenum, error):
error(filename, linenum, 'whitespace/semicolon', 3,
'Missing space after ;')
- # Next we will look for issues with function calls.
- CheckSpacingForFunctionCall(filename, line, linenum, error)
+
+def CheckBracesSpacing(filename, clean_lines, linenum, error):
+ """Checks for horizontal spacing near commas.
+
+ 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]
# 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 {')
+ match = Match(r'^(.*[^ ({>]){', line)
+ if match:
+ # Try a bit harder to check for brace initialization. This
+ # happens in one of the following forms:
+ # Constructor() : initializer_list_{} { ... }
+ # Constructor{}.MemberFunction()
+ # Type variable{};
+ # FunctionCall(type{}, ...);
+ # LastArgument(..., type{});
+ # LOG(INFO) << type{} << " ...";
+ # map_of_type[{...}] = ...;
+ # ternary = expr ? new type{} : nullptr;
+ # OuterTemplate<InnerTemplateConstructor<Type>{}>
+ #
+ # We check for the character following the closing brace, and
+ # silence the warning if it's one of those listed above, i.e.
+ # "{.;,)<>]:".
+ #
+ # To account for nested initializer list, we allow any number of
+ # closing braces up to "{;,)<". We can't simply silence the
+ # warning on first sight of closing brace, because that would
+ # cause false negatives for things that are not initializer lists.
+ # Silence this: But not this:
+ # Outer{ if (...) {
+ # Inner{...} if (...){ // Missing space before {
+ # }; }
+ #
+ # There is a false negative with this approach if people inserted
+ # spurious semicolons, e.g. "if (cond){};", but we will catch the
+ # spurious semicolon with a separate check.
+ (endline, endlinenum, endpos) = CloseExpression(
+ clean_lines, linenum, len(match.group(1)))
+ trailing_text = ''
+ if endpos > -1:
+ trailing_text = endline[endpos:]
+ for offset in xrange(endlinenum + 1,
+ min(endlinenum + 3, clean_lines.NumLines() - 1)):
+ trailing_text += clean_lines.elided[offset]
+ if not Match(r'^[\s}]*[{.;,)<>\]:]', trailing_text):
+ 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.')
+ '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.')
+ '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.')
+ 'statement, use {} instead.')
-def CheckSectionSpacing(filename, clean_lines, class_info, linenum, error):
- """Checks for additional blank line issues related to sections.
+def IsDecltype(clean_lines, linenum, column):
+ """Check if the token ending on (linenum, column) is decltype().
- Currently the only thing checked here is blank line before protected/private.
+ Args:
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: the number of the line to check.
+ column: end column of the token to check.
+ Returns:
+ True if this token is decltype() expression, False otherwise.
+ """
+ (text, _, start_col) = ReverseCloseExpression(clean_lines, linenum, column)
+ if start_col < 0:
+ return False
+ if Search(r'\bdecltype\s*$', text[0:start_col]):
+ return True
+ return False
+
+
+def IsTemplateParameterList(clean_lines, linenum, column):
+ """Check if the token ending on (linenum, column) is the end of template<>.
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.
+ linenum: the number of the line to check.
+ column: end column of the token to check.
+ Returns:
+ True if this token is end of a template parameter list, False otherwise.
"""
- # 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
+ (_, startline, startpos) = ReverseCloseExpression(
+ clean_lines, linenum, column)
+ if (startpos > -1 and
+ Search(r'\btemplate\s*$', clean_lines.elided[startline][0:startpos])):
+ return True
+ return False
+
+
+def IsRValueType(typenames, clean_lines, nesting_state, linenum, column):
+ """Check if the token ending on (linenum, column) is a type.
+
+ Assumes that text to the right of the column is "&&" or a function
+ name.
+
+ Args:
+ typenames: set of type names from template-argument-list.
+ clean_lines: A CleansedLines instance containing the file.
+ nesting_state: A NestingState instance which maintains information about
+ the current stack of nested blocks being parsed.
+ linenum: the number of the line to check.
+ column: end column of the token to check.
+ Returns:
+ True if this token is a type, False if we are not sure.
+ """
+ prefix = clean_lines.elided[linenum][0:column]
+
+ # Get one word to the left. If we failed to do so, this is most
+ # likely not a type, since it's unlikely that the type name and "&&"
+ # would be split across multiple lines.
+ match = Match(r'^(.*)(\b\w+|[>*)&])\s*$', prefix)
+ if not match:
+ return False
+
+ # Check text following the token. If it's "&&>" or "&&," or "&&...", it's
+ # most likely a rvalue reference used inside a template.
+ suffix = clean_lines.elided[linenum][column:]
+ if Match(r'&&\s*(?:[>,]|\.\.\.)', suffix):
+ return True
+
+ # Check for known types and end of templates:
+ # int&& variable
+ # vector<int>&& variable
+ #
+ # Because this function is called recursively, we also need to
+ # recognize pointer and reference types:
+ # int* Function()
+ # int& Function()
+ if (match.group(2) in typenames or
+ match.group(2) in ['char', 'char16_t', 'char32_t', 'wchar_t', 'bool',
+ 'short', 'int', 'long', 'signed', 'unsigned',
+ 'float', 'double', 'void', 'auto', '>', '*', '&']):
+ return True
+
+ # If we see a close parenthesis, look for decltype on the other side.
+ # decltype would unambiguously identify a type, anything else is
+ # probably a parenthesized expression and not a type.
+ if match.group(2) == ')':
+ return IsDecltype(
+ clean_lines, linenum, len(match.group(1)) + len(match.group(2)) - 1)
+
+ # Check for casts and cv-qualifiers.
+ # match.group(1) remainder
+ # -------------- ---------
+ # const_cast< type&&
+ # const type&&
+ # type const&&
+ if Search(r'\b(?:const_cast\s*<|static_cast\s*<|dynamic_cast\s*<|'
+ r'reinterpret_cast\s*<|\w+\s)\s*$',
+ match.group(1)):
+ return True
+
+ # Look for a preceding symbol that might help differentiate the context.
+ # These are the cases that would be ambiguous:
+ # match.group(1) remainder
+ # -------------- ---------
+ # Call ( expression &&
+ # Declaration ( type&&
+ # sizeof ( type&&
+ # if ( expression &&
+ # while ( expression &&
+ # for ( type&&
+ # for( ; expression &&
+ # statement ; type&&
+ # block { type&&
+ # constructor { expression &&
+ start = linenum
+ line = match.group(1)
+ match_symbol = None
+ while start >= 0:
+ # We want to skip over identifiers and commas to get to a symbol.
+ # Commas are skipped so that we can find the opening parenthesis
+ # for function parameter lists.
+ match_symbol = Match(r'^(.*)([^\w\s,])[\w\s,]*$', line)
+ if match_symbol:
+ break
+ start -= 1
+ line = clean_lines.elided[start]
+
+ if not match_symbol:
+ # Probably the first statement in the file is an rvalue reference
+ return True
+
+ if match_symbol.group(2) == '}':
+ # Found closing brace, probably an indicate of this:
+ # block{} type&&
+ return True
+
+ if match_symbol.group(2) == ';':
+ # Found semicolon, probably one of these:
+ # for(; expression &&
+ # statement; type&&
+
+ # Look for the previous 'for(' in the previous lines.
+ before_text = match_symbol.group(1)
+ for i in xrange(start - 1, max(start - 6, 0), -1):
+ before_text = clean_lines.elided[i] + before_text
+ if Search(r'for\s*\([^{};]*$', before_text):
+ # This is the condition inside a for-loop
+ return False
+
+ # Did not find a for-init-statement before this semicolon, so this
+ # is probably a new statement and not a condition.
+ return True
+
+ if match_symbol.group(2) == '{':
+ # Found opening brace, probably one of these:
+ # block{ type&& = ... ; }
+ # constructor{ expression && expression }
+
+ # Look for a closing brace or a semicolon. If we see a semicolon
+ # first, this is probably a rvalue reference.
+ line = clean_lines.elided[start][0:len(match_symbol.group(1)) + 1]
+ end = start
+ depth = 1
+ while True:
+ for ch in line:
+ if ch == ';':
+ return True
+ elif ch == '{':
+ depth += 1
+ elif ch == '}':
+ depth -= 1
+ if depth == 0:
+ return False
+ end += 1
+ if end >= clean_lines.NumLines():
+ break
+ line = clean_lines.elided[end]
+ # Incomplete program?
+ return False
+
+ if match_symbol.group(2) == '(':
+ # Opening parenthesis. Need to check what's to the left of the
+ # parenthesis. Look back one extra line for additional context.
+ before_text = match_symbol.group(1)
+ if linenum > 1:
+ before_text = clean_lines.elided[linenum - 1] + before_text
+ before_text = match_symbol.group(1)
+
+ # Patterns that are likely to be types:
+ # [](type&&
+ # for (type&&
+ # sizeof(type&&
+ # operator=(type&&
+ #
+ if Search(r'(?:\]|\bfor|\bsizeof|\boperator\s*\S+\s*)\s*$', before_text):
+ return True
+
+ # Patterns that are likely to be expressions:
+ # if (expression &&
+ # while (expression &&
+ # : initializer(expression &&
+ # , initializer(expression &&
+ # ( FunctionCall(expression &&
+ # + FunctionCall(expression &&
+ # + (expression &&
+ #
+ # The last '+' represents operators such as '+' and '-'.
+ if Search(r'(?:\bif|\bwhile|[-+=%^(<!?:,&*]\s*)$', before_text):
+ return False
+
+ # Something else. Check that tokens to the left look like
+ # return_type function_name
+ match_func = Match(r'^(.*\S.*)\s+\w(?:\w|::)*(?:<[^<>]*>)?\s*$',
+ match_symbol.group(1))
+ if match_func:
+ # Check for constructors, which don't have return types.
+ if Search(r'\b(?:explicit|inline)$', match_func.group(1)):
+ return True
+ implicit_constructor = Match(r'\s*(\w+)\((?:const\s+)?(\w+)', prefix)
+ if (implicit_constructor and
+ implicit_constructor.group(1) == implicit_constructor.group(2)):
+ return True
+ return IsRValueType(typenames, clean_lines, nesting_state, linenum,
+ len(match_func.group(1)))
+
+ # Nothing before the function name. If this is inside a block scope,
+ # this is probably a function call.
+ return not (nesting_state.previous_stack_top and
+ nesting_state.previous_stack_top.IsBlockInfo())
+
+ if match_symbol.group(2) == '>':
+ # Possibly a closing bracket, check that what's on the other side
+ # looks like the start of a template.
+ return IsTemplateParameterList(
+ clean_lines, start, len(match_symbol.group(1)))
+
+ # Some other symbol, usually something like "a=b&&c". This is most
+ # likely not a type.
+ return False
+
+
+def IsDeletedOrDefault(clean_lines, linenum):
+ """Check if current constructor or operator is deleted or default.
+
+ Args:
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ Returns:
+ True if this is a deleted or default constructor.
+ """
+ open_paren = clean_lines.elided[linenum].find('(')
+ if open_paren < 0:
+ return False
+ (close_line, _, close_paren) = CloseExpression(
+ clean_lines, linenum, open_paren)
+ if close_paren < 0:
+ return False
+ return Match(r'\s*=\s*(?:delete|default)\b', close_line[close_paren:])
+
+
+def IsRValueAllowed(clean_lines, linenum, typenames):
+ """Check if RValue reference is allowed on a particular line.
+
+ Args:
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ typenames: set of type names from template-argument-list.
+ Returns:
+ True if line is within the region where RValue references are allowed.
+ """
+ # Allow region marked by PUSH/POP macros
+ for i in xrange(linenum, 0, -1):
+ line = clean_lines.elided[i]
+ if Match(r'GOOGLE_ALLOW_RVALUE_REFERENCES_(?:PUSH|POP)', line):
+ if not line.endswith('PUSH'):
+ return False
+ for j in xrange(linenum, clean_lines.NumLines(), 1):
+ line = clean_lines.elided[j]
+ if Match(r'GOOGLE_ALLOW_RVALUE_REFERENCES_(?:PUSH|POP)', line):
+ return line.endswith('POP')
+
+ # Allow operator=
+ line = clean_lines.elided[linenum]
+ if Search(r'\boperator\s*=\s*\(', line):
+ return IsDeletedOrDefault(clean_lines, linenum)
+
+ # Allow constructors
+ match = Match(r'\s*(?:[\w<>]+::)*([\w<>]+)\s*::\s*([\w<>]+)\s*\(', line)
+ if match and match.group(1) == match.group(2):
+ return IsDeletedOrDefault(clean_lines, linenum)
+ if Search(r'\b(?:explicit|inline)\s+[\w<>]+\s*\(', line):
+ return IsDeletedOrDefault(clean_lines, linenum)
+
+ if Match(r'\s*[\w<>]+\s*\(', line):
+ previous_line = 'ReturnType'
+ if linenum > 0:
+ previous_line = clean_lines.elided[linenum - 1]
+ if Match(r'^\s*$', previous_line) or Search(r'[{}:;]\s*$', previous_line):
+ return IsDeletedOrDefault(clean_lines, linenum)
+
+ # Reject types not mentioned in template-argument-list
+ while line:
+ match = Match(r'^.*?(\w+)\s*&&(.*)$', line)
+ if not match:
+ break
+ if match.group(1) not in typenames:
+ return False
+ line = match.group(2)
+
+ # All RValue types that were in template-argument-list should have
+ # been removed by now. Those were allowed, assuming that they will
+ # be forwarded.
+ #
+ # If there are no remaining RValue types left (i.e. types that were
+ # not found in template-argument-list), flag those as not allowed.
+ return line.find('&&') < 0
+
+
+def GetTemplateArgs(clean_lines, linenum):
+ """Find list of template arguments associated with this function declaration.
+
+ Args:
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: Line number containing the start of the function declaration,
+ usually one line after the end of the template-argument-list.
+ Returns:
+ Set of type names, or empty set if this does not appear to have
+ any template parameters.
+ """
+ # Find start of function
+ func_line = linenum
+ while func_line > 0:
+ line = clean_lines.elided[func_line]
+ if Match(r'^\s*$', line):
+ return set()
+ if line.find('(') >= 0:
+ break
+ func_line -= 1
+ if func_line == 0:
+ return set()
+
+ # Collapse template-argument-list into a single string
+ argument_list = ''
+ match = Match(r'^(\s*template\s*)<', clean_lines.elided[func_line])
+ if match:
+ # template-argument-list on the same line as function name
+ start_col = len(match.group(1))
+ _, end_line, end_col = CloseExpression(clean_lines, func_line, start_col)
+ if end_col > -1 and end_line == func_line:
+ start_col += 1 # Skip the opening bracket
+ argument_list = clean_lines.elided[func_line][start_col:end_col]
+
+ elif func_line > 1:
+ # template-argument-list one line before function name
+ match = Match(r'^(.*)>\s*$', clean_lines.elided[func_line - 1])
+ if match:
+ end_col = len(match.group(1))
+ _, start_line, start_col = ReverseCloseExpression(
+ clean_lines, func_line - 1, end_col)
+ if start_col > -1:
+ start_col += 1 # Skip the opening bracket
+ while start_line < func_line - 1:
+ argument_list += clean_lines.elided[start_line][start_col:]
+ start_col = 0
+ start_line += 1
+ argument_list += clean_lines.elided[func_line - 1][start_col:end_col]
+
+ if not argument_list:
+ return set()
+
+ # Extract type names
+ typenames = set()
+ while True:
+ match = Match(r'^[,\s]*(?:typename|class)(?:\.\.\.)?\s+(\w+)(.*)$',
+ argument_list)
+ if not match:
+ break
+ typenames.add(match.group(1))
+ argument_list = match.group(2)
+ return typenames
+
+
+def CheckRValueReference(filename, clean_lines, linenum, nesting_state, error):
+ """Check for rvalue references.
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ nesting_state: A NestingState instance which maintains information about
+ the current stack of nested blocks being parsed.
+ error: The function to call with any errors found.
+ """
+ # Find lines missing spaces around &&.
+ # TODO(unknown): currently we don't check for rvalue references
+ # with spaces surrounding the && to avoid false positives with
+ # boolean expressions.
+ line = clean_lines.elided[linenum]
+ match = Match(r'^(.*\S)&&', line)
+ if not match:
+ match = Match(r'(.*)&&\S', line)
+ if (not match) or '(&&)' in line or Search(r'\boperator\s*$', match.group(1)):
+ return
+
+ # Either poorly formed && or an rvalue reference, check the context
+ # to get a more accurate error message. Mostly we want to determine
+ # if what's to the left of "&&" is a type or not.
+ typenames = GetTemplateArgs(clean_lines, linenum)
+ and_pos = len(match.group(1))
+ if IsRValueType(typenames, clean_lines, nesting_state, linenum, and_pos):
+ if not IsRValueAllowed(clean_lines, linenum, typenames):
+ error(filename, linenum, 'build/c++11', 3,
+ 'RValue references are an unapproved C++ feature.')
+ else:
+ error(filename, linenum, 'whitespace/operators', 3,
+ 'Missing spaces around &&')
+
+
+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
@@ -1938,8 +3832,8 @@ def CheckSectionSpacing(filename, clean_lines, class_info, linenum, error):
#
# 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):
+ if (class_info.last_line - class_info.starting_linenum <= 24 or
+ linenum <= class_info.starting_linenum):
return
matched = Match(r'\s*(public|protected|private):', clean_lines.lines[linenum])
@@ -1950,15 +3844,18 @@ def CheckSectionSpacing(filename, clean_lines, class_info, linenum, error):
# - 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.
+ # Also ignores cases where the previous line ends with a backslash as can be
+ # common when defining classes in C macros.
prev_line = clean_lines.lines[linenum - 1]
if (not IsBlankLine(prev_line) and
- not Search(r'\b(class|struct)\b', prev_line)):
+ not Search(r'\b(class|struct)\b', prev_line) and
+ not Search(r'\\$', 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):
+ end_class_head = class_info.starting_linenum
+ for i in range(class_info.starting_linenum, linenum):
if Search(r'\{\s*$', clean_lines.lines[i]):
end_class_head = i
break
@@ -2003,19 +3900,21 @@ def CheckBraces(filename, clean_lines, linenum, error):
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 '}'.
+ # 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. Braces are also
+ # used for brace initializers inside function calls. 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 '}', or if the
+ # previous line starts a preprocessor block.
prevline = GetPreviousNonBlankLine(clean_lines, linenum)[0]
- if not Search(r'[;:}{]\s*$', prevline):
+ if (not Search(r'[,;:}{(]\s*$', prevline) and
+ not Match(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):
+ if Match(r'\s*else\b\s*(?:if\b|\{|$)', line):
prevline = GetPreviousNonBlankLine(clean_lines, linenum)[0]
if Match(r'\s*}\s*$', prevline):
error(filename, linenum, 'whitespace/newline', 4,
@@ -2023,19 +3922,20 @@ def CheckBraces(filename, clean_lines, linenum, error):
# 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')
+ if Search(r'else if\s*\(', line): # could be multi-line if
+ brace_on_left = bool(Search(r'}\s*else if\s*\(', line))
+ # find the ( after the if
+ pos = line.find('else if')
+ pos = line.find('(', pos)
+ if pos > 0:
+ (endline, _, endpos) = CloseExpression(clean_lines, linenum, pos)
+ brace_on_right = endline[endpos:].find('{') != -1
+ if brace_on_left != brace_on_right: # 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')
+ elif Search(r'}\s*else[^{]*$', line) or Match(r'[^}]*else\s*{', line):
+ 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):
@@ -2047,56 +3947,255 @@ def CheckBraces(filename, clean_lines, linenum, error):
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 }")
+ # Check single-line if/else bodies. The style guide says 'curly braces are not
+ # required for single-line statements'. We additionally allow multi-line,
+ # single statements, but we reject anything with more than one semicolon in
+ # it. This means that the first semicolon after the if should be at the end of
+ # its line, and the line after that should have an indent level equal to or
+ # lower than the if. We also check for ambiguous if/else nesting without
+ # braces.
+ if_else_match = Search(r'\b(if\s*\(|else\b)', line)
+ if if_else_match and not Match(r'\s*#', line):
+ if_indent = GetIndentLevel(line)
+ endline, endlinenum, endpos = line, linenum, if_else_match.end()
+ if_match = Search(r'\bif\s*\(', line)
+ if if_match:
+ # This could be a multiline if condition, so find the end first.
+ pos = if_match.end() - 1
+ (endline, endlinenum, endpos) = CloseExpression(clean_lines, linenum, pos)
+ # Check for an opening brace, either directly after the if or on the next
+ # line. If found, this isn't a single-statement conditional.
+ if (not Match(r'\s*{', endline[endpos:])
+ and not (Match(r'\s*$', endline[endpos:])
+ and endlinenum < (len(clean_lines.elided) - 1)
+ and Match(r'\s*{', clean_lines.elided[endlinenum + 1]))):
+ while (endlinenum < len(clean_lines.elided)
+ and ';' not in clean_lines.elided[endlinenum][endpos:]):
+ endlinenum += 1
+ endpos = 0
+ if endlinenum < len(clean_lines.elided):
+ endline = clean_lines.elided[endlinenum]
+ # We allow a mix of whitespace and closing braces (e.g. for one-liner
+ # methods) and a single \ after the semicolon (for macros)
+ endpos = endline.find(';')
+ if not Match(r';[\s}]*(\\?)$', endline[endpos:]):
+ # Semicolon isn't the last character, there's something trailing.
+ # Output a warning if the semicolon is not contained inside
+ # a lambda expression.
+ if not Match(r'^[^{};]*\[[^\[\]]*\][^{}]*\{[^{}]*\}\s*\)*[;,]\s*$',
+ endline):
+ error(filename, linenum, 'readability/braces', 4,
+ 'If/else bodies with multiple statements require braces')
+ elif endlinenum < len(clean_lines.elided) - 1:
+ # Make sure the next line is dedented
+ next_line = clean_lines.elided[endlinenum + 1]
+ next_indent = GetIndentLevel(next_line)
+ # With ambiguous nested if statements, this will error out on the
+ # if that *doesn't* match the else, regardless of whether it's the
+ # inner one or outer one.
+ if (if_match and Match(r'\s*else\b', next_line)
+ and next_indent != if_indent):
+ error(filename, linenum, 'readability/braces', 4,
+ 'Else clause should be indented at the same level as if. '
+ 'Ambiguous nested if/else chains require braces.')
+ elif next_indent > if_indent:
+ error(filename, linenum, 'readability/braces', 4,
+ 'If/else bodies with multiple statements require braces')
+
+
+def CheckTrailingSemicolon(filename, clean_lines, linenum, error):
+ """Looks for redundant trailing semicolon.
+ 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.
+ """
-def ReplaceableCheck(operator, macro, line):
- """Determine whether a basic CHECK can be replaced with a more specific one.
+ line = clean_lines.elided[linenum]
- For example suggest using CHECK_EQ instead of CHECK(a == b) and
- similarly for CHECK_GE, CHECK_GT, CHECK_LE, CHECK_LT, CHECK_NE.
+ # Block bodies should not be followed by a semicolon. Due to C++11
+ # brace initialization, there are more places where semicolons are
+ # required than not, so we use a whitelist approach to check these
+ # rather than a blacklist. These are the places where "};" should
+ # be replaced by just "}":
+ # 1. Some flavor of block following closing parenthesis:
+ # for (;;) {};
+ # while (...) {};
+ # switch (...) {};
+ # Function(...) {};
+ # if (...) {};
+ # if (...) else if (...) {};
+ #
+ # 2. else block:
+ # if (...) else {};
+ #
+ # 3. const member function:
+ # Function(...) const {};
+ #
+ # 4. Block following some statement:
+ # x = 42;
+ # {};
+ #
+ # 5. Block at the beginning of a function:
+ # Function(...) {
+ # {};
+ # }
+ #
+ # Note that naively checking for the preceding "{" will also match
+ # braces inside multi-dimensional arrays, but this is fine since
+ # that expression will not contain semicolons.
+ #
+ # 6. Block following another block:
+ # while (true) {}
+ # {};
+ #
+ # 7. End of namespaces:
+ # namespace {};
+ #
+ # These semicolons seems far more common than other kinds of
+ # redundant semicolons, possibly due to people converting classes
+ # to namespaces. For now we do not warn for this case.
+ #
+ # Try matching case 1 first.
+ match = Match(r'^(.*\)\s*)\{', line)
+ if match:
+ # Matched closing parenthesis (case 1). Check the token before the
+ # matching opening parenthesis, and don't warn if it looks like a
+ # macro. This avoids these false positives:
+ # - macro that defines a base class
+ # - multi-line macro that defines a base class
+ # - macro that defines the whole class-head
+ #
+ # But we still issue warnings for macros that we know are safe to
+ # warn, specifically:
+ # - TEST, TEST_F, TEST_P, MATCHER, MATCHER_P
+ # - TYPED_TEST
+ # - INTERFACE_DEF
+ # - EXCLUSIVE_LOCKS_REQUIRED, SHARED_LOCKS_REQUIRED, LOCKS_EXCLUDED:
+ #
+ # We implement a whitelist of safe macros instead of a blacklist of
+ # unsafe macros, even though the latter appears less frequently in
+ # google code and would have been easier to implement. This is because
+ # the downside for getting the whitelist wrong means some extra
+ # semicolons, while the downside for getting the blacklist wrong
+ # would result in compile errors.
+ #
+ # In addition to macros, we also don't want to warn on
+ # - Compound literals
+ # - Lambdas
+ # - alignas specifier with anonymous structs:
+ closing_brace_pos = match.group(1).rfind(')')
+ opening_parenthesis = ReverseCloseExpression(
+ clean_lines, linenum, closing_brace_pos)
+ if opening_parenthesis[2] > -1:
+ line_prefix = opening_parenthesis[0][0:opening_parenthesis[2]]
+ macro = Search(r'\b([A-Z_]+)\s*$', line_prefix)
+ func = Match(r'^(.*\])\s*$', line_prefix)
+ if ((macro and
+ macro.group(1) not in (
+ 'TEST', 'TEST_F', 'MATCHER', 'MATCHER_P', 'TYPED_TEST',
+ 'EXCLUSIVE_LOCKS_REQUIRED', 'SHARED_LOCKS_REQUIRED',
+ 'LOCKS_EXCLUDED', 'INTERFACE_DEF')) or
+ (func and not Search(r'\boperator\s*\[\s*\]', func.group(1))) or
+ Search(r'\b(?:struct|union)\s+alignas\s*$', line_prefix) or
+ Search(r'\s+=\s*$', line_prefix)):
+ match = None
+ if (match and
+ opening_parenthesis[1] > 1 and
+ Search(r'\]\s*$', clean_lines.elided[opening_parenthesis[1] - 1])):
+ # Multi-line lambda-expression
+ match = None
- Args:
- operator: The C++ operator used in the CHECK.
- macro: The CHECK or EXPECT macro being called.
- line: The current source line.
+ else:
+ # Try matching cases 2-3.
+ match = Match(r'^(.*(?:else|\)\s*const)\s*)\{', line)
+ if not match:
+ # Try matching cases 4-6. These are always matched on separate lines.
+ #
+ # Note that we can't simply concatenate the previous line to the
+ # current line and do a single match, otherwise we may output
+ # duplicate warnings for the blank line case:
+ # if (cond) {
+ # // blank line
+ # }
+ prevline = GetPreviousNonBlankLine(clean_lines, linenum)[0]
+ if prevline and Search(r'[;{}]\s*$', prevline):
+ match = Match(r'^(\s*)\{', line)
+
+ # Check matching closing brace
+ if match:
+ (endline, endlinenum, endpos) = CloseExpression(
+ clean_lines, linenum, len(match.group(1)))
+ if endpos > -1 and Match(r'^\s*;', endline[endpos:]):
+ # Current {} pair is eligible for semicolon check, and we have found
+ # the redundant semicolon, output warning here.
+ #
+ # Note: because we are scanning forward for opening braces, and
+ # outputting warnings for the matching closing brace, if there are
+ # nested blocks with trailing semicolons, we will get the error
+ # messages in reversed order.
+ error(filename, endlinenum, 'readability/braces', 4,
+ "You don't need a ; after a }")
- Returns:
- True if the CHECK can be replaced with a more specific one.
+
+def CheckEmptyBlockBody(filename, clean_lines, linenum, error):
+ """Look for empty loop/conditional body with only a single semicolon.
+
+ 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.
"""
- # 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}|".*"|\'.*\')'
+ # Search for loop keywords at the beginning of the line. Because only
+ # whitespaces are allowed before the keywords, this will also ignore most
+ # do-while-loops, since those lines should start with closing brace.
+ #
+ # We also check "if" blocks here, since an empty conditional block
+ # is likely an error.
+ line = clean_lines.elided[linenum]
+ matched = Match(r'\s*(for|while|if)\s*\(', line)
+ if matched:
+ # Find the end of the conditional expression
+ (end_line, end_linenum, end_pos) = CloseExpression(
+ clean_lines, linenum, line.find('('))
+
+ # Output warning if what follows the condition expression is a semicolon.
+ # No warning for all other cases, including whitespace or newline, since we
+ # have a separate check for semicolons preceded by whitespace.
+ if end_pos >= 0 and Match(r';', end_line[end_pos:]):
+ if matched.group(1) == 'if':
+ error(filename, end_linenum, 'whitespace/empty_conditional_body', 5,
+ 'Empty conditional bodies should use {}')
+ else:
+ error(filename, end_linenum, 'whitespace/empty_loop_body', 5,
+ 'Empty loop bodies should use {} or continue')
+
- # 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*\))')
+def FindCheckMacro(line):
+ """Find a replaceable CHECK-like macro.
- # 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)
+ Args:
+ line: line to search on.
+ Returns:
+ (macro name, start position), or (None, -1) if no replaceable
+ macro is found.
+ """
+ for macro in _CHECK_MACROS:
+ i = line.find(macro)
+ if i >= 0:
+ # Find opening parenthesis. Do a regular expression match here
+ # to make sure that we are matching the expected CHECK macro, as
+ # opposed to some other macro that happens to contain the CHECK
+ # substring.
+ matched = Match(r'^(.*\b' + macro + r'\s*)\(', line)
+ if not matched:
+ continue
+ return (macro, len(matched.group(1)))
+ return (None, -1)
def CheckCheck(filename, clean_lines, linenum, error):
@@ -2110,26 +4209,143 @@ def CheckCheck(filename, clean_lines, linenum, error):
"""
# 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'
+ lines = clean_lines.elided
+ (check_macro, start_pos) = FindCheckMacro(lines[linenum])
+ if not check_macro:
return
- line = clean_lines.elided[linenum] # get rid of comments and strings
+ # Find end of the boolean expression by matching parentheses
+ (last_line, end_line, end_pos) = CloseExpression(
+ clean_lines, linenum, start_pos)
+ if end_pos < 0:
+ return
- # 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
+ # If the check macro is followed by something other than a
+ # semicolon, assume users will log their own custom error messages
+ # and don't suggest any replacements.
+ if not Match(r'\s*;', last_line[end_pos:]):
+ return
+
+ if linenum == end_line:
+ expression = lines[linenum][start_pos + 1:end_pos - 1]
+ else:
+ expression = lines[linenum][start_pos + 1:]
+ for i in xrange(linenum + 1, end_line):
+ expression += lines[i]
+ expression += last_line[0:end_pos - 1]
+
+ # Parse expression so that we can take parentheses into account.
+ # This avoids false positives for inputs like "CHECK((a < 4) == b)",
+ # which is not replaceable by CHECK_LE.
+ lhs = ''
+ rhs = ''
+ operator = None
+ while expression:
+ matched = Match(r'^\s*(<<|<<=|>>|>>=|->\*|->|&&|\|\||'
+ r'==|!=|>=|>|<=|<|\()(.*)$', expression)
+ if matched:
+ token = matched.group(1)
+ if token == '(':
+ # Parenthesized operand
+ expression = matched.group(2)
+ (end, _) = FindEndOfExpressionInLine(expression, 0, ['('])
+ if end < 0:
+ return # Unmatched parenthesis
+ lhs += '(' + expression[0:end]
+ expression = expression[end:]
+ elif token in ('&&', '||'):
+ # Logical and/or operators. This means the expression
+ # contains more than one term, for example:
+ # CHECK(42 < a && a < b);
+ #
+ # These are not replaceable with CHECK_LE, so bail out early.
+ return
+ elif token in ('<<', '<<=', '>>', '>>=', '->*', '->'):
+ # Non-relational operator
+ lhs += token
+ expression = matched.group(2)
+ else:
+ # Relational operator
+ operator = token
+ rhs = matched.group(2)
+ break
+ else:
+ # Unparenthesized operand. Instead of appending to lhs one character
+ # at a time, we do another regular expression match to consume several
+ # characters at once if possible. Trivial benchmark shows that this
+ # is more efficient when the operands are longer than a single
+ # character, which is generally the case.
+ matched = Match(r'^([^-=!<>()&|]+)(.*)$', expression)
+ if not matched:
+ matched = Match(r'^(\s*\S)(.*)$', expression)
+ if not matched:
+ break
+ lhs += matched.group(1)
+ expression = matched.group(2)
+
+ # Only apply checks if we got all parts of the boolean expression
+ if not (lhs and operator and rhs):
+ return
+
+ # Check that rhs do not contain logical operators. We already know
+ # that lhs is fine since the loop above parses out && and ||.
+ if rhs.find('&&') > -1 or rhs.find('||') > -1:
+ return
+
+ # At least one of the operands must be a constant literal. This is
+ # to avoid suggesting replacements for unprintable things like
+ # CHECK(variable != iterator)
+ #
+ # The following pattern matches decimal, hex integers, strings, and
+ # characters (in that order).
+ lhs = lhs.strip()
+ rhs = rhs.strip()
+ match_constant = r'^([-+]?(\d+|0[xX][0-9a-fA-F]+)[lLuU]{0,3}|".*"|\'.*\')$'
+ if Match(match_constant, lhs) or Match(match_constant, rhs):
+ # Note: since we know both lhs and rhs, we can provide a more
+ # descriptive error message like:
+ # Consider using CHECK_EQ(x, 42) instead of CHECK(x == 42)
+ # Instead of:
+ # Consider using CHECK_EQ instead of CHECK(a == b)
+ #
+ # We are still keeping the less descriptive message because if lhs
+ # or rhs gets long, the error message might become unreadable.
+ error(filename, linenum, 'readability/check', 2,
+ 'Consider using %s instead of %s(a %s b)' % (
+ _CHECK_REPLACEMENT[check_macro][operator],
+ check_macro, operator))
+
+
+def CheckAltTokens(filename, clean_lines, linenum, error):
+ """Check alternative keywords being used in boolean expressions.
+
+ 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]
+
+ # Avoid preprocessor lines
+ if Match(r'^\s*#', line):
+ return
+
+ # Last ditch effort to avoid multi-line comments. This will not help
+ # if the comment started before the current line or ended after the
+ # current line, but it catches most of the false positives. At least,
+ # it provides a way to workaround this warning for people who use
+ # multi-line comments in preprocessor macros.
+ #
+ # TODO(unknown): remove this once cpplint has better support for
+ # multi-line comments.
+ if line.find('/*') >= 0 or line.find('*/') >= 0:
+ return
+
+ for match in _ALT_TOKEN_REPLACEMENT_PATTERN.finditer(line):
+ error(filename, linenum, 'readability/alt_tokens', 2,
+ 'Use operator %s instead of %s' % (
+ _ALT_TOKEN_REPLACEMENT[match.group(1)], match.group(1)))
def GetLineWidth(line):
@@ -2154,7 +4370,7 @@ def GetLineWidth(line):
return len(line)
-def CheckStyle(filename, clean_lines, linenum, file_extension, class_state,
+def CheckStyle(filename, clean_lines, linenum, file_extension, nesting_state,
error):
"""Checks rules from the 'C++ style rules' section of cppguide.html.
@@ -2167,10 +4383,15 @@ def CheckStyle(filename, clean_lines, linenum, file_extension, class_state,
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.
+ nesting_state: A NestingState instance which maintains information about
+ the current stack of nested blocks being parsed.
error: The function to call with any errors found.
"""
- raw_lines = clean_lines.raw_lines
+ # Don't use "elided" lines here, otherwise we can't check commented lines.
+ # Don't want to use "raw" either, because we don't want to check inside C++11
+ # raw strings,
+ raw_lines = clean_lines.lines_without_raw_strings
line = raw_lines[linenum]
if line.find('\t') != -1:
@@ -2189,6 +4410,8 @@ def CheckStyle(filename, clean_lines, linenum, file_extension, class_state,
# if(match($0, " <<")) complain = 0;
# if(match(prev, " +for \\(")) complain = 0;
# if(prevodd && match(prevprev, " +for \\(")) complain = 0;
+ scope_or_label_pattern = r'\s*\w+\s*:\s*\\?$'
+ classinfo = nesting_state.InnermostClass()
initial_spaces = 0
cleansed_line = clean_lines.elided[linenum]
while initial_spaces < len(line) and line[initial_spaces] == ' ':
@@ -2196,21 +4419,15 @@ def CheckStyle(filename, clean_lines, linenum, file_extension, class_state,
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
+ # There are certain situations we allow one space, notably for
+ # section labels, and also lines containing multi-line raw strings.
elif ((initial_spaces == 1 or initial_spaces == 3) and
- not Match(r'\s*\w+\s*:\s*$', cleansed_line)):
+ not Match(scope_or_label_pattern, cleansed_line) and
+ not (clean_lines.raw_lines[linenum] != line and
+ Match(r'^\s*""', 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
@@ -2232,12 +4449,14 @@ def CheckStyle(filename, clean_lines, linenum, file_extension, class_state,
not Match(r'^\s*//.*http(s?)://\S*$', line) and
not Match(r'^// \$Id:.*#[0-9]+ \$$', line)):
line_width = GetLineWidth(line)
- if line_width > 100:
+ extended_length = int((_line_length * 1.25))
+ if line_width > extended_length:
error(filename, linenum, 'whitespace/line_length', 4,
- 'Lines should very rarely be longer than 100 characters')
- elif line_width > 80:
+ 'Lines should very rarely be longer than %i characters' %
+ extended_length)
+ elif line_width > _line_length:
error(filename, linenum, 'whitespace/line_length', 2,
- 'Lines should be <= 80 characters long')
+ 'Lines should be <= %i characters long' % _line_length)
if (cleansed_line.count(';') > 1 and
# for loops are allowed two ;'s (and may run over two lines).
@@ -2248,19 +4467,28 @@ def CheckStyle(filename, clean_lines, linenum, file_extension, class_state,
not ((cleansed_line.find('case ') != -1 or
cleansed_line.find('default:') != -1) and
cleansed_line.find('break;') != -1)):
- error(filename, linenum, 'whitespace/newline', 4,
+ error(filename, linenum, 'whitespace/newline', 0,
'More than one command on the same line')
# Some more style checks
CheckBraces(filename, clean_lines, linenum, error)
- CheckSpacing(filename, clean_lines, linenum, error)
+ CheckTrailingSemicolon(filename, clean_lines, linenum, error)
+ CheckEmptyBlockBody(filename, clean_lines, linenum, error)
+ CheckAccess(filename, clean_lines, linenum, nesting_state, error)
+ CheckSpacing(filename, clean_lines, linenum, nesting_state, error)
+ CheckOperatorSpacing(filename, clean_lines, linenum, error)
+ CheckParenthesisSpacing(filename, clean_lines, linenum, error)
+ CheckCommaSpacing(filename, clean_lines, linenum, error)
+ CheckBracesSpacing(filename, clean_lines, linenum, error)
+ CheckSpacingForFunctionCall(filename, clean_lines, linenum, error)
+ CheckRValueReference(filename, clean_lines, linenum, nesting_state, 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)
+ CheckAltTokens(filename, clean_lines, linenum, error)
+ classinfo = nesting_state.InnermostClass()
+ if classinfo:
+ CheckSectionSpacing(filename, clean_lines, classinfo, 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'
@@ -2340,8 +4568,7 @@ def _ClassifyInclude(fileinfo, include, is_system):
"""
# 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
+ is_cpp_h = include in _CPP_HEADERS
if is_system:
if is_cpp_h:
@@ -2390,11 +4617,17 @@ def CheckIncludeLine(filename, clean_lines, linenum, include_state, error):
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):
+ # Only do this check if the included header follows google naming
+ # conventions. If not, assume that it's a 3rd party API that
+ # requires special include conventions.
+ #
+ # We also make an exception for Lua headers, which follow google
+ # naming convention but not the include convention.
+ match = Match(r'#include\s*"([^/]+\.h)"', line)
+ if match and not _THIRD_PARTY_HEADERS_PATTERN.match(match.group(1)):
error(filename, linenum, 'build/include', 4,
'Include the directory when naming .h files')
@@ -2405,12 +4638,17 @@ def CheckIncludeLine(filename, clean_lines, linenum, include_state, error):
if match:
include = match.group(2)
is_system = (match.group(1) == '<')
- if include in include_state:
+ duplicate_line = include_state.FindHeader(include)
+ if duplicate_line >= 0:
error(filename, linenum, 'build/include', 4,
'"%s" already included at %s:%s' %
- (include, filename, include_state[include]))
- else:
- include_state[include] = linenum
+ (include, filename, duplicate_line))
+ elif (include.endswith('.cc') and
+ os.path.dirname(fileinfo.RepositoryName()) != os.path.dirname(include)):
+ error(filename, linenum, 'build/include', 4,
+ 'Do not include .cc files from other packages')
+ elif not _THIRD_PARTY_HEADERS_PATTERN.match(include):
+ include_state.include_list[-1].append((include, linenum))
# We want to ensure that headers appear in the right order:
# 1) for foo.cc, foo.h (preferred location)
@@ -2429,23 +4667,17 @@ def CheckIncludeLine(filename, clean_lines, linenum, include_state, error):
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):
+ canonical_include = include_state.CanonicalizeAlphabeticalOrder(include)
+ if not include_state.IsInAlphabeticalOrder(
+ clean_lines, linenum, canonical_include):
error(filename, linenum, 'build/include_alpha', 4,
'Include "%s" not in alphabetical order' % include)
+ include_state.SetLastHeader(canonical_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.
+ r"""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
@@ -2464,7 +4696,7 @@ def _GetTextInside(text, start_pattern):
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
+ # TODO(unknown): 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.
@@ -2500,8 +4732,34 @@ def _GetTextInside(text, start_pattern):
return text[start_position:position - 1]
-def CheckLanguage(filename, clean_lines, linenum, file_extension, include_state,
- error):
+# Patterns for matching call-by-reference parameters.
+#
+# Supports nested templates up to 2 levels deep using this messy pattern:
+# < (?: < (?: < [^<>]*
+# >
+# | [^<>] )*
+# >
+# | [^<>] )*
+# >
+_RE_PATTERN_IDENT = r'[_a-zA-Z]\w*' # =~ [[:alpha:]][[:alnum:]]*
+_RE_PATTERN_TYPE = (
+ r'(?:const\s+)?(?:typename\s+|class\s+|struct\s+|union\s+|enum\s+)?'
+ r'(?:\w|'
+ r'\s*<(?:<(?:<[^<>]*>|[^<>])*>|[^<>])*>|'
+ r'::)+')
+# A call-by-reference parameter ends with '& identifier'.
+_RE_PATTERN_REF_PARAM = re.compile(
+ r'(' + _RE_PATTERN_TYPE + r'(?:\s*(?:\bconst\b|[*]))*\s*'
+ r'&\s*' + _RE_PATTERN_IDENT + r')\s*(?:=[^,()]+)?[,)]')
+# A call-by-const-reference parameter either ends with 'const& identifier'
+# or looks like 'const type& identifier' when 'type' is atomic.
+_RE_PATTERN_CONST_REF_PARAM = (
+ r'(?:.*\s*\bconst\s*&\s*' + _RE_PATTERN_IDENT +
+ r'|const\s+' + _RE_PATTERN_TYPE + r'\s*&\s*' + _RE_PATTERN_IDENT + r')')
+
+
+def CheckLanguage(filename, clean_lines, linenum, file_extension,
+ include_state, nesting_state, error):
"""Checks rules from the 'C++ language rules' section of cppguide.html.
Some of these rules are hard to test (function overloading, using
@@ -2513,6 +4771,8 @@ def CheckLanguage(filename, clean_lines, linenum, file_extension, include_state,
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.
+ nesting_state: A NestingState instance which maintains information about
+ the current stack of nested blocks being parsed.
error: The function to call with any errors found.
"""
# If the line is empty or consists of entirely a comment, no need to
@@ -2526,121 +4786,25 @@ def CheckLanguage(filename, clean_lines, linenum, file_extension, include_state,
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
+ # Reset include state across preprocessor directives. This is meant
+ # to silence warnings for conditional includes.
+ match = Match(r'^\s*#\s*(if|ifdef|ifndef|elif|else|endif)\b', line)
+ if match:
+ include_state.ResetSection(match.group(1))
# 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.')
+
+ # Perform other checks now that we are sure that this is not an include line
+ CheckCasts(filename, clean_lines, linenum, error)
+ CheckGlobalStatic(filename, clean_lines, linenum, error)
+ CheckPrintf(filename, clean_lines, linenum, error)
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
+ # TODO(unknown): check that classes declare or disable copy/assign
# (level 1 error)
pass
@@ -2656,27 +4820,6 @@ def CheckLanguage(filename, clean_lines, linenum, file_extension, include_state,
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 {};
@@ -2696,14 +4839,14 @@ def CheckLanguage(filename, clean_lines, linenum, file_extension, include_state,
# 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
+ # TODO(unknown): 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:
+ if match and match.group(1) != '__VA_ARGS__':
function_name = re.search(r'\b((?:string)?printf)\s*\(',
line, re.I).group(1)
error(filename, linenum, 'runtime/printf', 4,
@@ -2761,26 +4904,6 @@ def CheckLanguage(filename, clean_lines, linenum, file_extension, include_state,
'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.
@@ -2793,17 +4916,431 @@ def CheckLanguage(filename, clean_lines, linenum, file_extension, include_state,
' 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.
+def CheckGlobalStatic(filename, clean_lines, linenum, error):
+ """Check for unsafe global or static objects.
+
+ 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]
+
+ # Match two lines at a time to support multiline declarations
+ if linenum + 1 < clean_lines.NumLines() and not Search(r'[;({]', line):
+ line += clean_lines.elided[linenum + 1].strip()
+
+ # 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)
+
+ # Remove false positives:
+ # - String pointers (as opposed to values).
+ # string *pointer
+ # const string *pointer
+ # string const *pointer
+ # string *const pointer
+ #
+ # - Functions and template specializations.
+ # string Function<Type>(...
+ # string Class<Type>::Method(...
+ #
+ # - Operators. These are matched separately because operator names
+ # cross non-word boundaries, and trying to match both operators
+ # and functions at the same time would decrease accuracy of
+ # matching identifiers.
+ # string Class::operator*()
+ if (match and
+ not Search(r'\bstring\b(\s+const)?\s*\*\s*(const\s+)?\w', line) and
+ not Search(r'\boperator\W', line) 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)))
+
+ 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.')
+
+
+def CheckPrintf(filename, clean_lines, linenum, error):
+ """Check for printf related issues.
+
+ 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]
+
+ # 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\s*\(', line):
+ error(filename, linenum, 'runtime/printf', 5,
+ 'Never use sprintf. Use snprintf instead.')
+ match = Search(r'\b(strcpy|strcat)\s*\(', line)
+ if match:
+ error(filename, linenum, 'runtime/printf', 4,
+ 'Almost always, snprintf is better than %s' % match.group(1))
+
+
+def IsDerivedFunction(clean_lines, linenum):
+ """Check if current line contains an inherited function.
+
+ Args:
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ Returns:
+ True if current line contains a function with "override"
+ virt-specifier.
+ """
+ # Scan back a few lines for start of current function
+ for i in xrange(linenum, max(-1, linenum - 10), -1):
+ match = Match(r'^([^()]*\w+)\(', clean_lines.elided[i])
+ if match:
+ # Look for "override" after the matching closing parenthesis
+ line, _, closing_paren = CloseExpression(
+ clean_lines, i, len(match.group(1)))
+ return (closing_paren >= 0 and
+ Search(r'\boverride\b', line[closing_paren:]))
+ return False
+
+
+def IsOutOfLineMethodDefinition(clean_lines, linenum):
+ """Check if current line contains an out-of-line method definition.
+
+ Args:
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ Returns:
+ True if current line contains an out-of-line method definition.
+ """
+ # Scan back a few lines for start of current function
+ for i in xrange(linenum, max(-1, linenum - 10), -1):
+ if Match(r'^([^()]*\w+)\(', clean_lines.elided[i]):
+ return Match(r'^[^()]*\w+::\w+\(', clean_lines.elided[i]) is not None
+ return False
+
+
+def IsInitializerList(clean_lines, linenum):
+ """Check if current line is inside constructor initializer list.
+
+ Args:
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ Returns:
+ True if current line appears to be inside constructor initializer
+ list, False otherwise.
+ """
+ for i in xrange(linenum, 1, -1):
+ line = clean_lines.elided[i]
+ if i == linenum:
+ remove_function_body = Match(r'^(.*)\{\s*$', line)
+ if remove_function_body:
+ line = remove_function_body.group(1)
+
+ if Search(r'\s:\s*\w+[({]', line):
+ # A lone colon tend to indicate the start of a constructor
+ # initializer list. It could also be a ternary operator, which
+ # also tend to appear in constructor initializer lists as
+ # opposed to parameter lists.
+ return True
+ if Search(r'\}\s*,\s*$', line):
+ # A closing brace followed by a comma is probably the end of a
+ # brace-initialized member in constructor initializer list.
+ return True
+ if Search(r'[{};]\s*$', line):
+ # Found one of the following:
+ # - A closing brace or semicolon, probably the end of the previous
+ # function.
+ # - An opening brace, probably the start of current class or namespace.
+ #
+ # Current line is probably not inside an initializer list since
+ # we saw one of those things without seeing the starting colon.
+ return False
+
+ # Got to the beginning of the file without seeing the start of
+ # constructor initializer list.
+ return False
+
+
+def CheckForNonConstReference(filename, clean_lines, linenum,
+ nesting_state, error):
+ """Check for non-const references.
+
+ Separate from CheckLanguage since it scans backwards from current
+ line, instead of scanning forward.
+
+ Args:
+ filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+ nesting_state: A NestingState instance which maintains information about
+ the current stack of nested blocks being parsed.
+ error: The function to call with any errors found.
+ """
+ # Do nothing if there is no '&' on current line.
+ line = clean_lines.elided[linenum]
+ if '&' not in line:
+ return
+
+ # If a function is inherited, current function doesn't have much of
+ # a choice, so any non-const references should not be blamed on
+ # derived function.
+ if IsDerivedFunction(clean_lines, linenum):
+ return
+
+ # Don't warn on out-of-line method definitions, as we would warn on the
+ # in-line declaration, if it isn't marked with 'override'.
+ if IsOutOfLineMethodDefinition(clean_lines, linenum):
+ return
+
+ # Long type names may be broken across multiple lines, usually in one
+ # of these forms:
+ # LongType
+ # ::LongTypeContinued &identifier
+ # LongType::
+ # LongTypeContinued &identifier
+ # LongType<
+ # ...>::LongTypeContinued &identifier
+ #
+ # If we detected a type split across two lines, join the previous
+ # line to current line so that we can match const references
+ # accordingly.
+ #
+ # Note that this only scans back one line, since scanning back
+ # arbitrary number of lines would be expensive. If you have a type
+ # that spans more than 2 lines, please use a typedef.
+ if linenum > 1:
+ previous = None
+ if Match(r'\s*::(?:[\w<>]|::)+\s*&\s*\S', line):
+ # previous_line\n + ::current_line
+ previous = Search(r'\b((?:const\s*)?(?:[\w<>]|::)+[\w<>])\s*$',
+ clean_lines.elided[linenum - 1])
+ elif Match(r'\s*[a-zA-Z_]([\w<>]|::)+\s*&\s*\S', line):
+ # previous_line::\n + current_line
+ previous = Search(r'\b((?:const\s*)?(?:[\w<>]|::)+::)\s*$',
+ clean_lines.elided[linenum - 1])
+ if previous:
+ line = previous.group(1) + line.lstrip()
+ else:
+ # Check for templated parameter that is split across multiple lines
+ endpos = line.rfind('>')
+ if endpos > -1:
+ (_, startline, startpos) = ReverseCloseExpression(
+ clean_lines, linenum, endpos)
+ if startpos > -1 and startline < linenum:
+ # Found the matching < on an earlier line, collect all
+ # pieces up to current line.
+ line = ''
+ for i in xrange(startline, linenum + 1):
+ line += clean_lines.elided[i].strip()
+
+ # Check for non-const references in function parameters. A single '&' may
+ # found in the following places:
+ # inside expression: binary & for bitwise AND
+ # inside expression: unary & for taking the address of something
+ # inside declarators: reference parameter
+ # We will exclude the first two cases by checking that we are not inside a
+ # function body, including one that was just introduced by a trailing '{'.
+ # TODO(unknown): Doesn't account for 'catch(Exception& e)' [rare].
+ if (nesting_state.previous_stack_top and
+ not (isinstance(nesting_state.previous_stack_top, _ClassInfo) or
+ isinstance(nesting_state.previous_stack_top, _NamespaceInfo))):
+ # Not at toplevel, not within a class, and not within a namespace
+ return
+
+ # Avoid initializer lists. We only need to scan back from the
+ # current line for something that starts with ':'.
+ #
+ # We don't need to check the current line, since the '&' would
+ # appear inside the second set of parentheses on the current line as
+ # opposed to the first set.
+ if linenum > 0:
+ for i in xrange(linenum - 1, max(0, linenum - 10), -1):
+ previous_line = clean_lines.elided[i]
+ if not Search(r'[),]\s*$', previous_line):
+ break
+ if Match(r'^\s*:\s+\S', previous_line):
+ return
+
+ # Avoid preprocessors
+ if Search(r'\\\s*$', line):
+ return
+
+ # Avoid constructor initializer lists
+ if IsInitializerList(clean_lines, linenum):
+ return
+
+ # We allow non-const references in a few standard places, like functions
+ # called "swap()" or iostream operators like "<<" or ">>". Do not check
+ # those function parameters.
+ #
+ # We also accept & in static_assert, which looks like a function but
+ # it's actually a declaration expression.
+ whitelisted_functions = (r'(?:[sS]wap(?:<\w:+>)?|'
+ r'operator\s*[<>][<>]|'
+ r'static_assert|COMPILE_ASSERT'
+ r')\s*\(')
+ if Search(whitelisted_functions, line):
+ return
+ elif not Search(r'\S+\([^)]*$', line):
+ # Don't see a whitelisted function on this line. Actually we
+ # didn't see any function name on this line, so this is likely a
+ # multi-line parameter list. Try a bit harder to catch this case.
+ for i in xrange(2):
+ if (linenum > i and
+ Search(whitelisted_functions, clean_lines.elided[linenum - i - 1])):
+ return
+
+ decls = ReplaceAll(r'{[^}]*}', ' ', line) # exclude function body
+ for parameter in re.findall(_RE_PATTERN_REF_PARAM, decls):
+ if not Match(_RE_PATTERN_CONST_REF_PARAM, parameter):
+ error(filename, linenum, 'runtime/references', 2,
+ 'Is this a non-const reference? '
+ 'If so, make const or use a pointer: ' +
+ ReplaceAll(' *<', '<', parameter))
+
+
+def CheckCasts(filename, clean_lines, linenum, error):
+ """Various cast related checks.
+
+ 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]
+
+ # 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+|\S<\s*(?:const\s+)?)?\b'
+ r'(int|float|double|bool|char|int32|uint32|int64|uint64)'
+ r'(\([^)].*)', line)
+ expecting_function = ExpectingFunctionArgs(clean_lines, linenum)
+ if match and not expecting_function:
+ matched_type = match.group(2)
+
+ # matched_new_or_template is used to silence two false positives:
+ # - New operators
+ # - Template arguments with function types
+ #
+ # For template arguments, we match on types immediately following
+ # an opening bracket without any spaces. This is a fast way to
+ # silence the common case where the function type is the first
+ # template argument. False negative with less-than comparison is
+ # avoided because those operators are usually followed by a space.
+ #
+ # function<double(double)> // bracket + no space = false positive
+ # value < double(42) // bracket + space = true positive
+ matched_new_or_template = match.group(1)
+
+ # Avoid arrays by looking for brackets that come after the closing
+ # parenthesis.
+ if Match(r'\([^()]+\)\s*\[', match.group(3)):
+ return
+
+ # Other things to ignore:
+ # - Function pointers
+ # - Casts to pointer types
+ # - Placement new
+ # - Alias declarations
+ matched_funcptr = match.group(3)
+ if (matched_new_or_template is None and
+ not (matched_funcptr and
+ (Match(r'\((?:[^() ]+::\s*\*\s*)?[^() ]+\)\s*\(',
+ matched_funcptr) or
+ matched_funcptr.startswith('(*)'))) and
+ not Match(r'\s*using\s+\S+\s*=\s*' + matched_type, line) and
+ not Search(r'new\(\S+\)\s*' + matched_type, line)):
+ error(filename, linenum, 'readability/casting', 4,
+ 'Using deprecated casting style. '
+ 'Use static_cast<%s>(...) instead' %
+ matched_type)
+
+ if not expecting_function:
+ CheckCStyleCast(filename, clean_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, clean_lines, linenum, 'const_cast',
+ r'\((char\s?\*+\s?)\)\s*"', error):
+ pass
+ else:
+ # Check pointer casts for other than string constants
+ CheckCStyleCast(filename, clean_lines, linenum, 'reinterpret_cast',
+ r'\((\w+\s?\*+\s?)\)', error)
- This also handles sizeof(type) warnings, due to similarity of content.
+ # 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.
+ #
+ # Some non-identifier character is required before the '&' for the
+ # expression to be recognized as a cast. These are casts:
+ # expression = &static_cast<int*>(temporary());
+ # function(&(int*)(temporary()));
+ #
+ # This is not a cast:
+ # reference_type&(int* function_param);
+ match = Search(
+ r'(?:[^\w]&\(([^)*][^)]*)\)[\w(])|'
+ r'(?:[^\w]&(static|dynamic|down|reinterpret)_cast\b)', line)
+ if match:
+ # Try a better error message when the & is bound to something
+ # dereferenced by the casted pointer, as opposed to the casted
+ # pointer itself.
+ parenthesis_error = False
+ match = Match(r'^(.*&(?:static|dynamic|down|reinterpret)_cast\b)<', line)
+ if match:
+ _, y1, x1 = CloseExpression(clean_lines, linenum, len(match.group(1)))
+ if x1 >= 0 and clean_lines.elided[y1][x1] == '(':
+ _, y2, x2 = CloseExpression(clean_lines, y1, x1)
+ if x2 >= 0:
+ extended_line = clean_lines.elided[y2][x2:]
+ if y2 < clean_lines.NumLines() - 1:
+ extended_line += clean_lines.elided[y2 + 1]
+ if Match(r'\s*(?:->|\[)', extended_line):
+ parenthesis_error = True
+
+ if parenthesis_error:
+ error(filename, linenum, 'readability/casting', 4,
+ ('Are you taking an address of something dereferenced '
+ 'from a cast? Wrapping the dereferenced expression in '
+ 'parentheses will make the binding more obvious'))
+ else:
+ 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'))
+
+
+def CheckCStyleCast(filename, clean_lines, linenum, cast_type, pattern, error):
+ """Checks for a C-style cast by looking for the pattern.
Args:
filename: The name of the current file.
+ clean_lines: A CleansedLines instance containing the 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.
@@ -2813,39 +5350,84 @@ def CheckCStyleCast(filename, linenum, line, raw_line, cast_type, pattern,
True if an error was emitted.
False otherwise.
"""
+ line = clean_lines.elided[linenum]
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
+ # Exclude lines with keywords that tend to look like casts
+ context = line[0:match.start(1) - 1]
+ if Match(r'.*\b(?:sizeof|alignof|alignas|[_A-Z][_A-Z0-9]*)\s*$', context):
+ return False
- remainder = line[match.end(0):]
+ # Try expanding current context to see if we one level of
+ # parentheses inside a macro.
+ if linenum > 0:
+ for i in xrange(linenum - 1, max(0, linenum - 5), -1):
+ context = clean_lines.elided[i] + context
+ if Match(r'.*\b[_A-Z][_A-Z0-9]*\s*\((?:\([^()]*\)|[^()])*$', context):
+ return False
- # 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<...> ...
+ # operator++(int) and operator--(int)
+ if context.endswith(' operator++') or context.endswith(' operator--'):
+ return False
+
+ # A single unnamed argument for a function tends to look like old
+ # style cast. If we see those, don't issue warnings for deprecated
+ # casts, instead issue warnings for unnamed arguments where
+ # appropriate.
+ #
+ # These are things that we want warnings for, since the style guide
+ # explicitly require all parameters to be named:
+ # Function(int);
+ # Function(int) {
+ # ConstMember(int) const;
+ # ConstMember(int) const {
+ # ExceptionMember(int) throw (...);
+ # ExceptionMember(int) throw (...) {
+ # PureVirtual(int) = 0;
+ # [](int) -> bool {
#
- # 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')
+ # These are functions of some sort, where the compiler would be fine
+ # if they had named parameters, but people often omit those
+ # identifiers to reduce clutter:
+ # (FunctionPointer)(int);
+ # (FunctionPointer)(int) = value;
+ # Function((function_pointer_arg)(int))
+ # Function((function_pointer_arg)(int), int param)
+ # <TemplateArgument(int)>;
+ # <(FunctionPointerTemplateArgument)(int)>;
+ remainder = line[match.end(0):]
+ if Match(r'^\s*(?:;|const\b|throw\b|final\b|override\b|[=>{),]|->)',
+ remainder):
+ # Looks like an unnamed parameter.
+
+ # Don't warn on any kind of template arguments.
+ if Match(r'^\s*>', remainder):
+ return False
+
+ # Don't warn on assignments to function pointers, but keep warnings for
+ # unnamed parameters to pure virtual functions. Note that this pattern
+ # will also pass on assignments of "0" to function pointers, but the
+ # preferred values for those would be "nullptr" or "NULL".
+ matched_zero = Match(r'^\s=\s*(\S+)\s*;', remainder)
+ if matched_zero and matched_zero.group(1) != '0':
+ return False
+
+ # Don't warn on function pointer declarations. For this we need
+ # to check what came before the "(type)" string.
+ if Match(r'.*\)\s*$', line[0:match.start(0)]):
+ return False
+
+ # Don't warn if the parameter is named with block comments, e.g.:
+ # Function(int /*unused_param*/);
+ raw_line = clean_lines.raw_lines[linenum]
+ if '/*' in raw_line:
+ return False
+
+ # Passed all filters, issue warning here.
+ 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.
@@ -2856,6 +5438,28 @@ def CheckCStyleCast(filename, linenum, line, raw_line, cast_type, pattern,
return True
+def ExpectingFunctionArgs(clean_lines, linenum):
+ """Checks whether where function type arguments are expected.
+
+ Args:
+ clean_lines: A CleansedLines instance containing the file.
+ linenum: The number of the line to check.
+
+ Returns:
+ True if the line at 'linenum' is inside something that expects arguments
+ of function types.
+ """
+ line = clean_lines.elided[linenum]
+ return (Match(r'^\s*MOCK_(CONST_)?METHOD\d+(_T)?\(', line) or
+ (linenum >= 2 and
+ (Match(r'^\s*MOCK_(?:CONST_)?METHOD\d+(?:_T)?\((?:\S+,)?\s*$',
+ clean_lines.elided[linenum - 1]) or
+ Match(r'^\s*MOCK_(?:CONST_)?METHOD\d+(?:_T)?\(\s*$',
+ clean_lines.elided[linenum - 2]) or
+ Search(r'\bstd::m?function\s*\<\s*$',
+ clean_lines.elided[linenum - 1]))))
+
+
_HEADERS_CONTAINING_TEMPLATES = (
('<deque>', ('deque',)),
('<functional>', ('unary_function', 'binary_function',
@@ -2883,6 +5487,7 @@ _HEADERS_CONTAINING_TEMPLATES = (
('<set>', ('set', 'multiset',)),
('<stack>', ('stack',)),
('<string>', ('char_traits', 'basic_string',)),
+ ('<tuple>', ('tuple',)),
('<utility>', ('pair',)),
('<vector>', ('vector',)),
@@ -2969,16 +5574,16 @@ def FilesBelongToSameModule(filename_cc, 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.
+def UpdateIncludeState(filename, include_dict, io=codecs):
+ """Fill up the include_dict 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.
+ include_dict: a dictionary 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.
+ True if a header was successfully added. False otherwise.
"""
headerfile = None
try:
@@ -2992,9 +5597,7 @@ def UpdateIncludeState(filename, include_state, io=codecs):
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))
+ include_dict.setdefault(include, linenum)
return True
@@ -3047,10 +5650,11 @@ def CheckForIncludeWhatYouUse(filename, clean_lines, include_state, error,
# 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()
+ # Let's flatten the include_state include_list and copy it into a dictionary.
+ include_dict = dict([item for sublist in include_state.include_list
+ for item in sublist])
- # Did we find the header for this file (if any) and succesfully load it?
+ # Did we find the header for this file (if any) and successfully load it?
header_found = False
# Use the absolute path so that matching works properly.
@@ -3065,13 +5669,13 @@ def CheckForIncludeWhatYouUse(filename, clean_lines, include_state, error,
# 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
+ # include_dict is modified during iteration, so we iterate over a copy of
# the keys.
- header_keys = include_state.keys()
+ header_keys = include_dict.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):
+ if same_module and UpdateIncludeState(fullpath, include_dict, io):
header_found = True
# If we can't find the header file for a .cc, assume it's because we don't
@@ -3085,7 +5689,7 @@ def CheckForIncludeWhatYouUse(filename, clean_lines, include_state, error,
# 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:
+ if required_header_unstripped.strip('<>"') not in include_dict:
error(filename, required[required_header_unstripped][0],
'build/include_what_you_use', 4,
'Add #include ' + required_header_unstripped + ' for ' + template)
@@ -3097,7 +5701,7 @@ _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
+ G++ 4.6 in C++11 mode fails badly if make_pair's template arguments are
specified explicitly, and such use isn't intended in any case.
Args:
@@ -3106,19 +5710,202 @@ def CheckMakePairUsesDeduction(filename, clean_lines, linenum, error):
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]
+ line = clean_lines.elided[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')
+ 'For C++11-compatibility, omit template arguments from make_pair'
+ ' OR use pair directly OR if appropriate, construct a pair directly')
+
+
+def CheckDefaultLambdaCaptures(filename, clean_lines, linenum, error):
+ """Check that default lambda captures are not used.
+
+ 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]
+
+ # A lambda introducer specifies a default capture if it starts with "[="
+ # or if it starts with "[&" _not_ followed by an identifier.
+ match = Match(r'^(.*)\[\s*(?:=|&[^\w])', line)
+ if match:
+ # Found a potential error, check what comes after the lambda-introducer.
+ # If it's not open parenthesis (for lambda-declarator) or open brace
+ # (for compound-statement), it's not a lambda.
+ line, _, pos = CloseExpression(clean_lines, linenum, len(match.group(1)))
+ if pos >= 0 and Match(r'^\s*[{(]', line[pos:]):
+ error(filename, linenum, 'build/c++11',
+ 4, # 4 = high confidence
+ 'Default lambda captures are an unapproved C++ feature.')
+
+
+def CheckRedundantVirtual(filename, clean_lines, linenum, error):
+ """Check if line contains a redundant "virtual" function-specifier.
+
+ 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.
+ """
+ # Look for "virtual" on current line.
+ line = clean_lines.elided[linenum]
+ virtual = Match(r'^(.*)(\bvirtual\b)(.*)$', line)
+ if not virtual: return
+
+ # Ignore "virtual" keywords that are near access-specifiers. These
+ # are only used in class base-specifier and do not apply to member
+ # functions.
+ if (Search(r'\b(public|protected|private)\s+$', virtual.group(1)) or
+ Match(r'^\s+(public|protected|private)\b', virtual.group(3))):
+ return
+
+ # Ignore the "virtual" keyword from virtual base classes. Usually
+ # there is a column on the same line in these cases (virtual base
+ # classes are rare in google3 because multiple inheritance is rare).
+ if Match(r'^.*[^:]:[^:].*$', line): return
+
+ # Look for the next opening parenthesis. This is the start of the
+ # parameter list (possibly on the next line shortly after virtual).
+ # TODO(unknown): doesn't work if there are virtual functions with
+ # decltype() or other things that use parentheses, but csearch suggests
+ # that this is rare.
+ end_col = -1
+ end_line = -1
+ start_col = len(virtual.group(2))
+ for start_line in xrange(linenum, min(linenum + 3, clean_lines.NumLines())):
+ line = clean_lines.elided[start_line][start_col:]
+ parameter_list = Match(r'^([^(]*)\(', line)
+ if parameter_list:
+ # Match parentheses to find the end of the parameter list
+ (_, end_line, end_col) = CloseExpression(
+ clean_lines, start_line, start_col + len(parameter_list.group(1)))
+ break
+ start_col = 0
+
+ if end_col < 0:
+ return # Couldn't find end of parameter list, give up
+
+ # Look for "override" or "final" after the parameter list
+ # (possibly on the next few lines).
+ for i in xrange(end_line, min(end_line + 3, clean_lines.NumLines())):
+ line = clean_lines.elided[i][end_col:]
+ match = Search(r'\b(override|final)\b', line)
+ if match:
+ error(filename, linenum, 'readability/inheritance', 4,
+ ('"virtual" is redundant since function is '
+ 'already declared as "%s"' % match.group(1)))
+
+ # Set end_col to check whole lines after we are done with the
+ # first line.
+ end_col = 0
+ if Search(r'[^\w]\s*$', line):
+ break
-def ProcessLine(filename, file_extension,
- clean_lines, line, include_state, function_state,
- class_state, error, extra_check_functions=[]):
+def CheckRedundantOverrideOrFinal(filename, clean_lines, linenum, error):
+ """Check if line contains a redundant "override" or "final" virt-specifier.
+
+ 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.
+ """
+ # Look for closing parenthesis nearby. We need one to confirm where
+ # the declarator ends and where the virt-specifier starts to avoid
+ # false positives.
+ line = clean_lines.elided[linenum]
+ declarator_end = line.rfind(')')
+ if declarator_end >= 0:
+ fragment = line[declarator_end:]
+ else:
+ if linenum > 1 and clean_lines.elided[linenum - 1].rfind(')') >= 0:
+ fragment = line
+ else:
+ return
+
+ # Check that at most one of "override" or "final" is present, not both
+ if Search(r'\boverride\b', fragment) and Search(r'\bfinal\b', fragment):
+ error(filename, linenum, 'readability/inheritance', 4,
+ ('"override" is redundant since function is '
+ 'already declared as "final"'))
+
+
+
+
+# Returns true if we are at a new block, and it is directly
+# inside of a namespace.
+def IsBlockInNameSpace(nesting_state, is_forward_declaration):
+ """Checks that the new block is directly in a namespace.
+
+ Args:
+ nesting_state: The _NestingState object that contains info about our state.
+ is_forward_declaration: If the class is a forward declared class.
+ Returns:
+ Whether or not the new block is directly in a namespace.
+ """
+ if is_forward_declaration:
+ if len(nesting_state.stack) >= 1 and (
+ isinstance(nesting_state.stack[-1], _NamespaceInfo)):
+ return True
+ else:
+ return False
+
+ return (len(nesting_state.stack) > 1 and
+ nesting_state.stack[-1].check_namespace_indentation and
+ isinstance(nesting_state.stack[-2], _NamespaceInfo))
+
+
+def ShouldCheckNamespaceIndentation(nesting_state, is_namespace_indent_item,
+ raw_lines_no_comments, linenum):
+ """This method determines if we should apply our namespace indentation check.
+
+ Args:
+ nesting_state: The current nesting state.
+ is_namespace_indent_item: If we just put a new class on the stack, True.
+ If the top of the stack is not a class, or we did not recently
+ add the class, False.
+ raw_lines_no_comments: The lines without the comments.
+ linenum: The current line number we are processing.
+
+ Returns:
+ True if we should apply our namespace indentation check. Currently, it
+ only works for classes and namespaces inside of a namespace.
+ """
+
+ is_forward_declaration = IsForwardClassDeclaration(raw_lines_no_comments,
+ linenum)
+
+ if not (is_namespace_indent_item or is_forward_declaration):
+ return False
+
+ # If we are in a macro, we do not want to check the namespace indentation.
+ if IsMacroDefinition(raw_lines_no_comments, linenum):
+ return False
+
+ return IsBlockInNameSpace(nesting_state, is_forward_declaration)
+
+
+# Call this method if the line is directly inside of a namespace.
+# If the line above is blank (excluding comments) or the start of
+# an inner namespace, it cannot be indented.
+def CheckItemIndentationInNamespace(filename, raw_lines_no_comments, linenum,
+ error):
+ line = raw_lines_no_comments[linenum]
+ if Match(r'^\s+', line):
+ error(filename, linenum, 'runtime/indentation_namespace', 4,
+ 'Do not indent within a namespace')
+
+
+def ProcessLine(filename, file_extension, clean_lines, line,
+ include_state, function_state, nesting_state, error,
+ extra_check_functions=[]):
"""Processes a single line in the file.
Args:
@@ -3129,8 +5916,8 @@ def ProcessLine(filename, file_extension,
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.
+ nesting_state: A NestingState instance which maintains information about
+ the current stack of nested blocks 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
@@ -3139,19 +5926,74 @@ def ProcessLine(filename, file_extension,
"""
raw_lines = clean_lines.raw_lines
ParseNolintSuppressions(filename, raw_lines[line], line, error)
+ nesting_state.Update(filename, clean_lines, line, error)
+ CheckForNamespaceIndentation(filename, nesting_state, clean_lines, line,
+ error)
+ if nesting_state.InAsmBlock(): return
CheckForFunctionLengths(filename, clean_lines, line, function_state, error)
CheckForMultilineCommentsAndStrings(filename, clean_lines, line, error)
- CheckStyle(filename, clean_lines, line, file_extension, class_state, error)
+ CheckStyle(filename, clean_lines, line, file_extension, nesting_state, error)
CheckLanguage(filename, clean_lines, line, file_extension, include_state,
- error)
+ nesting_state, error)
+ CheckForNonConstReference(filename, clean_lines, line, nesting_state, error)
CheckForNonStandardConstructs(filename, clean_lines, line,
- class_state, error)
+ nesting_state, error)
+ CheckVlogArguments(filename, clean_lines, line, error)
CheckPosixThreading(filename, clean_lines, line, error)
CheckInvalidIncrement(filename, clean_lines, line, error)
CheckMakePairUsesDeduction(filename, clean_lines, line, error)
+ CheckDefaultLambdaCaptures(filename, clean_lines, line, error)
+ CheckRedundantVirtual(filename, clean_lines, line, error)
+ CheckRedundantOverrideOrFinal(filename, clean_lines, line, error)
for check_fn in extra_check_functions:
check_fn(filename, clean_lines, line, error)
+def FlagCxx11Features(filename, clean_lines, linenum, error):
+ """Flag those c++11 features that we only allow in certain places.
+
+ 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]
+
+ # Flag unapproved C++11 headers.
+ include = Match(r'\s*#\s*include\s+[<"]([^<"]+)[">]', line)
+ if include and include.group(1) in ('cfenv',
+ 'condition_variable',
+ 'fenv.h',
+ 'future',
+ 'mutex',
+ 'thread',
+ 'chrono',
+ 'ratio',
+ 'regex',
+ 'system_error',
+ ):
+ error(filename, linenum, 'build/c++11', 5,
+ ('<%s> is an unapproved C++11 header.') % include.group(1))
+
+ # The only place where we need to worry about C++11 keywords and library
+ # features in preprocessor directives is in macro definitions.
+ if Match(r'\s*#', line) and not Match(r'\s*#\s*define\b', line): return
+
+ # These are classes and free functions. The classes are always
+ # mentioned as std::*, but we only catch the free functions if
+ # they're not found by ADL. They're alphabetical by header.
+ for top_name in (
+ # type_traits
+ 'alignment_of',
+ 'aligned_union',
+ ):
+ if Search(r'\bstd::%s\b' % top_name, line):
+ error(filename, linenum, 'build/c++11', 5,
+ ('std::%s is an unapproved C++11 class or function. Send c-style '
+ 'an example of where it would make your code more readable, and '
+ 'they may let you use it.') % top_name)
+
+
def ProcessFileData(filename, file_extension, lines, error,
extra_check_functions=[]):
"""Performs lint checks and reports any errors to the given error function.
@@ -3172,31 +6014,113 @@ def ProcessFileData(filename, file_extension, lines, error,
include_state = _IncludeState()
function_state = _FunctionState()
- class_state = _ClassState()
+ nesting_state = NestingState()
ResetNolintSuppressions()
CheckForCopyright(filename, lines, error)
- if file_extension == 'h':
- CheckForHeaderGuard(filename, lines, error)
-
RemoveMultiLineComments(filename, lines, error)
clean_lines = CleansedLines(lines)
+
+ if file_extension == 'h':
+ CheckForHeaderGuard(filename, clean_lines, error)
+
for line in xrange(clean_lines.NumLines()):
ProcessLine(filename, file_extension, clean_lines, line,
- include_state, function_state, class_state, error,
+ include_state, function_state, nesting_state, error,
extra_check_functions)
- class_state.CheckFinished(filename, error)
+ FlagCxx11Features(filename, clean_lines, line, error)
+ nesting_state.CheckCompletedBlocks(filename, error)
CheckForIncludeWhatYouUse(filename, clean_lines, include_state, error)
+
+ # Check that the .cc file has included its header if it exists.
+ if file_extension == 'cc':
+ CheckHeaderFileIncluded(filename, include_state, error)
# We check here rather than inside ProcessLine so that we see raw
# lines rather than "cleaned" lines.
- CheckForUnicodeReplacementCharacters(filename, lines, error)
+ CheckForBadCharacters(filename, lines, error)
CheckForNewlineAtEOF(filename, lines, error)
+def ProcessConfigOverrides(filename):
+ """ Loads the configuration files and processes the config overrides.
+
+ Args:
+ filename: The name of the file being processed by the linter.
+
+ Returns:
+ False if the current |filename| should not be processed further.
+ """
+
+ abs_filename = os.path.abspath(filename)
+ cfg_filters = []
+ keep_looking = True
+ while keep_looking:
+ abs_path, base_name = os.path.split(abs_filename)
+ if not base_name:
+ break # Reached the root directory.
+
+ cfg_file = os.path.join(abs_path, "CPPLINT.cfg")
+ abs_filename = abs_path
+ if not os.path.isfile(cfg_file):
+ continue
+
+ try:
+ with open(cfg_file) as file_handle:
+ for line in file_handle:
+ line, _, _ = line.partition('#') # Remove comments.
+ if not line.strip():
+ continue
+
+ name, _, val = line.partition('=')
+ name = name.strip()
+ val = val.strip()
+ if name == 'set noparent':
+ keep_looking = False
+ elif name == 'filter':
+ cfg_filters.append(val)
+ elif name == 'exclude_files':
+ # When matching exclude_files pattern, use the base_name of
+ # the current file name or the directory name we are processing.
+ # For example, if we are checking for lint errors in /foo/bar/baz.cc
+ # and we found the .cfg file at /foo/CPPLINT.cfg, then the config
+ # file's "exclude_files" filter is meant to be checked against "bar"
+ # and not "baz" nor "bar/baz.cc".
+ if base_name:
+ pattern = re.compile(val)
+ if pattern.match(base_name):
+ sys.stderr.write('Ignoring "%s": file excluded by "%s". '
+ 'File path component "%s" matches '
+ 'pattern "%s"\n' %
+ (filename, cfg_file, base_name, val))
+ return False
+ elif name == 'linelength':
+ global _line_length
+ try:
+ _line_length = int(val)
+ except ValueError:
+ sys.stderr.write('Line length must be numeric.')
+ else:
+ sys.stderr.write(
+ 'Invalid configuration option (%s) in file %s\n' %
+ (name, cfg_file))
+
+ except IOError:
+ sys.stderr.write(
+ "Skipping config file '%s': Can't open for reading\n" % cfg_file)
+ keep_looking = False
+
+ # Apply all the accumulated filters in reverse order (top-level directory
+ # config options having the least priority).
+ for filter in reversed(cfg_filters):
+ _AddFilters(filter)
+
+ return True
+
+
def ProcessFile(filename, vlevel, extra_check_functions=[]):
"""Does google-lint on a single file.
@@ -3212,7 +6136,14 @@ def ProcessFile(filename, vlevel, extra_check_functions=[]):
"""
_SetVerboseLevel(vlevel)
+ _BackupFilters()
+ if not ProcessConfigOverrides(filename):
+ _RestoreFilters()
+ return
+
+ lf_lines = []
+ crlf_lines = []
try:
# Support the UNIX convention of using "-" for stdin. Note that
# we are not opening the file with universal newline support
@@ -3220,10 +6151,7 @@ def ProcessFile(filename, vlevel, extra_check_functions=[]):
# 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.
-
+ # below.
if filename == '-':
lines = codecs.StreamReaderWriter(sys.stdin,
codecs.getreader('utf8'),
@@ -3232,16 +6160,19 @@ def ProcessFile(filename, vlevel, extra_check_functions=[]):
else:
lines = codecs.open(filename, 'r', 'utf8', 'replace').read().split('\n')
- carriage_return_found = False
# Remove trailing '\r'.
- for linenum in range(len(lines)):
+ # The -1 accounts for the extra trailing blank line we get from split()
+ for linenum in range(len(lines) - 1):
if lines[linenum].endswith('\r'):
lines[linenum] = lines[linenum].rstrip('\r')
- carriage_return_found = True
+ crlf_lines.append(linenum + 1)
+ else:
+ lf_lines.append(linenum + 1)
except IOError:
sys.stderr.write(
"Skipping input '%s': Can't open for reading\n" % filename)
+ _RestoreFilters()
return
# Note, if no dot is found, this will give the entire filename as the ext.
@@ -3249,20 +6180,33 @@ def ProcessFile(filename, vlevel, extra_check_functions=[]):
# 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)
+ if filename != '-' and file_extension not in _valid_extensions:
+ sys.stderr.write('Ignoring %s; not a valid file name '
+ '(%s)\n' % (filename, ', '.join(_valid_extensions)))
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')
+
+ # If end-of-line sequences are a mix of LF and CR-LF, issue
+ # warnings on the lines with CR.
+ #
+ # Don't issue any warnings if all lines are uniformly LF or CR-LF,
+ # since critique can handle these just fine, and the style guide
+ # doesn't dictate a particular end of line sequence.
+ #
+ # We can't depend on os.linesep to determine what the desired
+ # end-of-line sequence should be, since that will return the
+ # server-side end-of-line sequence.
+ if lf_lines and crlf_lines:
+ # Warn on every line with CR. An alternative approach might be to
+ # check whether the file is mostly CRLF or just LF, and warn on the
+ # minority, we bias toward LF here since most tools prefer LF.
+ for linenum in crlf_lines:
+ Error(filename, linenum, 'whitespace/newline', 1,
+ 'Unexpected \\r (^M) found; better to use only \\n')
sys.stderr.write('Done processing %s\n' % filename)
+ _RestoreFilters()
def PrintUsage(message):
@@ -3301,7 +6245,10 @@ def ParseArguments(args):
try:
(opts, filenames) = getopt.getopt(args, '', ['help', 'output=', 'verbose=',
'counting=',
- 'filter='])
+ 'filter=',
+ 'root=',
+ 'linelength=',
+ 'extensions='])
except getopt.GetoptError:
PrintUsage('Invalid arguments.')
@@ -3314,8 +6261,8 @@ def ParseArguments(args):
if opt == '--help':
PrintUsage(None)
elif opt == '--output':
- if not val in ('emacs', 'vs7'):
- PrintUsage('The only allowed output formats are emacs and vs7.')
+ if val not in ('emacs', 'vs7', 'eclipse'):
+ PrintUsage('The only allowed output formats are emacs, vs7 and eclipse.')
output_format = val
elif opt == '--verbose':
verbosity = int(val)
@@ -3327,6 +6274,21 @@ def ParseArguments(args):
if val not in ('total', 'toplevel', 'detailed'):
PrintUsage('Valid counting options are total, toplevel, and detailed')
counting_style = val
+ elif opt == '--root':
+ global _root
+ _root = val
+ elif opt == '--linelength':
+ global _line_length
+ try:
+ _line_length = int(val)
+ except ValueError:
+ PrintUsage('Line length must be digits.')
+ elif opt == '--extensions':
+ global _valid_extensions
+ try:
+ _valid_extensions = set(val.split(','))
+ except ValueError:
+ PrintUsage('Extensions must be comma seperated list.')
if not filenames:
PrintUsage('No files were specified.')
diff --git a/Tools/CMakeLists.txt b/Tools/CMakeLists.txt
new file mode 100644
index 0000000..fc7ca65
--- /dev/null
+++ b/Tools/CMakeLists.txt
@@ -0,0 +1,22 @@
+# This file is a part of the OpenSurgSim project.
+# Copyright 2013, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT 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(Boost 1.54 COMPONENTS program_options)
+
+if(BUILD_TOOLS AND Boost_PROGRAM_OPTIONS_FOUND)
+ add_subdirectory(NeedleSutureGeneration)
+else()
+ message("Can't build example NeedleSutureGeneration, missing library boost_program_options.")
+endif(BUILD_TOOLS AND Boost_PROGRAM_OPTIONS_FOUND)
diff --git a/Tools/Converters/tetgen2ply.py b/Tools/Converters/tetgen2ply.py
new file mode 100644
index 0000000..7eb7565
--- /dev/null
+++ b/Tools/Converters/tetgen2ply.py
@@ -0,0 +1,167 @@
+#!/usr/bin/python
+
+# This file is a part of the OpenSurgSim project.
+# Copyright 2012-2015, SimQuest Solutions Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Convert a TetGen set of files .node/.ele into a PLY file readable by OSS
+
+Typical usage:
+ TetGen_to_PLY.py input.node input.ele [input.face] [input.fixedNodes] output.ply
+"""
+
+import csv
+import argparse
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser(
+ description="Convert a TetGen set of filename into a PLY file readable by OSS.")
+ parser.add_argument('nodes', help='Filename for the nodes input.')
+ parser.add_argument('elements', help='Filename for the tetrahedrons input.')
+ parser.add_argument('--faces', help='Filename for the faces input.')
+ parser.add_argument('--fixedNodes', help='Filename for the fixed node indices.')
+ parser.add_argument('--massDensity', help='Mass density.', default='2000.0')
+ parser.add_argument('--youngModulus', help='Young modulus.', default='1e7')
+ parser.add_argument('--poissonRatio', help='Poisson ratio.', default='0.45')
+ parser.add_argument('output', help='Filename for the PLY output.')
+ args = parser.parse_args()
+
+ numNodes = 0 # Number of nodes, will be read from the header of the .node file
+ numElements = 0 # Number of elements, will be read from the header of the .ele file (support triangle (3) and tetrahedron(4) elements)
+ elementSize = 0 # Element size 3 for triangle, 4 for tetrahedron, will be read from the header of the .ele file
+ numFaces = 0 # Number of triangulated faces (if a .face is provided, number of entries in this file)
+ numFixedNodes = 0 # Number of fixed nodes (if a .fixedNode is provided, number of entries in this file)
+
+ with open(args.output, 'wb') as csvOutputFile:
+ writer = csv.writer(csvOutputFile, delimiter = ' ', quoting=csv.QUOTE_NONE)
+
+ writer.writerow(['ply'])
+ writer.writerow(['format', 'ascii', '1.0'])
+ writer.writerow(['comment', 'Created', 'by', 'tetgen2ply'])
+
+ with open(args.nodes, 'rb') as csvNodeFile:
+ reader = csv.reader(csvNodeFile, delimiter = ' ', skipinitialspace = True)
+ row = reader.next()
+ numNodes = row[0]
+ # Enforcing the need for 3 pieces of information per node (x, y, z)
+ if not row[1] == '3':
+ raise Exception('Invalid node information in ' + args.node + '. Node dimension (expecting 3) was ' + row[1])
+ writer.writerow(['element', 'vertex', numNodes])
+ writer.writerow(['property', 'double', 'x'])
+ writer.writerow(['property', 'double', 'y'])
+ writer.writerow(['property', 'double', 'z'])
+
+ with open(args.elements, 'rb') as csvElementFile:
+ reader = csv.reader(csvElementFile, delimiter = ' ', skipinitialspace = True)
+ row = reader.next()
+ numElements = row[0]
+ if row[1] == '4':
+ writer.writerow(['element', '3d_element', numElements])
+ elementSize = 4
+ elif row[1] == '3':
+ writer.writerow(['element', '2d_element', numElements])
+ elementSize = 3
+ else :
+ raise Exception('Invalid triangle/tetrahedron information in ' + args.ele + '. Element dimension (expecting 3 or 4) was ' + row[1])
+ writer.writerow(['property', 'list', 'uint', 'uint', 'vertex_indices'])
+
+ if args.faces:
+ with open(args.faces, 'rb') as csvFaceFile:
+ reader = csv.reader(csvFaceFile, delimiter = ' ', skipinitialspace = True)
+ row = reader.next()
+ numFaces = row[0]
+ writer.writerow(['element', 'face', numFaces])
+ writer.writerow(['property', 'list', 'uint', 'uint', 'vertex_indices'])
+
+ if args.fixedNodes:
+ with open(args.fixedNodes, 'rb') as csvFixedNodeFile:
+ readerFixedNodes = csv.reader(csvFixedNodeFile)
+ for row in readerFixedNodes:
+ numFixedNodes = numFixedNodes + 1
+ writer.writerow(['element', 'boundary_condition', numFixedNodes])
+ writer.writerow(['property', 'uint', 'vertex_index'])
+
+ # Extra parameter (thickness) if the element is a triangle
+ if elementSize == 3:
+ writer.writerow(['element', 'thickness', 1])
+ writer.writerow(['property', 'double', 'value'])
+
+ writer.writerow(['element', 'material', 1])
+ writer.writerow(['property', 'double', 'mass_density'])
+ writer.writerow(['property', 'double', 'poisson_ratio'])
+ writer.writerow(['property', 'double', 'young_modulus'])
+ writer.writerow(['end_header'])
+
+ # Parse the .node file to format the nodes (x,y,z)
+ with open(args.nodes, 'rb') as csvNodeFile:
+ reader = csv.reader(csvNodeFile, delimiter = ' ', skipinitialspace = True)
+ rowId = 0
+ # Write all nodes
+ for row in reader:
+ # Skip the commented lines (especially the last line of the .node generated by TetGen)
+ if row[0][0] == '#':
+ continue
+ # Skip the first line (header information), detected by the number of nodes being different than the expected node index
+ if not int(row[0]) == rowId:
+ continue
+ writer.writerow(row[1:])
+ rowId = rowId + 1
+
+ # Parse the .ele file to format the tetrahedrons
+ with open(args.elements, 'rb') as csvElementFile:
+ reader = csv.reader(csvElementFile, delimiter = ' ', skipinitialspace = True)
+ rowId = 0
+ # Write all tetrahedrons
+ for row in reader:
+ # Skip the commented lines (especially the last line of the .ele generated by TetGen)
+ if row[0][0] == '#':
+ continue
+ # Skip the first line (header information), detected by the number of elements being different than the expected element index
+ if not int(row[0]) == rowId:
+ continue
+ row[0] = elementSize
+ writer.writerow(row)
+ rowId = rowId + 1
+
+ # Parse the .face file to format the triangulated faces (if a file is specified)
+ if args.faces:
+ with open(args.faces, 'rb') as csvFaceFile:
+ reader = csv.reader(csvFaceFile, delimiter = ' ', skipinitialspace = True)
+ rowId = 0
+ # Write all faces
+ for row in reader:
+ # Skip the commented lines (especially the last line of the .face generated by TetGen)
+ if row[0][0] == '#':
+ continue
+ # Skip the first line (header information), detected by the number of faces being different than the expected face index
+ if not int(row[0]) == rowId:
+ continue
+ row[0] = 3
+ writer.writerow(row[:4])
+ rowId = rowId + 1
+
+ # Write the fixed nodes (boundary conditions) if any
+ if args.fixedNodes:
+ with open(args.fixedNodes, 'rb') as csvFixedNodeFile:
+ readerFixedNodes = csv.reader(csvFixedNodeFile, delimiter = ' ', skipinitialspace = True)
+ for row in readerFixedNodes:
+ writer.writerow(row)
+
+ # Write a default thickness if the element is a triangle
+ # We should have something closer to 1/100th of the mesh size if we wanted to have a more automated tool
+ if elementSize == 3:
+ writer.writerow(['0.01'])
+
+ # Write the material (default parameters)
+ writer.writerow([args.massDensity, args.poissonRatio, args.youngModulus])
diff --git a/Tools/NeedleSutureGeneration/CMakeLists.txt b/Tools/NeedleSutureGeneration/CMakeLists.txt
new file mode 100644
index 0000000..bc99725
--- /dev/null
+++ b/Tools/NeedleSutureGeneration/CMakeLists.txt
@@ -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.
+
+link_directories(
+ ${Boost_LIBRARY_DIRS}
+)
+
+include_directories(
+ "${CMAKE_CURRENT_SOURCE_DIR}"
+)
+
+set(SOURCES
+ NeedleSutureGeneration.cpp
+)
+
+set(HEADERS
+)
+
+surgsim_add_executable(NeedleSutureGeneration "${SOURCES}" "${HEADERS}")
+
+SET(LIBS
+ SurgSimBlocks
+ SurgSimGraphics
+ ${Boost_LIBRARIES}
+ ${YAML_CPP_LIBRARIES}
+)
+
+target_link_libraries(NeedleSutureGeneration ${LIBS})
+
+# Put NeedleSutureGeneration into folder "Tools"
+set_target_properties(NeedleSutureGeneration PROPERTIES FOLDER "Tools")
diff --git a/Tools/NeedleSutureGeneration/Data/properties.ini b/Tools/NeedleSutureGeneration/Data/properties.ini
new file mode 100644
index 0000000..a23536a
--- /dev/null
+++ b/Tools/NeedleSutureGeneration/Data/properties.ini
@@ -0,0 +1,9 @@
+[PhysicsProperty]
+#Mass density has unit Kg.m-3
+#Young Modulus has unit Pa
+needleMassDensity = 7500
+needlePoissonRatio = 0.305
+needleYoungModulus = 1.8e11
+sutureMassDensity = 900
+suturePoissonRatio = 0.45
+sutureYoungModulus = 1.75e9
diff --git a/Tools/NeedleSutureGeneration/NeedleSutureGeneration.cpp b/Tools/NeedleSutureGeneration/NeedleSutureGeneration.cpp
new file mode 100644
index 0000000..4aa67ec
--- /dev/null
+++ b/Tools/NeedleSutureGeneration/NeedleSutureGeneration.cpp
@@ -0,0 +1,292 @@
+// This file is a part of the OpenSurgSim project.
+// Copyright 2016, SimQuest Solutions Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <boost/filesystem.hpp>
+#include <boost/program_options.hpp>
+#include <boost/property_tree/ini_parser.hpp>
+#include <boost/property_tree/ptree.hpp>
+#include <string>
+#include <vector>
+#include <yaml-cpp/yaml.h>
+
+#include "SurgSim/DataStructures/Vertices.h"
+#include "SurgSim/Framework/Log.h"
+#include "SurgSim/Math/CardinalSplines.h"
+#include "SurgSim/Math/Vector.h"
+
+/// Function to generate points on a circle with radius 'radius'.
+/// \note The points always start from the origin and go on the circle to 'angle'.
+/// \param subdivisions Number of interpolated points on the circle.
+/// \param radius Radius of the circle on which points are generated.
+/// \param angle How much (of the circle) should points occupy, in radian.
+/// \return A list of points on the circle.
+std::vector<SurgSim::Math::Vector3d> generateNeedle(size_t subdivisions, double radius, double angle)
+{
+ SURGSIM_ASSERT(0 < angle && angle < 2.0 * M_PI) << __FUNCTION__ << " 'angle' must be in the range (0, 2PI)";
+ SURGSIM_ASSERT(2 <= subdivisions) << __FUNCTION__ << " 'subdivisions' must be at least 2";
+
+ double increment = angle / static_cast<double>(subdivisions);
+ double controlAngle = 0.0;
+
+ std::vector<SurgSim::Math::Vector3d> vertices(subdivisions+1);
+ for (size_t i = 0; i < subdivisions; ++i)
+ {
+ vertices[i] = radius * SurgSim::Math::Vector3d(std::cos(controlAngle) - 1.0, std::sin(controlAngle), 0.0);
+ controlAngle += increment;
+ }
+ vertices[subdivisions] =
+ radius * SurgSim::Math::Vector3d(std::cos(controlAngle) - 1.0, std::sin(controlAngle), 0.0);
+
+ return vertices;
+}
+
+/// Function to generate a suture, starting from the origin and wraps around 'axis' in circles.
+/// A circle (formed by the suture when wraps 'axis') is defined by four markers.
+/// Each marker is rotated 45 degrees along 'axis' from previous one.
+/// 'subdivisions' controls how many points to interpolate between each pair of markers.
+/// In the returned list, the origin will always be the first point.
+/// \param subdivisions Number of points between makers.
+/// \param radius Radius of the circle the suture forms.
+/// \param length Length of the suture.
+/// \param circles Number of circles the suture forms wrapping around 'axis'.
+/// \param axis The axis around which the suture wraps.
+/// \return A list of points representing the suture.
+std::vector<SurgSim::Math::Vector3d> generateSuture(size_t subdivisions, double radius, double length, size_t circles,
+ const SurgSim::Math::Vector3d& axis = SurgSim::Math::Vector3d(1.0, 0.0, 0.0))
+{
+ // Increment on X-axis.
+ double increment = length / circles / 4.0;
+
+ SurgSim::Math::Vector3d direction = axis;
+ direction.normalize();
+ auto rotateXToAxis = SurgSim::Math::makeRotationQuaternion(
+ -std::acos(direction.dot(SurgSim::Math::Vector3d(1.0, 0.0, 0.0))),
+ direction.cross(SurgSim::Math::Vector3d(1.0, 0.0, 0.0)));
+
+ std::vector<SurgSim::Math::Vector3d> controlPoints;
+ controlPoints.push_back(SurgSim::Math::Vector3d(0.0, 0.0, 0.0) - increment * direction);
+ controlPoints.push_back(SurgSim::Math::Vector3d(0.0, 0.0, 0.0));
+
+ // Used to control the rotation between markers.
+ SurgSim::Math::Quaterniond rotation =
+ SurgSim::Math::makeRotationQuaternion(M_PI_2, SurgSim::Math::Vector3d(1.0, 0.0, 0.0));
+ auto point = SurgSim::Math::Vector3d(increment, radius, 0.0);
+ for (size_t i = 0; i < circles * 4; ++i)
+ {
+ controlPoints.push_back(rotateXToAxis * point);
+ point = rotation * point;
+ point.x() += increment;
+ }
+
+ std::vector<SurgSim::Math::Vector3d> result;
+ SurgSim::Math::CardinalSplines::interpolate(subdivisions, controlPoints, &result);
+
+ return result;
+}
+
+/// Save a list of needle and a list of suture as one ply file.
+/// Points in 'needle' will be output first, as the order they appear in 'needle' and
+/// points in 'suture' will be output second, as the order they appear in 'suture'.
+/// The caller of this function needs to make sure
+/// the needle and the suture connects (smoothly) at needle[last] and suture[begin].
+/// \param fileName Name of the ply file.
+/// \param needle List of points representing the needle.
+/// \param suture List of points representing the suture.
+/// \param asPhysics Boolean to decide whether or not to save physics properties.
+/// \param needleMassDensity Mass density of needle.
+/// \param needlepoissonRatio Poisson ratio of needle.
+/// \param needleYoungModulus Young modulus of needle.
+/// \param sutureMassDensity Mass density of suture.
+/// \param suturepoissonRatio Poisson ratio of suture.
+/// \param sutureYoungModulus Young modulus of suture.
+/// \param radius Radius used for fem1d beam.
+void saveNeedleSuturePly(const std::string& fileName,
+ const std::vector<SurgSim::Math::Vector3d>& needle,
+ const std::vector<SurgSim::Math::Vector3d>& suture,
+ bool asPhysics,
+ double needleMassDensity, double needlePoissonRatio, double needleYoungModulus,
+ double sutureMassDensity, double suturePoissonRatio, double sutureYoungModulus,
+ double radius = 0.0001)
+{
+ std::ofstream out(fileName);
+
+ size_t numOfPoints = needle.size() + suture.size();
+ if (out.is_open())
+ {
+ out << "ply" << std::endl;
+ out << "format ascii 1.0" << std::endl;
+ out << "comment Created by OpenSurgSim, www.opensurgsim.org" << std::endl;
+ out << "element vertex " << numOfPoints << std::endl;
+ out << "property float x\nproperty float y\nproperty float z" << std::endl;
+ if (asPhysics)
+ {
+ out << "element 1d_element " << numOfPoints - 1 << std::endl;
+ out << "property list uint uint vertex_indices" << std::endl;
+ out << "property double mass_density" << std::endl;
+ out << "property double poisson_ratio" << std::endl;
+ out << "property double young_modulus" << std::endl;
+ }
+ out << "element radius 1" << std::endl;
+ out << "property double value" << std::endl;
+ out << "element boundary_condition 0" << std::endl;
+ out << "property uint vertex_index" << std::endl;
+ out << "end_header" << std::endl;
+ for (const auto& vertex : needle)
+ {
+ out << vertex[0] << " " << vertex[1] << " " << vertex[2] << std::endl;
+ }
+ for (const auto& vertex : suture)
+ {
+ out << vertex[0] << " " << vertex[1] << " " << vertex[2] << std::endl;
+ }
+
+ if (asPhysics)
+ {
+ int index = 0;
+ for (const auto& vertex : needle)
+ {
+ out << "2 " << index << " " << index + 1 << " " <<
+ needleMassDensity << " " << needlePoissonRatio << " " << needleYoungModulus << std::endl;
+ ++index;
+ }
+ for (const auto& vertex : suture)
+ {
+ out << "2 " << index << " " << index + 1 << " " <<
+ sutureMassDensity << " " << suturePoissonRatio << " " << sutureYoungModulus << std::endl;
+ ++index;
+ }
+ }
+
+ out << radius << std::endl;
+
+ if (out.bad())
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) << __FUNCTION__
+ << "There was a problem writing " << fileName;
+ }
+
+ out.close();
+ }
+ else
+ {
+ SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) << __FUNCTION__
+ << "Could not open " << fileName << " for writing.";
+ }
+};
+
+// Utility to generate a ply file for needle suture.
+// Command line input are supported.
+int main(int argc, char* argv[])
+{
+ namespace po = boost::program_options;
+
+ po::options_description commandLine("Allowed options");
+ commandLine.add_options()("help", "produce help message")
+ ("filename", po::value<std::string>()->default_value("needlesuture.ply"),
+ "File name to save the generated needle suture.")
+ ("needleSubdivisions", po::value<int>()->default_value(10),
+ "Number of interpolated points for the needle (default 10)")
+ ("needleRadius", po::value<double>()->default_value(0.019), "Radius of the needle (in m) (default 0.019)")
+ ("needleAngle", po::value<double>()->default_value(135),
+ "How big is the arc (common values: 90, 135, 180, 225) in degrees (default 135)")
+ ("sutureSubdivisions", po::value<int>()->default_value(20),
+ "Number of interpolated points for each pair of markers on the suture (default 20)")
+ ("sutureRadius", po::value<double>()->default_value(0.01), "Radius of the suture (default 0.01")
+ ("sutureLength", po::value<double>()->default_value(0.45), "Length of th suture (in m) (default 0.45)")
+ ("sutureCircles", po::value<int>()->default_value(2), "Number of circles formed by suture going around the axis")
+ ("savePhysicsProperty", po::value<bool>()->default_value(true),
+ "Output physics properties for the needle and suture in ply file")
+ ("physicsProperty", po::value<std::string>(),
+ "File name for physics properties. Note that command line values have priority: "
+ "program will use value(s) from command line if specified.")
+ ("needleMassDensity", po::value<double>(), "Mass density for the needle (default 7500 Kg.m-3)")
+ ("needlePoissonRatio", po::value<double>(), "Poisson ratio for the needle (default 0.305)")
+ ("needleYoungModulus", po::value<double>(), "Young Modulus for the needle (default (1.8e11 Pa)")
+ ("sutureMassDensity", po::value<double>(), "Mass density for the suture (default 900 Kg.m-3)")
+ ("suturePoissonRatio", po::value<double>(), "Poisson ratio for the suture (default 0.45)")
+ ("sutureYoungModulus", po::value<double>(), "Young Modulus for the suture (default 1.75e9 Pa)");
+
+ po::variables_map variables;
+ try
+ {
+ po::store(po::parse_command_line(argc, argv, commandLine), variables);
+ }
+ catch (po::error& e)
+ {
+ std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
+ std::cerr << commandLine << std::endl;
+ return 1;
+ }
+
+ if (variables.count("help"))
+ {
+ std::cout << commandLine << "\n";
+ return 1;
+ }
+
+ double needleMassDensity = 7500, needlePoissonRatio = 0.305, needleYoungModulus = 180 * 1e9;
+ double sutureMassDensity = 900, suturePoissonRatio = 0.45, sutureYoungModulus = 1.75 * 1e9;
+ if (variables.count("physicsProperty"))
+ {
+ boost::filesystem::path fileName("Data/" + variables["physicsProperty"].as<std::string>());
+ boost::filesystem::path filePath = boost::filesystem::complete(fileName);
+ std::cout << "File path: " << filePath << std::endl;
+ if (!boost::filesystem::exists(filePath))
+ {
+ std::cout << "Can't find my file!" << std::endl;
+ return -1;
+ }
+
+ boost::property_tree::ptree parameters;
+ boost::property_tree::ini_parser::read_ini(filePath.string(), parameters);
+ needleMassDensity = parameters.get<double>("PhysicsProperty.needleMassDensity");
+ needlePoissonRatio = parameters.get<double>("PhysicsProperty.needlePoissonRatio");
+ needleYoungModulus = parameters.get<double>("PhysicsProperty.needleYoungModulus");
+ sutureMassDensity = parameters.get<double>("PhysicsProperty.sutureMassDensity");
+ suturePoissonRatio = parameters.get<double>("PhysicsProperty.suturePoissonRatio");
+ sutureYoungModulus = parameters.get<double>("PhysicsProperty.sutureYoungModulus");
+ }
+
+ // Command line values for physics properties will take precedence if specified.
+ if (variables.count("needleMassDensity")) needleMassDensity = variables["needleMassDensity"].as<double>();
+ if (variables.count("needlePoissonRatio")) needlePoissonRatio = variables["needlePoissonRatio"].as<double>();
+ if (variables.count("needleYoungModulus")) needleYoungModulus = variables["needleYoungModulus"].as<double>();
+ if (variables.count("sutureMassDensity")) sutureMassDensity = variables["sutureMassDensity"].as<double>();
+ if (variables.count("suturePoissonRatio")) suturePoissonRatio = variables["suturePoissonRatio"].as<double>();
+ if (variables.count("sutureYoungModulus")) sutureYoungModulus = variables["sutureYoungModulus"].as<double>();
+
+ auto needlePoints = generateNeedle(variables["needleSubdivisions"].as<int>(),
+ variables["needleRadius"].as<double>(),
+ variables["needleAngle"].as<double>() / 180 * M_PI);
+
+ auto suturePoints = generateSuture(variables["sutureSubdivisions"].as<int>(),
+ variables["sutureRadius"].as<double>(),
+ variables["sutureLength"].as<double>(),
+ variables["sutureCircles"].as<int>(),
+ needlePoints[0] - needlePoints[1]);
+
+ // Points generated are starting from the origin, when constructing the needle suture,
+ // the list of needle points need to be reversed and the origin is removed to avoid duplication.
+ std::vector<SurgSim::Math::Vector3d> needlePointsReversed(needlePoints.rbegin(), needlePoints.rend()-1);
+
+ saveNeedleSuturePly(variables["filename"].as<std::string>(), needlePointsReversed, suturePoints,
+ variables["savePhysicsProperty"].as<bool>(),
+ needleMassDensity, needlePoissonRatio, needleYoungModulus,
+ sutureMassDensity, suturePoissonRatio, sutureYoungModulus);
+ return 0;
+}
diff --git a/Tools/run-lint.py b/Tools/run-lint.py
index 5d2dcc1..83582ce 100644
--- a/Tools/run-lint.py
+++ b/Tools/run-lint.py
@@ -411,7 +411,7 @@ if __name__ == '__main__':
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),
+ re.search(r'^(?:\.git|build.*|ThirdParty|Modules)$', x, re.IGNORECASE),
subdirs):
subdirs.remove(bad)
--
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